From 4789b74d529b19330687812f6c0cc0c5a8b3f266 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Luk=C3=A1=C5=A1=20Hr=C3=A1zk=C3=BD?= Date: Wed, 11 Dec 2024 09:17:13 +0100 Subject: [PATCH] Fix ATR/TT winddown with filtering --- src/atr.c | 36 +++++++++++++++++++++++++++--------- src/atr.h | 12 ++++++++---- src/main.c | 17 +++++++++-------- src/torque_tilt.c | 37 ++++++++++++++++++++++++------------- src/torque_tilt.h | 5 ++--- 5 files changed, 70 insertions(+), 37 deletions(-) diff --git a/src/atr.c b/src/atr.c index 745fdc8..0184335 100644 --- a/src/atr.c +++ b/src/atr.c @@ -65,7 +65,7 @@ void atr_configure(ATR *atr, const RefloatConfig *config) { ); } -void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt) { +static float calculate_atr_target(ATR *atr, const MotorData *motor, const RefloatConfig *config) { float abs_torque = fabsf(motor->atr_filtered_current); float torque_offset = 8; // hard-code to 8A for now (shouldn't really be changed much anyways) float atr_threshold = motor->braking ? config->atr_threshold_down : config->atr_threshold_up; @@ -211,6 +211,21 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f atr_step_size /= 2; } + return atr_step_size; +} + +void atr_update( + ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt +) { + float atr_step_size = 0; + + if (!wheelslip) { + atr_step_size = calculate_atr_target(atr, motor, config); + } else { + atr->target_offset *= 0.99; + atr_step_size = atr->off_step_size; + } + if (config->target_filter.type == SFT_NONE) { rate_limitf(&atr->offset, atr->target_offset, atr_step_size); } else if (config->target_filter.type == SFT_EMA3) { @@ -228,8 +243,18 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f } void braketilt_update( - ATR *atr, const MotorData *motor, const RefloatConfig *config, float proportional + ATR *atr, + const MotorData *motor, + const RefloatConfig *config, + bool wheelslip, + float proportional ) { + if (wheelslip) { + atr->braketilt_target_offset *= 0.99; + atr->braketilt_offset = atr->braketilt_target_offset; + return; + } + // braking also should cause setpoint change lift, causing a delayed lingering nose lift if (atr->braketilt_factor < 0 && motor->braking && motor->abs_erpm > 2000) { // negative currents alone don't necessarily constitute active braking, look at @@ -265,10 +290,3 @@ void braketilt_update( rate_limitf(&atr->braketilt_offset, atr->braketilt_target_offset, braketilt_step_size); } - -void atr_and_braketilt_winddown(ATR *atr) { - atr->offset *= 0.995; - atr->target_offset *= 0.99; - atr->braketilt_offset *= 0.995; - atr->braketilt_target_offset *= 0.99; -} diff --git a/src/atr.h b/src/atr.h index 8fdfbf7..98756f5 100644 --- a/src/atr.h +++ b/src/atr.h @@ -49,10 +49,14 @@ void atr_reset(ATR *atr); void atr_configure(ATR *atr, const RefloatConfig *config); -void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt); +void atr_update( + ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt +); void braketilt_update( - ATR *atr, const MotorData *motor, const RefloatConfig *config, float proportional + ATR *atr, + const MotorData *motor, + const RefloatConfig *config, + bool wheelslip, + float proportional ); - -void atr_and_braketilt_winddown(ATR *atr); diff --git a/src/main.c b/src/main.c index 39da912..748bbed 100644 --- a/src/main.c +++ b/src/main.c @@ -1198,21 +1198,22 @@ static void refloat_thd(void *arg) { if (!d->state.darkride) { // in case of wheelslip, don't change torque tilts, instead slightly decrease each // cycle - if (d->state.wheelslip) { - torque_tilt_winddown(&d->torque_tilt); - atr_and_braketilt_winddown(&d->atr); - } else { + if (!d->state.wheelslip) { apply_noseangling(d); d->setpoint += d->noseangling_interpolated; apply_turntilt(d); d->setpoint += d->turntilt_interpolated; - - torque_tilt_update(&d->torque_tilt, &d->motor, &d->float_conf, d->dt); - atr_update(&d->atr, &d->motor, &d->float_conf, d->dt); - braketilt_update(&d->atr, &d->motor, &d->float_conf, d->proportional); } + torque_tilt_update( + &d->torque_tilt, &d->motor, &d->float_conf, d->state.wheelslip, d->dt + ); + atr_update(&d->atr, &d->motor, &d->float_conf, d->state.wheelslip, d->dt); + braketilt_update( + &d->atr, &d->motor, &d->float_conf, d->state.wheelslip, d->proportional + ); + // aggregated torque tilts: // if signs match between torque tilt and ATR + brake tilt, use the more significant // one if signs do not match, they are simply added together diff --git a/src/torque_tilt.c b/src/torque_tilt.c index 145fb7a..89b94d0 100644 --- a/src/torque_tilt.c +++ b/src/torque_tilt.c @@ -23,6 +23,7 @@ #include void torque_tilt_reset(TorqueTilt *tt) { + tt->target_offset = 0; tt->offset = 0; tt->ramped_step_size = 0; @@ -49,8 +50,8 @@ void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config) { ); } -void torque_tilt_update( - TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, float dt +static float calculate_torque_tilt_target( + TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config ) { float strength = motor->braking ? config->torquetilt_strength_regen : config->torquetilt_strength; @@ -60,7 +61,7 @@ void torque_tilt_update( // multiply it by "power" to get our desired angle, and min with the limit // to respect boundaries. Finally multiply it by motor current sign to get // directionality back. - float target_offset = + tt->target_offset = fminf( fmaxf((fabsf(motor->atr_filtered_current) - config->torquetilt_start_current), 0) * strength, @@ -69,8 +70,8 @@ void torque_tilt_update( sign(motor->atr_filtered_current); float step_size = 0; - if ((tt->offset - target_offset > 0 && target_offset > 0) || - (tt->offset - target_offset < 0 && target_offset < 0)) { + if ((tt->offset - tt->target_offset > 0 && tt->target_offset > 0) || + (tt->offset - tt->target_offset < 0 && tt->target_offset < 0)) { step_size = tt->off_step_size; } else { step_size = tt->on_step_size; @@ -80,20 +81,30 @@ void torque_tilt_update( step_size /= 2; } + return step_size; +} + +void torque_tilt_update( + TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt +) { + float step_size = tt->off_step_size; + + if (!wheelslip) { + step_size = calculate_torque_tilt_target(tt, motor, config); + } else { + tt->target_offset *= 0.99; + } + if (config->target_filter.type == SFT_NONE) { - rate_limitf(&tt->offset, target_offset, step_size); + rate_limitf(&tt->offset, tt->target_offset, step_size); } else if (config->target_filter.type == SFT_EMA3) { - ema_filter_update(&tt->ema_target, target_offset, dt); + ema_filter_update(&tt->ema_target, tt->target_offset, dt); tt->offset = tt->ema_target.value; } else if (config->target_filter.type == SFT_THREE_STAGE) { - smooth_target_update(&tt->smooth_target, target_offset); + smooth_target_update(&tt->smooth_target, tt->target_offset); tt->offset = tt->smooth_target.value; } else { // Smoothen changes in tilt angle by ramping the step size - smooth_rampf(&tt->offset, &tt->ramped_step_size, target_offset, step_size, 0.04, 1.5); + smooth_rampf(&tt->offset, &tt->ramped_step_size, tt->target_offset, step_size, 0.04, 1.5); } } - -void torque_tilt_winddown(TorqueTilt *tt) { - tt->offset *= 0.995; -} diff --git a/src/torque_tilt.h b/src/torque_tilt.h index ce0cd55..87851e8 100644 --- a/src/torque_tilt.h +++ b/src/torque_tilt.h @@ -28,6 +28,7 @@ typedef struct { float off_step_size; float ramped_step_size; + float target_offset; // setpoint target offset float offset; // rate-limited setpoint offset SmoothTarget smooth_target; @@ -39,7 +40,5 @@ void torque_tilt_reset(TorqueTilt *tt); void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config); void torque_tilt_update( - TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, float dt + TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt ); - -void torque_tilt_winddown(TorqueTilt *tt);