ELCT/Hero-C-Board-Legacy
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Qi Bin
2025-11-02 21:21:11 +08:00
parent 456cc96c81
commit 3821589bed
693 changed files with 238044 additions and 2735 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
云台/云台-old/AHRS/AHRS_middleware.c Normal file
View File

@@ -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);
}

67
云台/云台-old/AHRS/AHRS_middleware.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef AHRS_MIDDLEWARE_H
#define AHRS_MIDDLEWARE_H
#include "struct_typedef.h"
/**
****************************(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****************************
*/
//重新对应的数据类型
// typedef signed char int8_t;
// typedef signed short int int16_t;
// typedef signed int int32_t;
// typedef signed long long int64_t;
// /* exact-width unsigned integer types */
// typedef unsigned char uint8_t;
// typedef unsigned short int uint16_t;
// typedef unsigned int uint32_t;
// typedef unsigned long long uint64_t;
// typedef unsigned char bool_t;
// typedef float fp32;
// typedef double fp64;
//定义 NULL
#ifndef NULL
#define NULL 0
#endif
//定义PI 值
#ifndef PI
#define PI 3.14159265358979f
#endif
//定义 角度(度)转换到 弧度的比例
#ifndef ANGLE_TO_RAD
#define ANGLE_TO_RAD 0.01745329251994329576923690768489f
#endif
//定义 弧度 转换到 角度的比例
#ifndef RAD_TO_ANGLE
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
#endif
extern void AHRS_get_height(fp32 *high);
extern void AHRS_get_latitude(fp32 *latitude);
extern fp32 AHRS_invSqrt(fp32 num);
extern fp32 AHRS_sinf(fp32 angle);
extern fp32 AHRS_cosf(fp32 angle);
extern fp32 AHRS_tanf(fp32 angle);
extern fp32 AHRS_asinf(fp32 sin);
extern fp32 AHRS_acosf(fp32 cos);
extern fp32 AHRS_atan2f(fp32 y, fp32 x);
#endif

69
云台/云台-old/AHRS/ahrs_lib.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef AHRS_H
#define AHRS_H
#include "AHRS_MiddleWare.h"
/**
* @brief 根据加速度的数据，磁力计的数据进行四元数初始化
* @author luopin
* @param[in] 需要初始化的四元数数组
* @param[in] 用于初始化的加速度计,(x,y,z)不为空 单位 m/s2
* @param[in] 用于初始化的磁力计计,(x,y,z)不为空 单位 uT
* @retval 返回空
*/
extern void AHRS_init(fp32 quat[4], const fp32 accel[3], const fp32 mag[3]);
/**
* @brief 根据陀螺仪的数据，加速度的数据，磁力计的数据进行四元数更新
* @author luopin
* @param[in] 需要更新的四元数数组
* @param[in] 更新定时时间，固定定时调用，例如1000Hz，传入的数据为0.001f,
* @param[in] 用于更新的陀螺仪数据,数组顺序(x,y,z) 单位 rad
* @param[in] 用于初始化的加速度数据,数组顺序(x,y,z) 单位 m/s2
* @param[in] 用于初始化的磁力计数据,数组顺序(x,y,z) 单位 uT
* @retval 返回空
*/
extern bool_t AHRS_update(fp32 quat[4], const fp32 timing_time, const fp32 gyro[3], const fp32 accel[3], const fp32 mag[3]);
/**
* @brief 根据四元数大小计算对应的欧拉角偏航yaw
* @author luopin
* @param[in] 四元数数组，不为NULL
* @retval 返回的偏航角yaw 单位 rad
*/
extern fp32 get_yaw(const fp32 quat[4]);
/**
* @brief 根据四元数大小计算对应的欧拉角俯仰角 pitch
* @author luopin
* @param[in] 四元数数组，不为NULL
* @retval 返回的俯仰角 pitch 单位 rad
*/
extern fp32 get_pitch(const fp32 quat[4]);
/**
* @brief 根据四元数大小计算对应的欧拉角横滚角 roll
* @author luopin
* @param[in] 四元数数组，不为NULL
* @retval 返回的横滚角 roll 单位 rad
*/
extern fp32 get_roll(const fp32 quat[4]);
/**
* @brief 根据四元数大小计算对应的欧拉角yaw，pitch，roll
* @author luopin
* @param[in] 四元数数组，不为NULL
* @param[in] 返回的偏航角yaw 单位 rad
* @param[in] 返回的俯仰角pitch 单位 rad
* @param[in] 返回的横滚角roll 单位 rad
*/
extern void get_angle(const fp32 quat[4], fp32 *yaw, fp32 *pitch, fp32 *roll);
/**
* @brief 返回当前的重力加速度
* @author luopin
* @param[in] 空
* @retval 返回重力加速度 单位 m/s2
*/
extern fp32 get_carrier_gravity(void);
#endif

7562
云台/云台-old/AHRS/arm_math.h Normal file
View File

File diff suppressed because it is too large Load Diff

27
云台/云台-old/AHRS/struct_typedef.h Normal file
View File

@@ -0,0 +1,27 @@
#ifndef STRUCT_TYPEDEF_H
#define STRUCT_TYPEDEF_H
#ifdef __CC__ARM
typedef signed char int8_t;
typedef signed short int int16_t;
typedef signed int int32_t;
typedef signed long long int64_t;
/* exact-width unsigned integer types */
typedef unsigned char uint8_t;
typedef unsigned short int uint16_t;
typedef unsigned int uint32_t;
typedef unsigned long long uint64_t;
#else
#include <stdint.h>
#endif
typedef unsigned char bool_t;
typedef float fp32;
typedef double fp64;
#endif

198
云台/云台-old/AHRS/user_lib.c Normal file
View File

@@ -0,0 +1,198 @@
#include "user_lib.h"
#include "arm_math.h"
////快速开方
//fp32 invSqrt(fp32 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));
// return y;
//}
/**
* @brief 斜波函数初始化
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 间隔的时间，单位 s
* @param[in] 最大值
* @param[in] 最小值
* @retval 返回空
*/
void ramp_init(ramp_function_source_t *ramp_source_type, fp32 frame_period, fp32 max, fp32 min)
{
ramp_source_type->frame_period = frame_period;
ramp_source_type->max_value = max;
ramp_source_type->min_value = min;
ramp_source_type->input = 0.0f;
ramp_source_type->out = 0.0f;
}
/**
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 输入值
* @param[in] 滤波参数
* @retval 返回空
*/
void ramp_calc(ramp_function_source_t *ramp_source_type, fp32 input)
{
ramp_source_type->input = input;
ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
if (ramp_source_type->out > ramp_source_type->max_value)
{
ramp_source_type->out = ramp_source_type->max_value;
}
else if (ramp_source_type->out < ramp_source_type->min_value)
{
ramp_source_type->out = ramp_source_type->min_value;
}
}
/**
* @brief 一阶低通滤波初始化
* @author RM
* @param[in] 一阶低通滤波结构体
* @param[in] 间隔的时间，单位 s
* @param[in] 滤波参数
* @retval 返回空
*/
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, fp32 frame_period, const fp32 num[1])
{
first_order_filter_type->frame_period = frame_period;
first_order_filter_type->num[0] = num[0];
first_order_filter_type->input = 0.0f;
first_order_filter_type->out = 0.0f;
}
/**
* @brief 一阶低通滤波计算
* @author RM
* @param[in] 一阶低通滤波结构体
* @param[in] 间隔的时间，单位 s
* @retval 返回空
*/
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, fp32 input)
{
first_order_filter_type->input = input;
first_order_filter_type->out =
first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
}
//绝对限制
void abs_limit(fp32 *num, fp32 Limit)
{
if (*num > Limit)
{
*num = Limit;
}
else if (*num < -Limit)
{
*num = -Limit;
}
}
//判断符号位
fp32 sign(fp32 value)
{
if (value >= 0.0f)
{
return 1.0f;
}
else
{
return -1.0f;
}
}
//浮点死区
fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0.0f;
}
return Value;
}
//int26死区
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0;
}
return Value;
}
//限幅函数
fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 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;
}
}
//循环限幅函数
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
if (maxValue < minValue)
{
return Input;
}
if (Input > maxValue)
{
fp32 len = maxValue - minValue;
while (Input > maxValue)
{
Input -= len;
}
}
else if (Input < minValue)
{
fp32 len = maxValue - minValue;
while (Input < minValue)
{
Input += len;
}
}
return Input;
}
//弧度格式化为-PI~PI
//角度格式化为-180~180
fp32 theta_format(fp32 Ang)
{
return loop_fp32_constrain(Ang, -180.0f, 180.0f);
}

58
云台/云台-old/AHRS/user_lib.h Normal file
View File

@@ -0,0 +1,58 @@
#ifndef USER_LIB_H
#define USER_LIB_H
#include "struct_typedef.h"
#pragma pack(push, 1)
typedef struct
{
fp32 input; //输入数据
fp32 out; //输出数据
fp32 min_value; //限幅最小值
fp32 max_value; //限幅最大值
fp32 frame_period; //时间间隔
} ramp_function_source_t;
typedef struct
{
fp32 input; //输入数据
fp32 out; //滤波输出的数据
fp32 num[1]; //滤波参数
fp32 frame_period; //滤波的时间间隔 单位 s
} first_order_filter_type_t;
#pragma pack(pop)
//快速开方
extern fp32 invSqrt(fp32 num);
//斜波函数初始化
void ramp_init(ramp_function_source_t *ramp_source_type, fp32 frame_period, fp32 max, fp32 min);
//斜波函数计算
void ramp_calc(ramp_function_source_t *ramp_source_type, fp32 input);
//一阶滤波初始化
extern void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, fp32 frame_period, const fp32 num[1]);
//一阶滤波计算
extern void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, fp32 input);
//绝对限制
extern void abs_limit(fp32 *num, fp32 Limit);
//判断符号位
extern fp32 sign(fp32 value);
//浮点死区
extern fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue);
//int26死区
extern int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
//限幅函数
extern fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 maxValue);
//限幅函数
extern int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
//循环限幅函数
extern fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue);
//角度 °限幅 180 ~ -180
extern fp32 theta_format(fp32 Ang);
//弧度格式化为-PI~PI
#define rad_format(Ang) loop_fp32_constrain((Ang), -PI, PI)
#endif