32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
465 lines
16 KiB
C
465 lines
16 KiB
C
/**
|
|
* \file Ifx_AngleTrkF32.c
|
|
* \brief Angle-tracking observer for sin/cos analog position sensor
|
|
*
|
|
*
|
|
* \version disabled
|
|
* \copyright Copyright (c) 2019 Infineon Technologies AG. All rights reserved.
|
|
*
|
|
*
|
|
* IMPORTANT NOTICE
|
|
*
|
|
*
|
|
* Use of this file is subject to the terms of use agreed between (i) you or
|
|
* the company in which ordinary course of business you are acting and (ii)
|
|
* Infineon Technologies AG or its licensees. If and as long as no such
|
|
* terms of use are agreed, use of this file is subject to following:
|
|
|
|
|
|
* Boost Software License - Version 1.0 - August 17th, 2003
|
|
|
|
* Permission is hereby granted, free of charge, to any person or
|
|
* organization obtaining a copy of the software and accompanying
|
|
* documentation covered by this license (the "Software") to use, reproduce,
|
|
* display, distribute, execute, and transmit the Software, and to prepare
|
|
* derivative works of the Software, and to permit third-parties to whom the
|
|
* Software is furnished to do so, all subject to the following:
|
|
|
|
* The copyright notices in the Software and this entire statement, including
|
|
* the above license grant, this restriction and the following disclaimer, must
|
|
* be included in all copies of the Software, in whole or in part, and all
|
|
* derivative works of the Software, unless such copies or derivative works are
|
|
* solely in the form of machine-executable object code generated by a source
|
|
* language processor.
|
|
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
|
|
* SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
|
|
* FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
|
|
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
|
|
* DEALINGS IN THE SOFTWARE.
|
|
|
|
*
|
|
*/
|
|
|
|
//________________________________________________________________________________________
|
|
// INCLUDES
|
|
|
|
#include "Ifx_AngleTrkF32.h"
|
|
#include <math.h>
|
|
#include "string.h"
|
|
#include "Ifx_LutAtan2F32.h"
|
|
|
|
//________________________________________________________________________________________
|
|
// LOCAL DEFINITIONS
|
|
|
|
#define IFX_ANGLETRKF32_ATAN2F Ifx_LutAtan2F32_float32
|
|
|
|
//________________________________________________________________________________________
|
|
// LOCAL FUNCTION PROTOTYPES
|
|
|
|
void Ifx_AngleTrkF32_setUserSampling(Ifx_AngleTrkF32 *aObsv, float32 Ts);
|
|
|
|
//________________________________________________________________________________________
|
|
// GLOBAL VARIABLES
|
|
|
|
//________________________________________________________________________________________
|
|
// LOCAL VARIABLES
|
|
|
|
//________________________________________________________________________________________
|
|
// LOCAL FUNCTION IMPLEMENTATIONS
|
|
|
|
IFX_INLINE float32 Ifx_AngleTrkF32_bound(float32 angle, float32 periodPerRotation)
|
|
{
|
|
IFX_UNUSED_PARAMETER(periodPerRotation);
|
|
float32 fullPeriod = 2 * IFX_PI;
|
|
|
|
if (angle >= (fullPeriod / 2))
|
|
{
|
|
angle = angle - fullPeriod;
|
|
}
|
|
else if (angle < (-fullPeriod / 2))
|
|
{
|
|
angle = angle + fullPeriod;
|
|
}
|
|
else
|
|
{}
|
|
|
|
return angle;
|
|
}
|
|
|
|
|
|
IFX_INLINE float32 Ifx_AngleTrkF32_boundInput(float32 input)
|
|
{
|
|
return Ifx_AngleTrkF32_bound(input, 1);
|
|
}
|
|
|
|
|
|
float32 round_f(float32 val)
|
|
{
|
|
/*lint -e632 -e633 -e524*/
|
|
sint32 val_i = (sint32)val;
|
|
|
|
/*lint +e632 +e633 +e524*/
|
|
if ((val - (float32)val_i) > 0.5)
|
|
{
|
|
val = (float32)(val_i + 1);
|
|
}
|
|
else
|
|
{
|
|
val = (float32)val_i;
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
|
|
//________________________________________________________________________________________
|
|
// PUBLIC FUNCTION IMPLEMENTATIONS
|
|
#define ATO_T (15e-3f)
|
|
#define ATO_K (39.05f)
|
|
#define ATO_PSI (1.5f * IFX_PI)
|
|
|
|
void Ifx_AngleTrkF32_setControlGains(Ifx_AngleTrkF32_CfgData *cfgData, float32 K, float32 T, float32 psi)
|
|
{
|
|
cfgData->kp = ((2 * K) + (psi * psi) + 1) / (T * T);
|
|
cfgData->ki = K * ((psi * psi) + 1) / (T * T * T);
|
|
cfgData->kd = (K + 2) / T;
|
|
}
|
|
|
|
|
|
/** \brief Fill config with default values */
|
|
void Ifx_AngleTrkF32_initConfig(Ifx_AngleTrkF32_Config *config, sint16 *sinIn, sint16 *cosIn)
|
|
{
|
|
Ifx_AngleTrkF32_CfgData cfgData;
|
|
Ifx_AngleTrkF32_setControlGains(&cfgData, ATO_K, ATO_T, ATO_PSI);
|
|
config->kp = cfgData.kp;
|
|
config->ki = cfgData.ki;
|
|
config->kd = cfgData.kd;
|
|
config->sinIn = sinIn;
|
|
config->cosIn = cosIn;
|
|
config->errorThreshold = 5.0f / 180 * IFX_PI;
|
|
config->sqrAmplMax = (sint32)((1.01f * 1.01f) * 2048);
|
|
config->sqrAmplMin = (sint32)((0.99f * 0.99f) * 2048);
|
|
config->speedLpfFc = 100;
|
|
config->periodPerRotation = 1;
|
|
config->reversed = FALSE;
|
|
config->offset = 0;
|
|
config->resolution = (1UL << 12);/** \brief 12-bit default resolution */
|
|
}
|
|
|
|
|
|
void Ifx_AngleTrkF32_setUserSampling(Ifx_AngleTrkF32 *aObsv, float32 Ts)
|
|
{
|
|
aObsv->base.Ts = Ts;
|
|
aObsv->halfTs = aObsv->base.Ts / 2.0F;
|
|
}
|
|
|
|
|
|
/** \brief Initialize the Angle Tracking object
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \param config Pointer to the configuration data
|
|
* \param Ts sampling period in seconds
|
|
*/
|
|
void Ifx_AngleTrkF32_init(Ifx_AngleTrkF32 *aObsv, const Ifx_AngleTrkF32_Config *config, float32 Ts)
|
|
{
|
|
aObsv->base.offset = config->offset;
|
|
aObsv->base.resolution = config->resolution;
|
|
aObsv->base.reversed = config->reversed;
|
|
aObsv->base.Ts = Ts;
|
|
aObsv->base.rawPosition = 0;
|
|
aObsv->base.direction = IfxStdIf_Pos_Dir_unknown;
|
|
aObsv->base.turn = 0;
|
|
aObsv->base.status.status = 0;
|
|
aObsv->base.periodPerRotation = config->periodPerRotation;
|
|
aObsv->base.positionConst = 1.0 / (float32)aObsv->base.resolution * 2.0 * IFX_PI;
|
|
|
|
aObsv->cfgData.kd = config->kd;
|
|
aObsv->cfgData.ki = config->ki;
|
|
aObsv->cfgData.kp = config->kp;
|
|
aObsv->cfgData.errorThreshold = config->errorThreshold;
|
|
aObsv->cfgData.sqrAmplMax = config->sqrAmplMax;
|
|
aObsv->cfgData.sqrAmplMin = config->sqrAmplMin;
|
|
aObsv->sinIn = config->sinIn;
|
|
aObsv->cosIn = config->cosIn;
|
|
|
|
if (!__neqf(config->kp, 0) && !__neqf(config->ki, 0) && !__neqf(config->kp, 0))
|
|
{ /* all gains are zero, use default */
|
|
/* FIXME might not be requied as set by Ifx_AngleTrkF32_initConfig() */
|
|
Ifx_AngleTrkF32_setControlGains(&aObsv->cfgData, ATO_K, ATO_T, ATO_PSI);
|
|
}
|
|
|
|
Ifx_AngleTrkF32_setUserSampling(aObsv, Ts);
|
|
aObsv->angleErr = 0.0F;
|
|
aObsv->angleEst = 0.0F;
|
|
aObsv->accelEst = 0.0F;
|
|
aObsv->speedEstA = 0.0F;
|
|
aObsv->speedEstB = 0.0F;
|
|
aObsv->angleRef = 0.0F;
|
|
{
|
|
#if IFX_CFG_ANGLETRKF32_SPEED_FILTER
|
|
Ifx_LowPassPt1F32_Config lpfConfig;
|
|
lpfConfig.gain = 1.0F;
|
|
lpfConfig.cutOffFrequency = (2 * IFX_PI * config->speedLpfFc);
|
|
lpfConfig.samplingTime = Ts;
|
|
Ifx_LowPassPt1F32_init(&aObsv->speedLpf, &lpfConfig);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
/** \brief Step function of Ifx_AngleTrkF32 should be called at every sampling period provided
|
|
* during initialisation (call to Ifx_AngleTrkF32_init()).
|
|
*
|
|
* \note This function is automatically called by Ifx_AngleTrkF32_update()
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \param sinIn sine input signal. The offset shall be zero.
|
|
* \param cosIn cosine input signal. The offset shall be zero.
|
|
* \param phase phase of input signal.
|
|
* \note This function returns the angular position in radians
|
|
* \note The amplitude of both sinIn and cosIn signal shall be the same.
|
|
*/
|
|
float32 Ifx_AngleTrkF32_step(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn, float32 phase)
|
|
{
|
|
float32 angleRef, angleEst, dAccel, dSpeed, dAngle;
|
|
|
|
if (aObsv->base.reversed != FALSE)
|
|
{
|
|
angleRef = IFX_ANGLETRKF32_ATAN2F((float32)cosIn, (float32)sinIn);
|
|
}
|
|
else
|
|
{
|
|
angleRef = IFX_ANGLETRKF32_ATAN2F((float32)sinIn, (float32)cosIn);
|
|
}
|
|
|
|
aObsv->angleAtan = angleRef;
|
|
angleRef = angleRef + phase;
|
|
|
|
aObsv->angleRef = angleRef;
|
|
|
|
// Acceleration, zero-order-hold integrator:
|
|
dAccel = aObsv->cfgData.ki * aObsv->angleErr;
|
|
aObsv->accelEst = aObsv->accelEst + (dAccel * aObsv->base.Ts);
|
|
|
|
// Speed, zero-order-hold integrator:
|
|
dSpeed = ((aObsv->cfgData.kp * aObsv->angleErr) + aObsv->accelEst);
|
|
aObsv->speedEstA = aObsv->speedEstA + (dSpeed * aObsv->base.Ts);
|
|
|
|
// Angle, first-order-hold integrator:
|
|
dAngle = ((aObsv->cfgData.kd * aObsv->angleErr) + aObsv->speedEstA);
|
|
angleEst = aObsv->angleEst + ((dAngle + aObsv->speedEstB) * aObsv->halfTs);
|
|
aObsv->speedEstB = dAngle;
|
|
|
|
aObsv->angleEst = Ifx_AngleTrkF32_boundInput(angleEst);
|
|
|
|
// Error, is calculated here to compensate one period delay
|
|
aObsv->angleErr = Ifx_AngleTrkF32_boundInput(aObsv->angleRef - aObsv->angleEst);
|
|
|
|
aObsv->base.direction = aObsv->speedEstB > 0 ? IfxStdIf_Pos_Dir_forward : IfxStdIf_Pos_Dir_backward;
|
|
|
|
#if IFX_CFG_ANGLETRKF32_SPEED_FILTER
|
|
// Filter speed:
|
|
Ifx_LowPassPt1F32_do(&aObsv->speedLpf, aObsv->speedEstB);
|
|
#endif
|
|
|
|
return aObsv->angleEst;
|
|
}
|
|
|
|
|
|
/** \brief Set the position offset (in ticks)
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \param offset Position offset in ticks
|
|
*/
|
|
void Ifx_AngleTrkF32_setOffset(Ifx_AngleTrkF32 *aObsv, IfxStdIf_Pos_RawAngle offset)
|
|
{
|
|
Ifx_AngleTrkF32_PosIf *base = &aObsv->base;
|
|
base->offset = offset;
|
|
}
|
|
|
|
|
|
/** \brief Update the status flags.
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \param sinIn Sinus value
|
|
* \param cosIn Cosinus value
|
|
* \note This function is automatically called by Ifx_AngleTrkF32_update()
|
|
*/
|
|
void Ifx_AngleTrkF32_updateStatus(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn)
|
|
{
|
|
Ifx_AngleTrkF32_PosIf *base = &aObsv->base;
|
|
sint32 sqrAmpl = ((sint32)sinIn * (sint32)sinIn) + ((sint32)cosIn * (sint32)cosIn);
|
|
base->status.B.signalLoss = (sqrAmpl < aObsv->cfgData.sqrAmplMin);
|
|
base->status.B.signalDegradation = (sqrAmpl > aObsv->cfgData.sqrAmplMax);
|
|
base->status.B.trackingLoss = __absf(aObsv->angleErr) > aObsv->cfgData.errorThreshold;
|
|
}
|
|
|
|
|
|
/** \brief Update the object directly from its analog input
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
*/
|
|
void Ifx_AngleTrkF32_update(Ifx_AngleTrkF32 *aObsv)
|
|
{
|
|
Ifx_AngleTrkF32_PosIf *base = &aObsv->base;
|
|
float32 angleEst = Ifx_AngleTrkF32_step(aObsv, *aObsv->sinIn, *aObsv->cosIn, 0);
|
|
|
|
IfxStdIf_Pos_RawAngle newPosition = (IfxStdIf_Pos_RawAngle)(angleEst * (aObsv->base.resolution / 2) / IFX_PI);
|
|
newPosition = newPosition & (aObsv->base.resolution - 1);
|
|
base->rawPosition = newPosition;
|
|
|
|
Ifx_AngleTrkF32_updateStatus(aObsv, *aObsv->sinIn, *aObsv->cosIn);
|
|
}
|
|
|
|
|
|
/** \brief Returns the actual position
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \note This function returns the angular position in ticks [0 .. Ifx_AngleTrkF32.base.resolution-1] (represents 0..360 degree)
|
|
*/
|
|
IfxStdIf_Pos_RawAngle Ifx_AngleTrkF32_getRawPosition(Ifx_AngleTrkF32 *aObsv)
|
|
{
|
|
Ifx_AngleTrkF32_PosIf *base = &aObsv->base;
|
|
return base->rawPosition;
|
|
}
|
|
|
|
|
|
/** \brief Returns the Speed
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
*/
|
|
float32 Ifx_AngleTrkF32_getSpeed(Ifx_AngleTrkF32 *aObsv)
|
|
{
|
|
#if !IFX_CFG_ANGLETRKF32_FILTERED_SPEED
|
|
return aObsv->speedEstB; /* note: using speedEstB has better dynamic */
|
|
#else
|
|
return aObsv->speedLpf.out; /* note: using filtered speed has less ripples */
|
|
#endif
|
|
}
|
|
|
|
|
|
/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>*/
|
|
float32 Ifx_AngleTrkF32_getAbsolutePosition(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return ((float32)driver->base.turn + (float32)driver->base.rawPosition / (float32)driver->base.resolution) * 2.0 * IFX_PI;
|
|
}
|
|
|
|
|
|
IfxStdIf_Pos_Dir Ifx_AngleTrkF32_getDirection(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.direction;
|
|
}
|
|
|
|
|
|
IfxStdIf_Pos_Status Ifx_AngleTrkF32_getFault(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.status;
|
|
}
|
|
|
|
|
|
sint32 Ifx_AngleTrkF32_getOffset(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.offset;
|
|
}
|
|
|
|
|
|
uint16 Ifx_AngleTrkF32_getPeriodPerRotation(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.periodPerRotation;
|
|
}
|
|
|
|
|
|
float32 Ifx_AngleTrkF32_getPosition(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return (float32)driver->base.rawPosition * driver->base.positionConst;
|
|
}
|
|
|
|
|
|
float32 Ifx_AngleTrkF32_getRefreshPeriod(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.Ts;
|
|
}
|
|
|
|
|
|
sint32 Ifx_AngleTrkF32_getResolution(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.resolution;
|
|
}
|
|
|
|
|
|
IfxStdIf_Pos_SensorType Ifx_AngleTrkF32_getSensorType(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
IFX_UNUSED_PARAMETER(driver);
|
|
return IfxStdIf_Pos_SensorType_angletrk;
|
|
}
|
|
|
|
|
|
sint32 Ifx_AngleTrkF32_getTurn(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
return driver->base.turn;
|
|
}
|
|
|
|
|
|
void Ifx_AngleTrkF32_resetFaults(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
driver->base.status.status = 0;
|
|
}
|
|
|
|
|
|
void Ifx_AngleTrkF32_setRefreshPeriod(Ifx_AngleTrkF32 *driver, float32 updatePeriod)
|
|
{
|
|
driver->base.Ts = updatePeriod;
|
|
}
|
|
|
|
|
|
void Ifx_AngleTrkF32_reset(Ifx_AngleTrkF32 *driver)
|
|
{
|
|
driver->base.rawPosition = 0;
|
|
driver->base.direction = IfxStdIf_Pos_Dir_unknown;
|
|
driver->base.turn = 0;
|
|
driver->angleErr = 0.0F;
|
|
driver->angleEst = 0.0F;
|
|
driver->accelEst = 0.0F;
|
|
driver->speedEstA = 0.0F;
|
|
driver->speedEstB = 0.0F;
|
|
driver->angleRef = 0.0F;
|
|
#if IFX_CFG_ANGLETRKF32_SPEED_FILTER
|
|
Ifx_LowPassPt1F32_reset(&driver->speedLpf);
|
|
#endif
|
|
|
|
driver->base.status.status = 0;
|
|
}
|
|
|
|
|
|
boolean Ifx_AngleTrkF32_stdIfPosInit(IfxStdIf_Pos *stdif, Ifx_AngleTrkF32 *driver)
|
|
{
|
|
/* Ensure the stdif is reset to zeros */
|
|
memset(stdif, 0, sizeof(IfxStdIf_Pos));
|
|
|
|
/* Set the driver */
|
|
stdif->driver = driver;
|
|
|
|
/* *INDENT-OFF* Note: this file was indented manually by the author. */
|
|
/* Set the API link */
|
|
stdif->onZeroIrq =(IfxStdIf_Pos_OnZeroIrq )NULL_PTR;
|
|
stdif->getAbsolutePosition=(IfxStdIf_Pos_GetAbsolutePosition )&Ifx_AngleTrkF32_getAbsolutePosition;
|
|
stdif->getDirection =(IfxStdIf_Pos_GetDirection )&Ifx_AngleTrkF32_getDirection;
|
|
stdif->getFault =(IfxStdIf_Pos_GetFault )&Ifx_AngleTrkF32_getFault;
|
|
stdif->getOffset =(IfxStdIf_Pos_GetOffset )&Ifx_AngleTrkF32_getOffset;
|
|
stdif->getPeriodPerRotation =(IfxStdIf_Pos_GetPeriodPerRotation )&Ifx_AngleTrkF32_getPeriodPerRotation;
|
|
stdif->getPosition =(IfxStdIf_Pos_GetPosition )&Ifx_AngleTrkF32_getPosition;
|
|
stdif->getRawPosition =(IfxStdIf_Pos_GetRawPosition )&Ifx_AngleTrkF32_getRawPosition;
|
|
stdif->getRefreshPeriod =(IfxStdIf_Pos_GetRefreshPeriod )&Ifx_AngleTrkF32_getRefreshPeriod;
|
|
stdif->getResolution =(IfxStdIf_Pos_GetResolution )&Ifx_AngleTrkF32_getResolution;
|
|
stdif->getSensorType =(IfxStdIf_Pos_GetSensorType )&Ifx_AngleTrkF32_getSensorType;
|
|
stdif->reset =(IfxStdIf_Pos_Reset )&Ifx_AngleTrkF32_reset;
|
|
stdif->resetFaults =(IfxStdIf_Pos_ResetFaults )&Ifx_AngleTrkF32_resetFaults;
|
|
stdif->getSpeed =(IfxStdIf_Pos_GetSpeed )&Ifx_AngleTrkF32_getSpeed;
|
|
stdif->update =(IfxStdIf_Pos_Update )&Ifx_AngleTrkF32_update;
|
|
stdif->setPosition =(IfxStdIf_Pos_SetPosition )NULL_PTR;
|
|
stdif->setRawPosition =(IfxStdIf_Pos_SetRawPosition )NULL_PTR;
|
|
stdif->setSpeed =(IfxStdIf_Pos_SetSpeed )NULL_PTR;
|
|
stdif->setOffset =(IfxStdIf_Pos_SetOffset )&Ifx_AngleTrkF32_setOffset;
|
|
stdif->setRefreshPeriod =(IfxStdIf_Pos_SetRefreshPeriod )&Ifx_AngleTrkF32_setRefreshPeriod;
|
|
stdif->getTurn =(IfxStdIf_Pos_GetTurn )&Ifx_AngleTrkF32_getTurn;
|
|
/* *INDENT-ON* */
|
|
|
|
return TRUE;
|
|
}
|