From 0066d9d856b187ceeb5a9f2a647109b44d77d98e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 10 Mar 2015 19:15:35 +0100 Subject: [PATCH] [ahrs] mlkf: update state interface from wrapper --- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c | 45 ----------------- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h | 1 - .../subsystems/ahrs/ahrs_float_mlkf_wrapper.c | 48 +++++++++++++++++-- 3 files changed, 45 insertions(+), 49 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 8ea56716421..2b5e0e8d052 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -33,8 +33,6 @@ #include /* for memcpy */ -#include "state.h" - #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" #include "math/pprz_simple_matrix.h" @@ -42,11 +40,6 @@ //#include -#ifndef AHRS_MLKF_OUTPUT_ENABLED -#define AHRS_MLKF_OUTPUT_ENABLED TRUE -#endif -PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED) - #ifndef AHRS_MAG_NOISE_X #define AHRS_MAG_NOISE_X 0.2 #define AHRS_MAG_NOISE_Y 0.2 @@ -62,7 +55,6 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise); static inline void reset_state(void); -static inline void set_body_state_from_quat(void); struct AhrsMlkf ahrs_mlkf; @@ -71,7 +63,6 @@ void ahrs_mlkf_init(void) { ahrs_mlkf.is_aligned = FALSE; - ahrs_mlkf.output_enabled = AHRS_MLKF_OUTPUT_ENABLED; /* init ltp_to_imu quaternion as zero/identity rotation */ float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat); @@ -121,11 +112,6 @@ bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag); - /* set initial body orientation */ - if (ahrs_mlkf.output_enabled) { - set_body_state_from_quat(); - } - /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); @@ -140,9 +126,6 @@ void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt) { propagate_ref(gyro, dt); propagate_state(dt); - if (ahrs_mlkf.output_enabled) { - set_body_state_from_quat(); - } } void ahrs_mlkf_update_accel(struct Int32Vect3 *accel) @@ -157,9 +140,6 @@ void ahrs_mlkf_update_accel(struct Int32Vect3 *accel) struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn}; update_state(&earth_g, &imu_g, &g_noise); reset_state(); - if (ahrs_mlkf.output_enabled) { - set_body_state_from_quat(); - } } void ahrs_mlkf_update_mag(struct Int32Vect3 *mag) @@ -169,9 +149,6 @@ void ahrs_mlkf_update_mag(struct Int32Vect3 *mag) #else ahrs_mlkf_update_mag_2d(mag); #endif - if (ahrs_mlkf.output_enabled) { - set_body_state_from_quat(); - } } void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag) @@ -398,25 +375,3 @@ static inline void reset_state(void) float_quat_identity(&ahrs_mlkf.gibbs_cor); } - -/** - * Compute body orientation and rates from imu orientation and rates - */ -static inline void set_body_state_from_quat(void) -{ - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu); - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu); - - /* Compute LTP to BODY quaternion */ - struct FloatQuat ltp_to_body_quat; - float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat); - /* Set in state interface */ - stateSetNedToBodyQuat_f(<p_to_body_quat); - - /* compute body rates */ - struct FloatRates body_rate; - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate); - /* Set state */ - stateSetBodyRates_f(&body_rate); - -} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 2c32c457e44..b75501f5d38 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -59,7 +59,6 @@ struct AhrsMlkf { enum AhrsMlkfStatus status; bool_t is_aligned; - bool_t output_enabled; ///< if TRUE with push the estimation results to the state interface }; extern struct AhrsMlkf ahrs_mlkf; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c index 34eeb24301b..17e4363bd98 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -28,9 +28,19 @@ #include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h" #include "subsystems/ahrs.h" #include "subsystems/abi.h" +#include "state.h" +#ifndef AHRS_MLKF_OUTPUT_ENABLED +#define AHRS_MLKF_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_mlkf_output_enabled; static uint32_t ahrs_mlkf_last_stamp; +static void set_body_state_from_quat(void); + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" #include "mcu_periph/sys_time.h" @@ -85,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_mlkf.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_mlkf_propagate(gyro, dt); + set_body_state_from_quat(); } last_stamp = stamp; #else @@ -93,6 +104,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); ahrs_mlkf_propagate(gyro, dt); + set_body_state_from_quat(); } #endif } @@ -103,6 +115,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)), { if (ahrs_mlkf.is_aligned) { ahrs_mlkf_update_accel(accel); + set_body_state_from_quat(); } } @@ -112,6 +125,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)), { if (ahrs_mlkf.is_aligned) { ahrs_mlkf_update_mag(mag); + set_body_state_from_quat(); } } @@ -121,7 +135,10 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_mlkf.is_aligned) { - ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag); + /* set initial body orientation in state interface if alignment was successful */ + if (ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag)) { + set_body_state_from_quat(); + } } } @@ -138,12 +155,36 @@ static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVe static bool_t ahrs_mlkf_enable_output(bool_t enable) { - ahrs_mlkf.output_enabled = enable; - return ahrs_mlkf.output_enabled; + ahrs_mlkf_output_enabled = enable; + return ahrs_mlkf_output_enabled; +} + +/** + * Compute body orientation and rates from imu orientation and rates + */ +static void set_body_state_from_quat(void) +{ + if (ahrs_mlkf_output_enabled) { + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu); + + /* Compute LTP to BODY quaternion */ + struct FloatQuat ltp_to_body_quat; + float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat); + /* Set in state interface */ + stateSetNedToBodyQuat_f(<p_to_body_quat); + + /* compute body rates */ + struct FloatRates body_rate; + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate); + /* Set state */ + stateSetBodyRates_f(&body_rate); + } } void ahrs_mlkf_register(void) { + ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED; ahrs_mlkf_init(); ahrs_register_impl(ahrs_mlkf_enable_output); @@ -162,3 +203,4 @@ void ahrs_mlkf_register(void) register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif } +