Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[ahrs] Fix AHRS DCM course (#3630)

Browse Source
This commit is contained in:
Fabien-B
2026-04-03 16:23:19 +02:00
committed by GitHub
parent 219363e102
commit 8217738ed4
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/ahrs/ahrs_float_dcm.c
+3 -3
View File
@@ -444,7 +444,7 @@ void Drift_correction()
#else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_dcm.gps_course_valid) {
float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float course = ahrs_dcm.gps_course; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis
@@ -464,9 +464,9 @@ void Drift_correction()
float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
float COGY = mag->y; // Non-Tilt-Compensated (for filter stability reasons)
errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
errorCourse = (DCM_Matrix[0][0] * COGY) + (DCM_Matrix[1][0] * COGX); //Calculating YAW error
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
Vector_Scale(errorYaw, &DCM_Matrix[2][0], -errorCourse);
// P only
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);