Skip to content

Commit a05f591

Browse files
lukashlukash
committed
Fix ATR/TT winddown with filtering
1 parent 1746b29 commit a05f591

File tree

7 files changed

+68
-49
lines changed

7 files changed

+68
-49
lines changed

‎src/atr.c‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ void atr_configure(ATR *atr, const RefloatConfig *config) {
5656
);
5757
}
5858

59-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt) {
59+
static float calculate_atr_target(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
6060
float abs_torque = fabsf(motor->filt_current);
6161
float torque_offset = 8; // hard-code to 8A for now (shouldn't really be changed much anyways)
6262
float atr_threshold = motor->braking ? config->atr_threshold_down : config->atr_threshold_up;
@@ -199,6 +199,21 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f
199199
atr_step_size /= 2;
200200
}
201201

202+
return atr_step_size;
203+
}
204+
205+
void atr_update(
206+
ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
207+
) {
208+
float atr_step_size = 0;
209+
210+
if (!wheelslip) {
211+
atr_step_size = calculate_atr_target(atr, motor, config);
212+
} else {
213+
atr->target *= 0.99;
214+
atr_step_size = atr->off_step_size;
215+
}
216+
202217
if (config->target_filter.type == SFT_NONE) {
203218
rate_limitf(&atr->setpoint, atr->target, atr_step_size);
204219
} else if (config->target_filter.type == SFT_EMA3) {
@@ -209,13 +224,6 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f
209224
atr->setpoint = atr->smooth_target.value;
210225
} else {
211226
// Smoothen changes in tilt angle by ramping the step size
212-
smooth_rampf(
213-
&atr->setpoint, &atr->ramped_step_size, atr->target, atr_step_size, 0.05, 1.5
214-
);
227+
smooth_rampf(&atr->setpoint, &atr->ramped_step_size, atr->target, atr_step_size, 0.05, 1.5);
215228
}
216229
}
217-
218-
void atr_winddown(ATR *atr) {
219-
atr->setpoint *= 0.995;
220-
atr->target *= 0.99;
221-
}

‎src/atr.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,6 @@ void atr_reset(ATR *atr);
4444

4545
void atr_configure(ATR *atr, const RefloatConfig *config);
4646

47-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt);
48-
49-
void atr_winddown(ATR *atr);
47+
void atr_update(
48+
ATR *atr, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
49+
);

‎src/brake_tilt.c‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,15 @@ void brake_tilt_update(
4141
const MotorData *motor,
4242
const ATR *atr,
4343
const RefloatConfig *config,
44+
bool wheelslip,
4445
float balance_offset
4546
) {
47+
if (wheelslip) {
48+
bt->target *= 0.99;
49+
bt->setpoint = bt->target;
50+
return;
51+
}
52+
4653
// braking also should cause setpoint change lift, causing a delayed lingering nose lift
4754
if (bt->factor < 0 && motor->braking && motor->abs_erpm > 2000) {
4855
// negative currents alone don't necessarily constitute active braking, look at
@@ -78,8 +85,3 @@ void brake_tilt_update(
7885

7986
rate_limitf(&bt->setpoint, bt->target, braketilt_step_size);
8087
}
81-
82-
void brake_tilt_winddown(BrakeTilt *bt) {
83-
bt->setpoint *= 0.995;
84-
bt->target *= 0.99;
85-
}

‎src/brake_tilt.h‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,6 @@ void brake_tilt_update(
3737
const MotorData *motor,
3838
const ATR *atr,
3939
const RefloatConfig *config,
40+
bool wheelslip,
4041
float balance_offset
4142
);
42-
43-
void brake_tilt_winddown(BrakeTilt *bt);

‎src/main.c‎

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -802,11 +802,7 @@ static void refloat_thd(void *arg) {
802802
if (!d->state.darkride) {
803803
// in case of wheelslip, don't change torque tilts, instead slightly decrease each
804804
// cycle
805-
if (d->state.wheelslip) {
806-
torque_tilt_winddown(&d->torque_tilt);
807-
atr_winddown(&d->atr);
808-
brake_tilt_winddown(&d->brake_tilt);
809-
} else {
805+
if (!d->state.wheelslip) {
810806
apply_noseangling(d);
811807
d->setpoint += d->noseangling_interpolated;
812808

@@ -819,18 +815,21 @@ static void refloat_thd(void *arg) {
819815
&d->float_conf
820816
);
821817
d->setpoint += d->turn_tilt.setpoint;
822-
823-
torque_tilt_update(&d->torque_tilt, &d->motor, &d->float_conf, d->dt);
824-
atr_update(&d->atr, &d->motor, &d->float_conf, d->dt);
825-
brake_tilt_update(
826-
&d->brake_tilt,
827-
&d->motor,
828-
&d->atr,
829-
&d->float_conf,
830-
d->setpoint - d->imu.balance_pitch
831-
);
832818
}
833819

820+
torque_tilt_update(
821+
&d->torque_tilt, &d->motor, &d->float_conf, d->state.wheelslip, d->dt
822+
);
823+
atr_update(&d->atr, &d->motor, &d->float_conf, d->state.wheelslip, d->dt);
824+
brake_tilt_update(
825+
&d->brake_tilt,
826+
&d->motor,
827+
&d->atr,
828+
&d->float_conf,
829+
d->state.wheelslip,
830+
d->setpoint - d->imu.balance_pitch
831+
);
832+
834833
// aggregated torque tilts:
835834
// if signs match between torque tilt and ATR + brake tilt, use the more significant
836835
// one if signs do not match, they are simply added together

‎src/torque_tilt.c‎

Lines changed: 24 additions & 12 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 = 0;
2627
tt->setpoint = 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,15 +61,16 @@ 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 =
64+
tt->target =
6465
fminf(
6566
fmaxf((fabsf(motor->filt_current) - config->torquetilt_start_current), 0) * strength,
6667
config->torquetilt_angle_limit
6768
) *
6869
sign(motor->filt_current);
6970

7071
float step_size = 0;
71-
if ((tt->setpoint - target > 0 && target > 0) || (tt->setpoint - target < 0 && target < 0)) {
72+
if ((tt->setpoint - tt->target > 0 && tt->target > 0) ||
73+
(tt->setpoint - tt->target < 0 && tt->target < 0)) {
7274
step_size = tt->off_step_size;
7375
} else {
7476
step_size = tt->on_step_size;
@@ -78,20 +80,30 @@ void torque_tilt_update(
7880
step_size /= 2;
7981
}
8082

83+
return step_size;
84+
}
85+
86+
void torque_tilt_update(
87+
TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config, bool wheelslip, float dt
88+
) {
89+
float step_size = tt->off_step_size;
90+
91+
if (!wheelslip) {
92+
step_size = calculate_torque_tilt_target(tt, motor, config);
93+
} else {
94+
tt->target *= 0.99;
95+
}
96+
8197
if (config->target_filter.tt_type == SFT_NONE) {
82-
rate_limitf(&tt->setpoint, target, step_size);
98+
rate_limitf(&tt->setpoint, tt->target, step_size);
8399
} else if (config->target_filter.tt_type == SFT_EMA3) {
84-
ema_filter_update(&tt->ema_target, target, dt);
100+
ema_filter_update(&tt->ema_target, tt->target, dt);
85101
tt->setpoint = tt->ema_target.value;
86102
} else if (config->target_filter.tt_type == SFT_THREE_STAGE) {
87-
smooth_target_update(&tt->smooth_target, target);
103+
smooth_target_update(&tt->smooth_target, tt->target);
88104
tt->setpoint = tt->smooth_target.value;
89105
} else {
90106
// Smoothen changes in tilt angle by ramping the step size
91-
smooth_rampf(&tt->setpoint, &tt->ramped_step_size, target, step_size, 0.04, 1.5);
107+
smooth_rampf(&tt->setpoint, &tt->ramped_step_size, tt->target, step_size, 0.04, 1.5);
92108
}
93109
}
94-
95-
void torque_tilt_winddown(TorqueTilt *tt) {
96-
tt->setpoint *= 0.995;
97-
}

‎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;
3132
float setpoint;
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)