HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

修改下位机发送的云台陀螺仪信息，角度制改为弧度制

This commit is contained in:
shmily744 2024-01-19 17:58:40 +08:00
parent 3beffa3dea
commit ba3879f815
1 changed files with 29 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
modules/imu/ins_task.c
View File

@ -36,8 +36,7 @@ static float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
static void IMUPWMSet(uint16_t pwm) static void IMUPWMSet(uint16_t pwm) {
{
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm); __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
} }
@ -45,21 +44,18 @@ static void IMUPWMSet(uint16_t pwm)
* @brief 温度控制 * @brief 温度控制
* *
*/ */
static void IMU_Temperature_Ctrl(void) static void IMU_Temperature_Ctrl(void) {
{
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp); PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX)); IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
} }
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差 // 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4) static void InitQuaternion(float *init_q4) {
{
float acc_init[3] = {0}; float acc_init[3] = {0};
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1) float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
float axis_rot[3] = {0}; // 旋转轴 float axis_rot[3] = {0}; // 旋转轴
// 读取100次加速度计数据,取平均值作为初始值 // 读取100次加速度计数据,取平均值作为初始值
for (uint8_t i = 0; i < 100; ++i) for (uint8_t i = 0; i < 100; ++i) {
{
BMI088_Read(&BMI088); BMI088_Read(&BMI088);
acc_init[X] += BMI088.Accel[X]; acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y]; acc_init[Y] += BMI088.Accel[Y];
@ -78,8 +74,7 @@ static void InitQuaternion(float *init_q4)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量) init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
} }
attitude_t *INS_Init(void) attitude_t *INS_Init(void) {
{
if (!INS.init) if (!INS.init)
INS.init = 1; INS.init = 1;
else else
@ -87,8 +82,7 @@ attitude_t *INS_Init(void)
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR) while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
;
IMU_Param.scale[X] = 1; IMU_Param.scale[X] = 1;
IMU_Param.scale[Y] = 1; IMU_Param.scale[Y] = 1;
IMU_Param.scale[Z] = 1; IMU_Param.scale[Z] = 1;
@ -117,8 +111,7 @@ attitude_t *INS_Init(void)
} }
/* 注意以1kHz的频率运行此任务 */ /* 注意以1kHz的频率运行此任务 */
void INS_Task(void) void INS_Task(void) {
{
static uint32_t count = 0; static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f}; const float gravity[3] = {0, 0, 9.81f};
@ -126,8 +119,7 @@ void INS_Task(void)
t += dt; t += dt;
// ins update // ins update
if ((count % 1) == 0) if ((count % 1) == 0) {
{
BMI088_Read(&BMI088); BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X]; INS.Accel[X] = BMI088.Accel[X];
@ -159,7 +151,8 @@ void INS_Task(void)
EarthFrameToBodyFrame(gravity, gravity_b, INS.q); EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波 for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
{ {
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
} }
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
@ -169,18 +162,16 @@ void INS_Task(void)
INS.YawTotalAngle = QEKF_INS.YawTotalAngle; INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
//VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
VisionSetAltitude(INS.Yaw, INS.Pitch); VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
} }
// temperature control // temperature control
if ((count % 2) == 0) if ((count % 2) == 0) {
{
// 500hz // 500hz
IMU_Temperature_Ctrl(); IMU_Temperature_Ctrl();
} }
if ((count++ % 1000) == 0) if ((count++ % 1000) == 0) {
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
} }
} }
@ -191,8 +182,7 @@ void INS_Task(void)
* @param[2] vector in EarthFrame * @param[2] vector in EarthFrame
* @param[3] quaternion * @param[3] quaternion
*/ */
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) {
{
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] + vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] + (q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]); (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
@ -212,8 +202,7 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
* @param[2] vector in BodyFrame * @param[2] vector in BodyFrame
* @param[3] quaternion * @param[3] quaternion
*/ */
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) {
{
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] + vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] + (q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]); (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
@ -235,16 +224,14 @@ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
* @param gyro 角速度 * @param gyro 角速度
* @param accel 加速度 * @param accel 加速度
*/ */
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) {
{
static float lastYawOffset, lastPitchOffset, lastRollOffset; static float lastYawOffset, lastPitchOffset, lastRollOffset;
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33; static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll; float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
if (fabsf(param->Yaw - lastYawOffset) > 0.001f || if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
fabsf(param->Pitch - lastPitchOffset) > 0.001f || fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) {
{
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f); cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f); cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
cosRoll = arm_cos_f32(param->Roll / 57.295779513f); cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
@ -304,8 +291,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
/** /**
* @brief Update quaternion * @brief Update quaternion
*/ */
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) {
{
float qa, qb, qc; float qa, qb, qc;
gx *= 0.5f * dt; gx *= 0.5f * dt;
@ -323,8 +309,7 @@ void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
/** /**
* @brief Convert quaternion to eular angle * @brief Convert quaternion to eular angle
*/ */
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) {
{
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f; *Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f; *Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f; *Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
@ -333,8 +318,7 @@ void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
/** /**
* @brief Convert eular angle to quaternion * @brief Convert eular angle to quaternion
*/ */
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) {
{
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll; float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
Yaw /= 57.295779513f; Yaw /= 57.295779513f;
Pitch /= 57.295779513f; Pitch /= 57.295779513f;