diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 7f885351418..3ca1a0f63d9 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -91,6 +91,18 @@ int renorm_blowup_count = 0; float imu_health = 0.; #endif +#ifdef USE_HIGH_ACCEL_FLAG +// High Accel Flag +#define HIGH_ACCEL_LOW_SPEED 15.0 +#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis +#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ) +#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis +bool_t high_accel_done; +bool_t high_accel_flag; +// Command vector for thrust (fixed_wing) +#include "inter_mcu.h" +#endif + static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) { @@ -175,6 +187,11 @@ void ahrs_init(void) { /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); + +#ifdef USE_HIGH_ACCEL_FLAG + high_accel_done = FALSE; + high_accel_flag = FALSE; +#endif } void ahrs_align(void) @@ -411,6 +428,25 @@ void Drift_correction(void) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // +#ifdef USE_HIGH_ACCEL_FLAG + // Test for high acceleration: + // - low speed + // - high thrust + if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { + high_accel_flag = TRUE; + } else { + high_accel_flag = FALSE; + if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { + high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small) + } + if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { + high_accel_done = FALSE; // Activate high accel after landing + } + } + if (high_accel_flag) { Accel_weight = 0.; } +#endif + + #if PERFORMANCE_REPORTING == 1 {