init commit

2026-07-24 03:27:45 +08:00 · 2022-10-20 17:13:02 +08:00
commit 78f85b7fda
518 changed files with 409143 additions and 0 deletions
--- a/modules/algorithm/user_lib.h
+++ b/modules/algorithm/user_lib.h
@@ -0,0 +1,152 @@
+/**
+ ******************************************************************************
+ * @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