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

add possibilty to use ahrs_int_cmpl_euler for fixedwings

Browse Source
This commit is contained in:
Felix Ruess
2011-08-23 17:34:05 +02:00
parent cce5d4d9ff
commit 1987d26b88
2 changed files with 39 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+31
View File
@@ -222,3 +222,34 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
}
#ifdef AHRS_UPDATE_FW_ESTIMATOR
// TODO use ahrs result directly
#include "estimator.h"
// remotely settable
#ifndef INS_ROLL_NEUTRAL_DEFAULT
#define INS_ROLL_NEUTRAL_DEFAULT 0
#endif
#ifndef INS_PITCH_NEUTRAL_DEFAULT
#define INS_PITCH_NEUTRAL_DEFAULT 0
#endif
float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
void ahrs_update_fw_estimator(void)
{
struct FloatEulers att;
// export results to estimator
EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler);
estimator_phi = att.phi - ins_roll_neutral;
estimator_theta = att.theta - ins_pitch_neutral;
estimator_psi = att.psi;
struct FloatRates rates;
RATES_FLOAT_OF_BFP(rates, ahrs.body_rate);
estimator_p = rates.p;
estimator_q = rates.q;
}
#endif //AHRS_UPDATE_FW_ESTIMATOR
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+8
View File
@@ -37,4 +37,12 @@ struct AhrsIntCmplEuler {
extern struct AhrsIntCmplEuler ahrs_impl;
#ifdef AHRS_UPDATE_FW_ESTIMATOR
// TODO copy ahrs to state instead of estimator
void ahrs_update_fw_estimator(void);
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
#endif
#endif /* AHRS_INT_CMPL_EULER_H */