imu test failed

User_Code/module/algorithm/user_lib/user_lib.c

@@ -0,0 +1,215 @@
+/**
+ ******************************************************************************
+ * @file	 user_lib.c
+ * @author  Wang Hongxi
+ * @author  modified by neozng
+ * @version 0.2 beta
+ * @date    2021/2/18
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "stdlib.h"
+#include "memory.h"
+#include "user_lib.h"
+#include "math.h"
+#include "main.h"
+
+#ifdef _CMSIS_OS_H
+#define user_malloc pvPortMalloc
+#else
+#define user_malloc malloc
+#endif
+
+void *zmalloc(size_t size)
+{
+    void *ptr = malloc(size);
+    memset(ptr, 0, size);
+    return ptr;
+}
+
+// 快速开方
+float Sqrt(float x)
+{
+    float y;
+    float delta;
+    float maxError;
+
+    if (x <= 0)
+    {
+        return 0;
+    }
+
+    // initial guess
+    y = x / 2;
+
+    // refine
+    maxError = x * 0.001f;
+
+    do
+    {
+        delta = (y * y) - x;
+        y -= delta / (2 * y);
+    }
+    while (delta > maxError || delta < -maxError);
+
+    return y;
+}
+
+// 绝对值限制
+float abs_limit(float num, float Limit)
+{
+    if (num > Limit)
+    {
+        num = Limit;
+    }
+    else if (num < -Limit)
+    {
+        num = -Limit;
+    }
+    return num;
+}
+
+// 判断符号位
+float sign(float value)
+{
+    if (value >= 0.0f)
+    {
+        return 1.0f;
+    }
+    else
+    {
+        return -1.0f;
+    }
+}
+
+// 浮点死区
+float float_deadband(float Value, float minValue, float maxValue)
+{
+    if (Value < maxValue && Value > minValue)
+    {
+        Value = 0.0f;
+    }
+    return Value;
+}
+
+// 限幅函数
+float float_constrain(float Value, float minValue, float maxValue)
+{
+    if (Value < minValue)
+        return minValue;
+    else if (Value > maxValue)
+        return maxValue;
+    else
+        return Value;
+}
+
+// 限幅函数
+int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
+{
+    if (Value < minValue)
+        return minValue;
+    else if (Value > maxValue)
+        return maxValue;
+    else
+        return Value;
+}
+
+// 循环限幅函数
+float loop_float_constrain(float Input, float minValue, float maxValue)
+{
+    if (maxValue < minValue)
+    {
+        return Input;
+    }
+
+    if (Input > maxValue)
+    {
+        float len = maxValue - minValue;
+        while (Input > maxValue)
+        {
+            Input -= len;
+        }
+    }
+    else if (Input < minValue)
+    {
+        float len = maxValue - minValue;
+        while (Input < minValue)
+        {
+            Input += len;
+        }
+    }
+    return Input;
+}
+
+// 弧度格式化为-PI~PI
+
+// 角度格式化为-180~180
+float theta_format(float Ang)
+{
+    return loop_float_constrain(Ang, -180.0f, 180.0f);
+}
+
+int float_rounding(float raw)
+{
+    static int integer;
+    static float decimal;
+    integer = (int) raw;
+    decimal = raw - integer;
+    if (decimal > 0.5f)
+        integer++;
+    return integer;
+}
+
+// 三维向量归一化
+float *Norm3d(float *v)
+{
+    float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+    v[0] /= len;
+    v[1] /= len;
+    v[2] /= len;
+    return v;
+}
+
+// 计算模长
+float NormOf3d(float *v)
+{
+    return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+}
+
+// 三维向量叉乘v1 x v2
+void Cross3d(float *v1, float *v2, float *res)
+{
+    res[0] = v1[1] * v2[2] - v1[2] * v2[1];
+    res[1] = v1[2] * v2[0] - v1[0] * v2[2];
+    res[2] = v1[0] * v2[1] - v1[1] * v2[0];
+}
+
+// 三维向量点乘
+float Dot3d(float *v1, float *v2)
+{
+    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}
+
+// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
+float AverageFilter(float new_data, float *buf, uint8_t len)
+{
+    float sum = 0;
+    for (uint8_t i = 0; i < len - 1; i++)
+    {
+        buf[i] = buf[i + 1];
+        sum += buf[i];
+    }
+    buf[len - 1] = new_data;
+    sum += new_data;
+    return sum / len;
+}
+
+void MatInit(mat *m, uint8_t row, uint8_t col)
+{
+    m->numCols = col;
+    m->numRows = row;
+    m->pData = (float *) zmalloc(row * col * sizeof(float));
+}

User_Code/module/algorithm/user_lib/user_lib.h

@@ -0,0 +1,165 @@
+/**
+ ******************************************************************************
+ * @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; //输入数据
+    float out; //输出数据
+    float min_value; //限幅最小值
+    float max_value; //限幅最大值
+    float frame_period; //时间间隔
+} 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;
+
+//快速开方
+float Sqrt(float x);
+
+//斜波函数初始化
+void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min);
+
+//斜波函数计算
+float ramp_calc(ramp_function_source_t *ramp_source_type, float input);
+
+//绝对限制
+float abs_limit(float num, float Limit);
+
+//判断符号位
+float sign(float value);
+
+//浮点死区
+float float_deadband(float Value, float minValue, float maxValue);
+
+// int26死区
+int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
+
+//限幅函数
+float float_constrain(float Value, float minValue, float maxValue);
+
+//限幅函数
+int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
+
+//循环限幅函数
+float loop_float_constrain(float Input, float minValue, float maxValue);
+
+//角度 °限幅 180 ~ -180
+float theta_format(float Ang);
+
+int float_rounding(float raw);
+
+//弧度格式化为-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