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

add a mag offset on ahrs (only with cmpl filter for now)

Browse Source
This commit is contained in:
Gautier Hattenberger
2010-08-04 17:30:16 +00:00
parent 06929bc425
commit 3f8ef4dfa7
3 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/ahrs/booz_ahrs_cmpl_euler.c
+7 -1
View File
@@ -53,6 +53,12 @@ void booz_ahrs_init(void) {
booz2_face_reinj_1 = BOOZ2_FACE_REINJ_1;
INT_EULERS_ZERO(booz2_face_uncorrected);
#ifdef IMU_MAG_OFFSET
booz_ahrs_mag_offset = IMU_MAG_OFFSET;
#else
booz_ahrs_mag_offset = 0.;
#endif
}
void booz_ahrs_align(void) {
@@ -177,7 +183,7 @@ void booz_ahrs_update_mag(void) {
// sphi_ctheta * booz_imu.mag.y +
// cphi_ctheta * booz_imu.mag.z;
float m_psi = -atan2(me, mn);
measurement.psi = ((m_psi)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
measurement.psi = ((m_psi - RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
}
sw/airborne/booz/booz_ahrs.c
+3
View File
@@ -26,3 +26,6 @@
struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float;
float booz_ahrs_mag_offset;
sw/airborne/booz/booz_ahrs.h
+2
View File
@@ -64,6 +64,8 @@ struct BoozAhrsFloat {
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
extern float booz_ahrs_mag_offset;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
EULERS_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_euler, booz_ahrs.ltp_to_body_euler); \