153 lines
3.7 KiB
C
153 lines
3.7 KiB
C
|
/**
|
|||
|
|
******************************************************************************
|
|||
|
|
* @file user_lib.h
|
|||
|
|
* @author Wang Hongxi
|
|||
|
|
* @version V1.0.0
|
|||
|
|
* @date 2021/2/18
|
|||
|
|
* @brief
|
|||
|
|
******************************************************************************
|
|||
|
|
* @attention
|
|||
|
|
*
|
|||
|
|
******************************************************************************
|
|||
|
|
*/
|
|||
|
|
#ifndef _USER_LIB_H
|
|||
|
|
#define _USER_LIB_H
|
|||
|
|
#include "stdint.h"
|
|||
|
|
#include "main.h"
|
|||
|
|
#include "cmsis_os.h"
|
|||
|
|
|
|||
|
|
enum
|
|||
|
|
{
|
|||
|
|
CHASSIS_DEBUG = 1,
|
|||
|
|
GIMBAL_DEBUG,
|
|||
|
|
INS_DEBUG,
|
|||
|
|
RC_DEBUG,
|
|||
|
|
IMU_HEAT_DEBUG,
|
|||
|
|
SHOOT_DEBUG,
|
|||
|
|
AIMASSIST_DEBUG,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
extern uint8_t GlobalDebugMode;
|
|||
|
|
|
|||
|
|
#ifndef user_malloc
|
|||
|
|
#ifdef _CMSIS_OS_H
|
|||
|
|
#define user_malloc pvPortMalloc
|
|||
|
|
#else
|
|||
|
|
#define user_malloc malloc
|
|||
|
|
#endif
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* boolean type definitions */
|
|||
|
|
#ifndef TRUE
|
|||
|
|
#define TRUE 1 /**< boolean true */
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifndef FALSE
|
|||
|
|
#define FALSE 0 /**< boolean fails */
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* math relevant */
|
|||
|
|
/* radian coefficient */
|
|||
|
|
#ifndef RADIAN_COEF
|
|||
|
|
#define RADIAN_COEF 57.295779513f
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* circumference ratio */
|
|||
|
|
#ifndef PI
|
|||
|
|
#define PI 3.14159265354f
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#define VAL_LIMIT(val, min, max) \
|
|||
|
|
do \
|
|||
|
|
{ \
|
|||
|
|
if ((val) <= (min)) \
|
|||
|
|
{ \
|
|||
|
|
(val) = (min); \
|
|||
|
|
} \
|
|||
|
|
else if ((val) >= (max)) \
|
|||
|
|
{ \
|
|||
|
|
(val) = (max); \
|
|||
|
|
} \
|
|||
|
|
} while (0)
|
|||
|
|
|
|||
|
|
#define ANGLE_LIMIT_360(val, angle) \
|
|||
|
|
do \
|
|||
|
|
{ \
|
|||
|
|
(val) = (angle) - (int)(angle); \
|
|||
|
|
(val) += (int)(angle) % 360; \
|
|||
|
|
} while (0)
|
|||
|
|
|
|||
|
|
#define ANGLE_LIMIT_360_TO_180(val) \
|
|||
|
|
do \
|
|||
|
|
{ \
|
|||
|
|
if ((val) > 180) \
|
|||
|
|
(val) -= 360; \
|
|||
|
|
} while (0)
|
|||
|
|
|
|||
|
|
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
|
|||
|
|
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
|
|||
|
|
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
float input; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float min_value; //<2F><EFBFBD><DEB7><EFBFBD>Сֵ
|
|||
|
|
float max_value; //<2F><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
float frame_period; //ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
} ramp_function_source_t;
|
|||
|
|
|
|||
|
|
typedef __packed struct
|
|||
|
|
{
|
|||
|
|
uint16_t Order;
|
|||
|
|
uint32_t Count;
|
|||
|
|
|
|||
|
|
float *x;
|
|||
|
|
float *y;
|
|||
|
|
|
|||
|
|
float k;
|
|||
|
|
float b;
|
|||
|
|
|
|||
|
|
float StandardDeviation;
|
|||
|
|
|
|||
|
|
float t[4];
|
|||
|
|
} Ordinary_Least_Squares_t;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
|
|||
|
|
float Sqrt(float x);
|
|||
|
|
|
|||
|
|
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
|
|||
|
|
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
|
|||
|
|
//б<><D0B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float abs_limit(float num, float Limit);
|
|||
|
|
//<2F>жϷ<D0B6><CFB7><EFBFBD>λ
|
|||
|
|
float sign(float value);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float float_deadband(float Value, float minValue, float maxValue);
|
|||
|
|
// int26<32><36><EFBFBD><EFBFBD>
|
|||
|
|
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
|
|||
|
|
//<2F><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float float_constrain(float Value, float minValue, float maxValue);
|
|||
|
|
//<2F><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
|
|||
|
|
//ѭ<><D1AD><EFBFBD><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float loop_float_constrain(float Input, float minValue, float maxValue);
|
|||
|
|
//<2F>Ƕ<EFBFBD> <20><><EFBFBD><EFBFBD> 180 ~ -180
|
|||
|
|
float theta_format(float Ang);
|
|||
|
|
|
|||
|
|
int float_rounding(float raw);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
|
|||
|
|
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
|||
|
|
|
|||
|
|
void OLS_Init(Ordinary_Least_Squares_t *OLS, uint16_t order);
|
|||
|
|
void OLS_Update(Ordinary_Least_Squares_t *OLS, float deltax, float y);
|
|||
|
|
float OLS_Derivative(Ordinary_Least_Squares_t *OLS, float deltax, float y);
|
|||
|
|
float OLS_Smooth(Ordinary_Least_Squares_t *OLS, float deltax, float y);
|
|||
|
|
float Get_OLS_Derivative(Ordinary_Least_Squares_t *OLS);
|
|||
|
|
float Get_OLS_Smooth(Ordinary_Least_Squares_t *OLS);
|
|||
|
|
|
|||
|
|
#endif
|