Skip to content

Commit 013ae41

Browse files
lukashlukash
committed
Fix ATR/TT winddown with filtering
1 parent 91d5385 commit 013ae41

5 files changed

Lines changed: 70 additions & 37 deletions

File tree

‎src/atr.c‎

Lines changed: 27 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ void atr_configure(ATR *atr, const RefloatConfig *config) {
6565
);
6666
}
6767

68-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt) {
68+
static float calculate_atr_target(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
6969
float abs_torque = fabsf(motor->atr_filtered_current);
7070
float torque_offset = 8; // hard-code to 8A for now (shouldn't really be changed much anyways)
7171
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
211211
atr_step_size /= 2;
212212
}
213213

214+
return atr_step_size;
215+
}
216+
217+
void atr_update(
218+
ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
219+
) {
220+
float atr_step_size = 0;
221+
222+
if (!wheelslip) {
223+
atr_step_size = calculate_atr_target(atr, motor, config);
224+
} else {
225+
atr->target_offset *= 0.99;
226+
atr_step_size = atr->off_step_size;
227+
}
228+
214229
if (config->target_filter.type == SFT_NONE) {
215230
rate_limitf(&atr->offset, atr->target_offset, atr_step_size);
216231
} else if (config->target_filter.type == SFT_EMA3) {
@@ -228,8 +243,18 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f
228243
}
229244

230245
void braketilt_update(
231-
ATR *atr, const MotorData *motor, const RefloatConfig *config, float proportional
246+
ATR *atr,
247+
const MotorData *motor,
248+
const RefloatConfig *config,
249+
bool wheelslip,
250+
float proportional
232251
) {
252+
if (wheelslip) {
253+
atr->braketilt_target_offset *= 0.99;
254+
atr->braketilt_offset = atr->braketilt_target_offset;
255+
return;
256+
}
257+
233258
// braking also should cause setpoint change lift, causing a delayed lingering nose lift
234259
if (atr->braketilt_factor < 0 && motor->braking && motor->abs_erpm > 2000) {
235260
// negative currents alone don't necessarily constitute active braking, look at
@@ -265,10 +290,3 @@ void braketilt_update(
265290

266291
rate_limitf(&atr->braketilt_offset, atr->braketilt_target_offset, braketilt_step_size);
267292
}
268-
269-
void atr_and_braketilt_winddown(ATR *atr) {
270-
atr->offset *= 0.995;
271-
atr->target_offset *= 0.99;
272-
atr->braketilt_offset *= 0.995;
273-
atr->braketilt_target_offset *= 0.99;
274-
}

‎src/atr.h‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,14 @@ void atr_reset(ATR *atr);
4949

5050
void atr_configure(ATR *atr, const RefloatConfig *config);
5151

52-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt);
52+
void atr_update(
53+
ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
54+
);
5355

5456
void braketilt_update(
55-
ATR *atr, const MotorData *motor, const RefloatConfig *config, float proportional
57+
ATR *atr,
58+
const MotorData *motor,
59+
const RefloatConfig *config,
60+
bool wheelslip,
61+
float proportional
5662
);
57-
58-
void atr_and_braketilt_winddown(ATR *atr);

‎src/main.c‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1193,21 +1193,22 @@ static void refloat_thd(void *arg) {
11931193
if (!d->state.darkride) {
11941194
// in case of wheelslip, don't change torque tilts, instead slightly decrease each
11951195
// cycle
1196-
if (d->state.wheelslip) {
1197-
torque_tilt_winddown(&d->torque_tilt);
1198-
atr_and_braketilt_winddown(&d->atr);
1199-
} else {
1196+
if (!d->state.wheelslip) {
12001197
apply_noseangling(d);
12011198
d->setpoint += d->noseangling_interpolated;
12021199

12031200
apply_turntilt(d);
12041201
d->setpoint += d->turntilt_interpolated;
1205-
1206-
torque_tilt_update(&d->torque_tilt, &d->motor, &d->float_conf, d->dt);
1207-
atr_update(&d->atr, &d->motor, &d->float_conf, d->dt);
1208-
braketilt_update(&d->atr, &d->motor, &d->float_conf, d->proportional);
12091202
}
12101203

1204+
torque_tilt_update(
1205+
&d->torque_tilt, &d->motor, &d->float_conf, d->state.wheelslip, d->dt
1206+
);
1207+
atr_update(&d->atr, &d->motor, &d->float_conf, d->state.wheelslip, d->dt);
1208+
braketilt_update(
1209+
&d->atr, &d->motor, &d->float_conf, d->state.wheelslip, d->proportional
1210+
);
1211+
12111212
// aggregated torque tilts:
12121213
// if signs match between torque tilt and ATR + brake tilt, use the more significant
12131214
// one if signs do not match, they are simply added together

‎src/torque_tilt.c‎

Lines changed: 24 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <math.h>
2424

2525
void torque_tilt_reset(TorqueTilt *tt) {
26+
tt->target_offset = 0;
2627
tt->offset = 0;
2728
tt->ramped_step_size = 0;
2829

@@ -49,8 +50,8 @@ void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config) {
4950
);
5051
}
5152

52-
void torque_tilt_update(
53-
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, float dt
53+
static float calculate_torque_tilt_target(
54+
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config
5455
) {
5556
float strength =
5657
motor->braking ? config->torquetilt_strength_regen : config->torquetilt_strength;
@@ -60,7 +61,7 @@ void torque_tilt_update(
6061
// multiply it by "power" to get our desired angle, and min with the limit
6162
// to respect boundaries. Finally multiply it by motor current sign to get
6263
// directionality back.
63-
float target_offset =
64+
tt->target_offset =
6465
fminf(
6566
fmaxf((fabsf(motor->atr_filtered_current) - config->torquetilt_start_current), 0) *
6667
strength,
@@ -69,8 +70,8 @@ void torque_tilt_update(
6970
sign(motor->atr_filtered_current);
7071

7172
float step_size = 0;
72-
if ((tt->offset - target_offset > 0 && target_offset > 0) ||
73-
(tt->offset - target_offset < 0 && target_offset < 0)) {
73+
if ((tt->offset - tt->target_offset > 0 && tt->target_offset > 0) ||
74+
(tt->offset - tt->target_offset < 0 && tt->target_offset < 0)) {
7475
step_size = tt->off_step_size;
7576
} else {
7677
step_size = tt->on_step_size;
@@ -80,20 +81,30 @@ void torque_tilt_update(
8081
step_size /= 2;
8182
}
8283

84+
return step_size;
85+
}
86+
87+
void torque_tilt_update(
88+
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
89+
) {
90+
float step_size = tt->off_step_size;
91+
92+
if (!wheelslip) {
93+
step_size = calculate_torque_tilt_target(tt, motor, config);
94+
} else {
95+
tt->target_offset *= 0.99;
96+
}
97+
8398
if (config->target_filter.tt_type == SFT_NONE) {
84-
rate_limitf(&tt->offset, target_offset, step_size);
99+
rate_limitf(&tt->offset, tt->target_offset, step_size);
85100
} else if (config->target_filter.tt_type == SFT_EMA3) {
86-
ema_filter_update(&tt->ema_target, target_offset, dt);
101+
ema_filter_update(&tt->ema_target, tt->target_offset, dt);
87102
tt->offset = tt->ema_target.value;
88103
} else if (config->target_filter.tt_type == SFT_THREE_STAGE) {
89-
smooth_target_update(&tt->smooth_target, target_offset);
104+
smooth_target_update(&tt->smooth_target, tt->target_offset);
90105
tt->offset = tt->smooth_target.value;
91106
} else {
92107
// Smoothen changes in tilt angle by ramping the step size
93-
smooth_rampf(&tt->offset, &tt->ramped_step_size, target_offset, step_size, 0.04, 1.5);
108+
smooth_rampf(&tt->offset, &tt->ramped_step_size, tt->target_offset, step_size, 0.04, 1.5);
94109
}
95110
}
96-
97-
void torque_tilt_winddown(TorqueTilt *tt) {
98-
tt->offset *= 0.995;
99-
}

‎src/torque_tilt.h‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ typedef struct {
2828
float off_step_size;
2929
float ramped_step_size;
3030

31+
float target_offset; // setpoint target offset
3132
float offset; // rate-limited setpoint offset
3233

3334
SmoothTarget smooth_target;
@@ -39,7 +40,5 @@ void torque_tilt_reset(TorqueTilt *tt);
3940
void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config);
4041

4142
void torque_tilt_update(
42-
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, float dt
43+
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
4344
);
44-
45-
void torque_tilt_winddown(TorqueTilt *tt);

0 commit comments

Comments
 (0)