btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat 更新角度闭环相关功能

Browse Source
This commit is contained in:
井明江
2024-01-16 20:03:21 +08:00
parent 16ead48bba
commit 3bc5e3cf93
19 changed files with 908 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
app/by_imu.c
View File

@@ -14,8 +14,8 @@ gyro_param_t GyroOffset;
bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
float param_Kp = 0.17f; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004f; // 陀螺仪收敛速率的积分增益 0.004
float fast_sqrt(float x)
{
@@ -170,9 +170,9 @@ void ICM_getEulerianAngles(void)
float q3 = Q_info.q3;
// 四元数计算欧拉角
eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch
eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw
eulerAngle.pitch = asinf(-2 * q1 * q3 + 2 * q0 * q2) * 180.0f / M_PI; // pitch
eulerAngle.roll = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180.0f / M_PI; // roll
eulerAngle.yaw = atan2f(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180.0f / M_PI; // yaw
/* 姿态限制*/
if (eulerAngle.roll > 90 || eulerAngle.roll < -90) {
@@ -184,9 +184,9 @@ void ICM_getEulerianAngles(void)
}
}
if (eulerAngle.yaw > 360) {
eulerAngle.yaw -= 360;
} else if (eulerAngle.yaw < 0) {
eulerAngle.yaw += 360;
if (eulerAngle.yaw > 180) {
eulerAngle.yaw -= 180;
} else if (eulerAngle.yaw < -180) {
eulerAngle.yaw += 180;
}
}