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

estimated attitude converted back to binary floating point

Browse Source
This commit is contained in:
Felix Ruess
2009-07-28 20:58:13 +00:00
parent 3921e22738
commit 05fdaf2ad0
2 changed files with 57 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
+55 -1
View File
@@ -489,7 +489,31 @@ void booz_ahrs_update_accel(void) {
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
/*
* convert to binary floating point
*/
/* IMU rate */
RATES_BFP_OF_REAL(booz_ahrs.imu_rate, bafl_rates);
/* LTP to IMU eulers */
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_imu_euler, bafl_eulers);
/* LTP to IMU quaternion */
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_quat, bafl_quat);
/* LTP to IMU rotation matrix */
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_rmat, bafl_dcm);
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate);
}
@@ -705,4 +729,34 @@ void booz_ahrs_update_mag(void) {
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
/*
* convert to binary floating point
*/
/* IMU rate */
RATES_BFP_OF_REAL(booz_ahrs.imu_rate, bafl_rates);
/* LTP to IMU eulers */
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_imu_euler, bafl_eulers);
/* LTP to IMU quaternion */
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_quat, bafl_quat);
/* LTP to IMU rotation matrix */
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_rmat, bafl_dcm);
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate);
}
void booz_ahrs_update(void) {
booz_ahrs_update_accel();
booz_ahrs_update_mag();
}
sw/airborne/booz/booz2_main.c
+2 -2
View File
@@ -197,7 +197,7 @@ static inline void on_gps_event(void) {
static inline void on_mag_event(void) {
BoozImuScaleMag();
#ifdef USE_LKF_AHRS
//booz_ahrs_update_mag();
#ifdef USE_AHRS_LKF
booz_ahrs_update_mag();
#endif
}