Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:31:55 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

AHRS_DCM_FLOAT fix heading: export has 180deg error, du to sign error in correction, so the align is always 180deg wrong (#3470)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source
This commit is contained in:
Christophe De Wagter
2025-06-17 22:21:06 +02:00
committed by GitHub
parent 6370ff488b
commit 3f7bc48507
1 changed files with 1 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/ahrs/ahrs_float_dcm.c
+1 -2
View File
@@ -434,7 +434,7 @@ void Drift_correction()
//Calculating YAW error //Calculating YAW error
errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X); errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. //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);
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW); Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional. Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
@@ -532,6 +532,5 @@ static void compute_ahrs_representations(void)
ahrs_dcm.ltp_to_body_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]); ahrs_dcm.ltp_to_body_euler.phi = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
ahrs_dcm.ltp_to_body_euler.theta = -asin(DCM_Matrix[2][0]); ahrs_dcm.ltp_to_body_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_dcm.ltp_to_body_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); ahrs_dcm.ltp_to_body_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
ahrs_dcm.ltp_to_body_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif #endif
} }