Skip to content

Commit

Permalink
Fix ATR/TT winddown with filtering
Browse files Browse the repository at this point in the history
  • Loading branch information
@lukash
lukash committed Dec 12, 2024
1 parent 2406924 commit 4789b74
Show file tree
Hide file tree
Showing 5 changed files with 70 additions and 37 deletions.
36 changes: 27 additions & 9 deletions src/atr.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand 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) {
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
12 changes: 8 additions & 4 deletions src/atr.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
17 changes: 9 additions & 8 deletions src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
37 changes: 24 additions & 13 deletions src/torque_tilt.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <math.h>

void torque_tilt_reset(TorqueTilt *tt) {
tt->target_offset = 0;
tt->offset = 0;
tt->ramped_step_size = 0;

Expand All @@ -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;
Expand All @@ -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,
Expand All @@ -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;
Expand All @@ -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;
}
5 changes: 2 additions & 3 deletions src/torque_tilt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);

0 comments on commit 4789b74

Please sign in to comment.