diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 428fa2fe44..9b36d7a0f2 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -112,7 +112,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index ff2a2c3510..0fde4a1238 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -115,7 +115,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 9166f503b2..115801cade 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -105,7 +105,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index df4334d751..571700196d 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -101,7 +101,7 @@
- + diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 8881af6298..0fb7f99e9d 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -35,7 +35,7 @@ - + diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 151464b016..17208c6594 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -27,9 +27,7 @@ - - - + diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 2f10e1c38b..98acc16d07 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -24,7 +24,7 @@ - + diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index d57a70036b..e1662703b4 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -30,7 +30,7 @@ - + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 7c18312931..600de04e88 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -4,7 +4,7 @@ - + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 0ff2abbc07..b4ab80b7cd 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -91,8 +91,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #endif /** by default use the gravity heuristic to reduce gain */ -#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC -#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 #endif #ifdef AHRS_UPDATE_FW_ESTIMATOR @@ -146,11 +146,7 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif -#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - ahrs_impl.use_gravity_heuristic = TRUE; -#else - ahrs_impl.use_gravity_heuristic = FALSE; -#endif + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); } @@ -263,16 +259,15 @@ void ahrs_update_accel(void) { VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.use_gravity_heuristic) { + if (ahrs_impl.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. - * e.g. for WEIGHT_FACTOR 3: + * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ -#define WEIGHT_FACTOR 3 const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; - ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 5c22c81331..a3b83ba6b7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -38,16 +38,22 @@ struct AhrsFloatCmpl { struct FloatRates imu_rate; struct FloatQuat ltp_to_imu_quat; struct FloatRMat ltp_to_imu_rmat; - /* for gravity correction during coordinated turns */ - float ltp_vel_norm; + + bool_t correct_gravity; ///< enable gravity correction during coordinated turns + float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns + bool_t ltp_vel_norm_valid; + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; float weight; - bool_t ltp_vel_norm_valid; - bool_t correct_gravity; - bool_t use_gravity_heuristic; + bool_t heading_aligned; struct FloatVect3 mag_h; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 7f3e17ac42..5f89196323 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -94,8 +94,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_OMEGA) PRINT_CONFIG_VAR(AHRS_MAG_ZETA) /** by default use the gravity heuristic to reduce gain */ -#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC -#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 #endif @@ -141,7 +141,7 @@ void ahrs_init(void) { ahrs_set_mag_gains(); /* set default gravity heuristic */ - ahrs_impl.weight = 1.0; + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -149,8 +149,6 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif - ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC; - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); @@ -287,20 +285,22 @@ void ahrs_update_accel(void) { VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.use_gravity_heuristic) { + if (ahrs_impl.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. - * e.g. for WEIGHT_FACTOR 3: + * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ - #define WEIGHT_FACTOR 5 struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; - ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10; Bound(ahrs_impl.weight, 0.15, 1.0); } + else { + ahrs_impl.weight = 1.0; + } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 56d7ad1131..e7c5fbce6d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -57,12 +57,35 @@ struct AhrsIntCmplQuat { float mag_ki; /* parameters/options that can be changed */ + /** enable gravity vector correction by removing centrifugal acceleration */ bool_t correct_gravity; - bool_t use_gravity_heuristic; - float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) - float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) - float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer - float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; + + /** filter cut-off frequency for correcting the attitude from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelOmega(omega) + */ + float accel_omega; + + /** filter damping for correcting the gyro-bias from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelZeta(zeta) + */ + float accel_zeta; + + /** filter cut-off frequency for correcting the attitude (heading) from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagOmega(omega) + */ + float mag_omega; + + /** filter damping for correcting the gyro bias from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagZeta(zeta) + */ + float mag_zeta; }; extern struct AhrsIntCmplQuat ahrs_impl;