138 lines
3.7 KiB
C
138 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"
|
|||
|
|
#include "stm32f407xx.h"
|
|||
|
|
#include "arm_math.h"
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifndef user_malloc
|
|||
|
|
#ifdef _CMSIS_OS_H
|
|||
|
|
#define user_malloc pvPortMalloc
|
|||
|
|
#else
|
|||
|
|
#define user_malloc malloc
|
|||
|
|
#endif
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#define msin(x) (arm_sin_f32(x))
|
|||
|
|
#define mcos(x) (arm_cos_f32(x))
|
|||
|
|
|
|||
|
|
typedef arm_matrix_instance_f32 mat;
|
|||
|
|
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
|
|||
|
|
#define MatAdd arm_mat_add_f32
|
|||
|
|
#define MatSubtract arm_mat_sub_f32
|
|||
|
|
#define MatMultiply arm_mat_mult_f32
|
|||
|
|
#define MatTranspose arm_mat_trans_f32
|
|||
|
|
#define MatInverse arm_mat_inverse_f32
|
|||
|
|
void MatInit(mat *m, uint8_t row, uint8_t col);
|
|||
|
|
|
|||
|
|
/* boolean type definitions */
|
|||
|
|
#ifndef TRUE
|
|||
|
|
#define TRUE 1 /**< boolean true */
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifndef FALSE
|
|||
|
|
#define FALSE 0 /**< boolean fails */
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* circumference ratio */
|
|||
|
|
#ifndef PI
|
|||
|
|
#define PI 3.14159265354f
|
|||
|
|
#endif
|
|||
|
|
typedef __packed struct
|
|||
|
|
{
|
|||
|
|
float input; //输入数据
|
|||
|
|
float out; //滤波输出的数据
|
|||
|
|
float num[1]; //滤波参数
|
|||
|
|
float frame_period; //滤波的时间间隔 单位 s
|
|||
|
|
} first_order_filter_type_t;
|
|||
|
|
|
|||
|
|
#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))
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 返回一块干净的内<EFBFBD>?,不过仍然需要强制转<EFBFBD>?为你需要的类型
|
|||
|
|
*
|
|||
|
|
* @param size 分配大小
|
|||
|
|
* @return void*
|
|||
|
|
*/
|
|||
|
|
void *zmalloc(size_t size);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
|
|||
|
|
float Sqrt(float x);
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float abs_limit(float num, float Limit);
|
|||
|
|
// <20>жϷ<D0B6><CFB7><EFBFBD>λ
|
|||
|
|
float sign(float value);
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float float_deadband(float Value, float minValue, float maxValue);
|
|||
|
|
// <20><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float float_constrain(float Value, float minValue, float maxValue);
|
|||
|
|
// <20><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);
|
|||
|
|
// <20>Ƕ<EFBFBD> <20><><EFBFBD><EFBFBD> 180 ~ -180
|
|||
|
|
float theta_format(float Ang);
|
|||
|
|
|
|||
|
|
int float_rounding(float raw);
|
|||
|
|
|
|||
|
|
float *Norm3d(float *v);
|
|||
|
|
|
|||
|
|
float NormOf3d(float *v);
|
|||
|
|
|
|||
|
|
void Cross3d(float *v1, float *v2, float *res);
|
|||
|
|
|
|||
|
|
float Dot3d(float *v1, float *v2);
|
|||
|
|
|
|||
|
|
float AverageFilter(float new_data, float *buf, uint8_t len);
|
|||
|
|
//一阶低通滤波初始化
|
|||
|
|
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1]);
|
|||
|
|
//一阶低通滤波计算
|
|||
|
|
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input);
|
|||
|
|
|
|||
|
|
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
|||
|
|
|
|||
|
|
#endif
|