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
310 lines
12 KiB
C
310 lines
12 KiB
C
/**
|
|
* \file Ifx_AngleTrkF32.h
|
|
* \brief Angle-tracking observer for sin/cos analog position sensor
|
|
*
|
|
*
|
|
*
|
|
* \version disabled
|
|
* \copyright Copyright (c) 2013 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.
|
|
|
|
*
|
|
* \defgroup library_srvsw_sysse_math_f32_angletrk Angle-Tracking Observer
|
|
* This driver implements \ref library_srvsw_stdif_posif using 3rd order Angle-Tracking Observer algorithm.
|
|
* \ingroup library_srvsw_sysse_math_f32
|
|
*
|
|
*/
|
|
|
|
#ifndef IFX_ANGLETRKF32_H
|
|
#define IFX_ANGLETRKF32_H
|
|
|
|
//________________________________________________________________________________________
|
|
// INCLUDES
|
|
|
|
#include "StdIf/IfxStdIf_Pos.h"
|
|
#include "SysSe/Math/Ifx_LowPassPt1F32.h"
|
|
|
|
//________________________________________________________________________________________
|
|
// CONFIGURATION DEFINES
|
|
|
|
#ifndef IFX_CFG_ANGLETRKF32_SPEED_FILTER
|
|
#define IFX_CFG_ANGLETRKF32_SPEED_FILTER (1)
|
|
#endif
|
|
|
|
/** \brief Specify whether the speed reading is filtered by low-pass filter or not */
|
|
#ifndef IFX_CFG_ANGLETRKF32_FILTERED_SPEED
|
|
#define IFX_CFG_ANGLETRKF32_FILTERED_SPEED (1)
|
|
#endif
|
|
|
|
//________________________________________________________________________________________
|
|
// DATA STRUCTURES
|
|
|
|
/** \brief Angle Tracking Observer configuration */
|
|
typedef struct
|
|
{
|
|
float32 kp; /**< \brief Proportional gain */
|
|
float32 ki; /**< \brief Integrator gain */
|
|
float32 kd; /**< \brief Derivative gain */
|
|
float32 speedLpfFc; /**< \brief Cut-off frequency of speed low-pass filter. */
|
|
float32 errorThreshold; /**< \brief Threshold of error value in the tracking loop */
|
|
sint32 sqrAmplMax; /**< \brief Maximum value for square of signal amplitudes */
|
|
sint32 sqrAmplMin; /**< \brief Minimum value for square of signal amplitudes */
|
|
uint16 periodPerRotation; /**< \brief Number of electrical periods per mechanical rotation */
|
|
boolean reversed; /**< \brief TRUE: reversed direction, FALSE: straight direction */
|
|
sint32 resolution; /**< \brief Sensor resolution */
|
|
IfxStdIf_Pos_RawAngle offset; /**< \brief Offset in ticks. [0 .. (\ref Ifx_AngleTrkF32_Config.resolution - 1)] */
|
|
sint16 *sinIn; /**< \brief Pointer to SIN input variable */
|
|
sint16 *cosIn; /**< \brief Pointer to COS input variable */
|
|
} Ifx_AngleTrkF32_Config;
|
|
|
|
typedef struct
|
|
{
|
|
float32 kp; /**< \brief Proportional gain */
|
|
float32 ki; /**< \brief Integrator gain */
|
|
float32 kd; /**< \brief Derivative gain */
|
|
float32 errorThreshold; /**< \brief Threshold of error value in the tracking loop */
|
|
sint32 sqrAmplMax; /**< \brief Maximum value for square of signal amplitudes */
|
|
sint32 sqrAmplMin; /**< \brief Minimum value for square of signal amplitudes */
|
|
} Ifx_AngleTrkF32_CfgData;
|
|
|
|
typedef struct
|
|
{
|
|
IfxStdIf_Pos_RawAngle rawPosition; /**< \brief raw position in ticks. \note: the value already contains the offset */
|
|
sint32 turn; /**< \brief number of mechanical turns. FIXME to be implemented */
|
|
IfxStdIf_Pos_Dir direction; /**< \brief rotation direction */
|
|
IfxStdIf_Pos_Status status; /**< \brief error code (0 = no error) */
|
|
/* configuration */
|
|
IfxStdIf_Pos_RawAngle offset; /**< \brief raw position offset */
|
|
boolean reversed; /**< \brief reverse direction */
|
|
uint16 periodPerRotation; /**< \brief sensor 'electrical' periods per mechanical rotation */
|
|
IfxStdIf_Pos_RawAngle resolution; /**< \brief resolution of this position sensor interface */
|
|
float32 Ts; /**< \brief update period in seconds */
|
|
float32 positionConst; /**< \brief constant for calculating mechanical position (in rad) from raw position */
|
|
} Ifx_AngleTrkF32_PosIf;
|
|
|
|
/** \brief Angle Tracking Observer object */
|
|
typedef struct
|
|
{
|
|
Ifx_AngleTrkF32_PosIf base;
|
|
Ifx_AngleTrkF32_CfgData cfgData;
|
|
sint16 *sinIn; /**< \brief Pointer to SIN input variable */
|
|
sint16 *cosIn; /**< \brief Pointer to COS input variable */
|
|
float32 halfTs;
|
|
float32 angleAtan;
|
|
float32 angleRef;
|
|
float32 angleEst;
|
|
float32 speedEstA;
|
|
float32 speedEstB;
|
|
float32 accelEst;
|
|
float32 angleErr;
|
|
Ifx_LowPassPt1F32 speedLpf; /**< Only used if IFX_CFG_ANGLETRKF32_SPEED_FILTER is set */
|
|
} Ifx_AngleTrkF32;
|
|
|
|
/** \addtogroup library_srvsw_sysse_math_f32_angletrk
|
|
* \{ */
|
|
|
|
//________________________________________________________________________________________
|
|
// PUBLIC FUNCTION PROTOTYPES
|
|
|
|
/** \name Initialisation functions
|
|
* Example use:
|
|
* \code
|
|
* extern sint16 g_SinInput, g_CosInput;
|
|
* Ifx_AngleTrkF32_Config driverConfig;
|
|
* Ifx_AngleTrkF32 driverData;
|
|
* Ifx_AngleTrkF32_initConfig(&driverConfig, &g_SinInput, g_CosInput);
|
|
* driverConfig.errorThreshold = 5.0 / 180 * IFX_PI; // 5 degree error
|
|
* driverConfig.sqrAmplMax = 1.01 * 1.01;
|
|
* driverConfig.sqrAmplMin = 0.99 * 0.99;
|
|
* driverConfig.speedLpfFc = 100;
|
|
* driverConfig.periodPerRotation = 1;
|
|
* driverConfig.reversed = FALSE;
|
|
* driverConfig.offset = 0;
|
|
* Ifx_AngleTrkF32_init(&driverData, &driverConfig);
|
|
* \endcode
|
|
* Prototypes:
|
|
* \{ */
|
|
IFX_EXTERN void Ifx_AngleTrkF32_init(Ifx_AngleTrkF32 *aObsv, const Ifx_AngleTrkF32_Config *config, float32 Ts);
|
|
IFX_EXTERN void Ifx_AngleTrkF32_initConfig(Ifx_AngleTrkF32_Config *config, sint16 *sinIn, sint16 *cosIn);
|
|
/** \} */
|
|
|
|
/** \name Protected functions
|
|
* These functions are not for end-user but may be used by another driver which extends
|
|
* the functionality of library_srvsw_sysse_math_f32_angletrk
|
|
* \{ */
|
|
IFX_EXTERN float32 Ifx_AngleTrkF32_step(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn, float32 phase);
|
|
IFX_EXTERN void Ifx_AngleTrkF32_updateStatus(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn);
|
|
IFX_INLINE float32 Ifx_AngleTrkF32_getLoopSpeed(Ifx_AngleTrkF32 *aObsv);
|
|
/** \} */
|
|
|
|
/** \} */
|
|
|
|
/** \brief get the speed.
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
*/
|
|
IFX_INLINE float32 Ifx_AngleTrkF32_getLoopSpeed(Ifx_AngleTrkF32 *aObsv)
|
|
{
|
|
return aObsv->speedEstB; /* note: using speedEstB has better dynamic */
|
|
/* return aObsv->speedLpf.out; */
|
|
}
|
|
|
|
|
|
/** \addtogroup library_srvsw_sysse_math_f32_angletrk
|
|
* \{ */
|
|
|
|
/******************************************************************************/
|
|
/*-------------------------Global Function Prototypes-------------------------*/
|
|
/******************************************************************************/
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetAbsolutePosition
|
|
* \param driver driver handle
|
|
* \return absolute position
|
|
*/
|
|
IFX_EXTERN float32 Ifx_AngleTrkF32_getAbsolutePosition(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetDirection
|
|
* \param driver driver handle
|
|
* \return direction
|
|
*/
|
|
IFX_EXTERN IfxStdIf_Pos_Dir Ifx_AngleTrkF32_getDirection(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetFault
|
|
* \param driver driver handle
|
|
* \return Fault
|
|
*/
|
|
IFX_EXTERN IfxStdIf_Pos_Status Ifx_AngleTrkF32_getFault(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetOffset
|
|
* \param driver driver handle
|
|
* \return offset address
|
|
*/
|
|
IFX_EXTERN sint32 Ifx_AngleTrkF32_getOffset(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetPeriodPerRotation
|
|
* \param driver driver handle
|
|
* \return Period per rotation
|
|
*/
|
|
IFX_EXTERN uint16 Ifx_AngleTrkF32_getPeriodPerRotation(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetPosition
|
|
* \param driver driver handle
|
|
* \return position
|
|
*/
|
|
IFX_EXTERN float32 Ifx_AngleTrkF32_getPosition(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetRawPosition
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \return position in ticks
|
|
*/
|
|
IFX_EXTERN IfxStdIf_Pos_RawAngle Ifx_AngleTrkF32_getRawPosition(Ifx_AngleTrkF32 *aObsv);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetRefreshPeriod
|
|
* \param driver driver handle
|
|
* \return update period
|
|
*/
|
|
IFX_EXTERN float32 Ifx_AngleTrkF32_getRefreshPeriod(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetResolution
|
|
* \param driver driver handle
|
|
* \return resolution
|
|
*/
|
|
IFX_EXTERN sint32 Ifx_AngleTrkF32_getResolution(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetTurn
|
|
* \param driver driver handle
|
|
* \return Returns the number of turns
|
|
*/
|
|
IFX_EXTERN sint32 Ifx_AngleTrkF32_getTurn(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetSensorType
|
|
* \param driver driver handle
|
|
* \return sensor type
|
|
*/
|
|
IFX_EXTERN IfxStdIf_Pos_SensorType Ifx_AngleTrkF32_getSensorType(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_GetSpeed
|
|
* \param aObsv Pointer to the Ifx_AngleTrkF32 object
|
|
* \return speed
|
|
*/
|
|
IFX_EXTERN float32 Ifx_AngleTrkF32_getSpeed(Ifx_AngleTrkF32 *aObsv);
|
|
|
|
/** \brief \see IfxStdIf_Pos_Reset
|
|
* \param driver driver handle
|
|
* \return None
|
|
*/
|
|
IFX_EXTERN void Ifx_AngleTrkF32_reset(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_ResetFaults
|
|
* \param driver driver handle
|
|
* \return None
|
|
*/
|
|
IFX_EXTERN void Ifx_AngleTrkF32_resetFaults(Ifx_AngleTrkF32 *driver);
|
|
|
|
/** \brief \see IfxStdIf_Pos_SetOffset
|
|
* \param aObsv driver handle
|
|
* \param offset offset
|
|
* \return None
|
|
*/
|
|
IFX_EXTERN void Ifx_AngleTrkF32_setOffset(Ifx_AngleTrkF32 *aObsv, IfxStdIf_Pos_RawAngle offset);
|
|
|
|
/** \brief \see IfxStdIf_Pos_SetRefreshPeriod
|
|
* \param driver driver handle
|
|
* \param updatePeriod update period
|
|
* \return None
|
|
*/
|
|
IFX_EXTERN void Ifx_AngleTrkF32_setRefreshPeriod(Ifx_AngleTrkF32 *driver, float32 updatePeriod);
|
|
|
|
/** \brief \see IfxStdIf_Pos_Update
|
|
* \param aObsv driver handle
|
|
* \return None
|
|
*/
|
|
IFX_EXTERN void Ifx_AngleTrkF32_update(Ifx_AngleTrkF32 *aObsv);
|
|
|
|
/** \} */
|
|
|
|
/******************************************************************************/
|
|
/*-------------------------Global Function Prototypes-------------------------*/
|
|
/******************************************************************************/
|
|
|
|
/** \brief Initializes the standard interface "Pos"
|
|
* \param stdif Standard interface position object
|
|
* \param driver Virtual position sensor
|
|
* \return TRUE on success else FALSE
|
|
*/
|
|
IFX_EXTERN boolean Ifx_AngleTrkF32_stdIfPosInit(IfxStdIf_Pos *stdif, Ifx_AngleTrkF32 *driver);
|
|
|
|
#endif /* IFX_ANGLETRKF32_H */
|