first commit

modules/algorithm/user_lib.h

@@ -0,0 +1,138 @@
+/**
+ ******************************************************************************
+ * @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
+
+#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)
+
+/**
+ * @brief 将±360以内的角度，限制角度在-180 ~ 180
+ *
+ */
+#define ANGLE_LIMIT_360_TO_180_ABS(val) \
+    do {                                \
+        if ((val) > 180)                \
+            (val) -= 360;               \
+        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 返回一块干净的内<E79A84>?,不过仍然需要强制转<E588B6>?为你需要的类型
+ *
+ * @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);
+
+#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
+
+#endif