146 lines
3.1 KiB
C
146 lines
3.1 KiB
C
/**
|
||
****************************(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);
|
||
}
|