同步步兵代码，暂时关闭底盘跟随

云台/云台-old/AHRS/AHRS_middleware.c

@@ -0,0 +1,145 @@
+/**
+  ****************************(C) COPYRIGHT 2016 DJI****************************
+  * @file       AHRS_MiddleWare.c/h
+  * @brief      姿态解算中间层，为姿态解算提供相关函数
+  * @note
+  * @history
+  *  Version    Date            Author          Modification
+  *  V1.0.0     Dec-26-2018     RM              1. 完成
+  *
+  @verbatim
+  ==============================================================================
+
+  ==============================================================================
+  @endverbatim
+  ****************************(C) COPYRIGHT 2016 DJI****************************
+  */
+
+#include "AHRS_MiddleWare.h"
+#include "ahrs_lib.h"
+#include "arm_math.h"
+//#include "main.h"
+
+/**
+ * @brief          用于获取当前高度
+ * @author         RM
+ * @param[in]      高度的指针，fp32
+ * @retval         返回空
+ */
+
+void AHRS_get_height(fp32 *high)
+{
+    if (high != NULL)
+    {
+        *high = 0.0f;
+    }
+}
+
+/**
+ * @brief          用于获取当前纬度
+ * @author         RM
+ * @param[in]      纬度的指针，fp32
+ * @retval         返回空
+ */
+
+void AHRS_get_latitude(fp32 *latitude)
+{
+    if (latitude != NULL)
+    {
+        *latitude = 22.0f;
+    }
+}
+
+/**
+ * @brief          快速开方函数，
+ * @author         RM
+ * @param[in]      输入需要开方的浮点数，fp32
+ * @retval         返回1/sqrt 开方后的倒数
+ */
+
+fp32 AHRS_invSqrt(fp32 num)
+{
+    return 1 / sqrtf(num);
+
+    //    fp32 halfnum = 0.5f * num;
+    //    fp32 y = num;
+    //    long i = *(long*)&y;
+    //    i = 0x5f3759df - (i >> 1);
+    //    y = *(fp32*)&i;
+    //    y = y * (1.5f - (halfnum * y * y));
+    //    y = y * (1.5f - (halfnum * y * y));
+    //    return y;
+}
+
+/**
+ * @brief          sin函数
+ * @author         RM
+ * @param[in]      角度 单位 rad
+ * @retval         返回对应角度的sin值
+ */
+
+fp32 AHRS_sinf(fp32 angle)
+{
+    return sinf(angle);
+}
+/**
+ * @brief          cos函数
+ * @author         RM
+ * @param[in]      角度 单位 rad
+ * @retval         返回对应角度的cos值
+ */
+
+fp32 AHRS_cosf(fp32 angle)
+{
+    return cosf(angle);
+}
+
+/**
+ * @brief          tan函数
+ * @author         RM
+ * @param[in]      角度 单位 rad
+ * @retval         返回对应角度的tan值
+ */
+
+fp32 AHRS_tanf(fp32 angle)
+{
+    return tanf(angle);
+}
+/**
+ * @brief          用于32位浮点数的反三角函数 asin函数
+ * @author         RM
+ * @param[in]      输入sin值，最大1.0f，最小-1.0f
+ * @retval         返回角度 单位弧度
+ */
+
+fp32 AHRS_asinf(fp32 sin)
+{
+
+    return asinf(sin);
+}
+
+/**
+ * @brief          反三角函数acos函数
+ * @author         RM
+ * @param[in]      输入cos值，最大1.0f，最小-1.0f
+ * @retval         返回对应的角度 单位弧度
+ */
+
+fp32 AHRS_acosf(fp32 cos)
+{
+
+    return acosf(cos);
+}
+
+/**
+ * @brief          反三角函数atan函数
+ * @author         RM
+ * @param[in]      输入tan值中的y值 最大正无穷，最小负无穷
+ * @param[in]      输入tan值中的x值 最大正无穷，最小负无穷
+ * @retval         返回对应的角度 单位弧度
+ */
+
+fp32 AHRS_atan2f(fp32 y, fp32 x)
+{
+    return atan2f(y, x);
+}