Skip to content

Commit 1860683

Browse files
lukashlukash
committed
Add new ATR Transition Boost
Feature: New ATR Transition Boost > The ATR Transition Boost feature didn't fit the new Setpoint Smoothing algorithm. It was reworked and the multiplier now applies to the smoothing constants as well as the angling rates.
1 parent 1eb2346 commit 1860683

13 files changed

Lines changed: 48 additions & 22 deletions

File tree

‎src/atr.c‎

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ void atr_init(ATR *atr) {
2828
atr->ad_alpha2 = 0.0f;
2929
atr->ad_alpha3 = 0.0f;
3030

31+
ema_init(&atr->transition_target);
3132
smooth_setpoint_init(&atr->setpoint);
3233

3334
atr_reset(atr);
@@ -38,6 +39,8 @@ void atr_reset(ATR *atr) {
3839
atr->speed_boost = 0.0f;
3940

4041
atr->target = 0.0f;
42+
ema_reset(&atr->transition_target, 0.0f);
43+
atr->transition_boost = 1.0f;
4144
smooth_setpoint_reset(&atr->setpoint);
4245
}
4346

@@ -53,6 +56,7 @@ void atr_configure(ATR *atr, const RefloatConfig *config, float frequency) {
5356
atr->ad_alpha2 = ema_calculate_alpha(6.0f, frequency);
5457
atr->ad_alpha1 = ema_calculate_alpha(1.0f, frequency);
5558

59+
ema_configure(&atr->transition_target, 6.0f, frequency);
5660
smooth_setpoint_configure(
5761
&atr->setpoint,
5862
config->atr.filter.time_constant,
@@ -139,8 +143,26 @@ void atr_update(
139143
) {
140144
if (!wheelslip) {
141145
calculate_atr_target(atr, motor, config);
142-
smooth_setpoint_update(&atr->setpoint, atr->target, motor->forward, dt);
146+
147+
ema_update(&atr->transition_target, atr->target);
148+
149+
float transition_target = atr->transition_target.value;
150+
float degrees_diff = fabsf(atr->setpoint.value - transition_target) - 1.0f;
151+
// Only apply transition boost if the setpoint and target differ in
152+
// signs and the degree diff is greater than 1
153+
if (atr->setpoint.value * transition_target < 0 && degrees_diff > 0.0f) {
154+
// Scale the transition multiplier linearly from 1 to 2 degrees of difference
155+
atr->transition_boost =
156+
1.0f + min(degrees_diff, 1.0f) * (config->atr.transition_boost - 1.0f);
157+
} else {
158+
atr->transition_boost = 1.0f;
159+
}
160+
161+
smooth_setpoint_update(
162+
&atr->setpoint, atr->target, motor->forward, atr->transition_boost, dt
163+
);
143164
} else {
144165
smooth_setpoint_winddown(&atr->setpoint);
166+
ema_reset(&atr->transition_target, atr->setpoint.value);
145167
}
146168
}

‎src/atr.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,9 @@ typedef struct {
3434

3535
float target;
3636

37+
EMA transition_target;
38+
float transition_boost;
39+
3740
SmoothSetpoint setpoint;
3841
} ATR;
3942

‎src/brake_tilt.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ void brake_tilt_update(
9090
bt->target = 0;
9191
}
9292

93-
smooth_setpoint_update(&bt->setpoint, bt->target, motor->forward, dt);
93+
smooth_setpoint_update(&bt->setpoint, bt->target, motor->forward, 1.0f, dt);
9494
} else {
9595
smooth_setpoint_winddown(&bt->setpoint);
9696
}

‎src/conf/datatypes.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -203,6 +203,7 @@ typedef struct {
203203

204204
typedef struct {
205205
CfgSetpointFilter filter;
206+
float transition_boost;
206207
float wheelslip_winddown_time_constant;
207208
} CfgATR;
208209

@@ -312,7 +313,6 @@ typedef struct {
312313
float atr_on_speed_downhill;
313314
float atr_off_speed_downhill;
314315
float atr_response_boost;
315-
float atr_transition_boost;
316316
float atr_filter;
317317
float atr_amps_accel_ratio;
318318
float atr_amps_decel_ratio;

‎src/conf/settings.xml‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2404,7 +2404,7 @@ p, li { white-space: pre-wrap; }
24042404
<suffix>x</suffix>
24052405
<vTx>7</vTx>
24062406
</atr_response_boost>
2407-
<atr_transition_boost>
2407+
<atr.transition_boost>
24082408
<longName>Tiltback Transition Boost</longName>
24092409
<type>1</type>
24102410
<transmittable>1</transmittable>
@@ -2417,15 +2417,15 @@ p, li { white-space: pre-wrap; }
24172417
<editorDecimalsDouble>1</editorDecimalsDouble>
24182418
<editorScale>1</editorScale>
24192419
<editAsPercentage>0</editAsPercentage>
2420-
<maxDouble>10</maxDouble>
2420+
<maxDouble>4</maxDouble>
24212421
<minDouble>1</minDouble>
24222422
<showDisplay>0</showDisplay>
24232423
<stepDouble>0.5</stepDouble>
2424-
<valDouble>3</valDouble>
2424+
<valDouble>2</valDouble>
24252425
<vTxDoubleScale>1000</vTxDoubleScale>
24262426
<suffix>x</suffix>
24272427
<vTx>7</vTx>
2428-
</atr_transition_boost>
2428+
</atr.transition_boost>
24292429
<atr_filter>
24302430
<longName>Current Filter</longName>
24312431
<type>1</type>
@@ -4427,7 +4427,7 @@ p, li { white-space: pre-wrap; }
44274427
<ser>atr_on_speed_downhill</ser>
44284428
<ser>atr_off_speed_downhill</ser>
44294429
<ser>atr_response_boost</ser>
4430-
<ser>atr_transition_boost</ser>
4430+
<ser>atr.transition_boost</ser>
44314431
<ser>atr_filter</ser>
44324432
<ser>atr_amps_accel_ratio</ser>
44334433
<ser>atr_amps_decel_ratio</ser>
@@ -4591,7 +4591,7 @@ p, li { white-space: pre-wrap; }
45914591
<param>atr_on_speed_downhill</param>
45924592
<param>atr_off_speed_downhill</param>
45934593
<param>atr_response_boost</param>
4594-
<param>atr_transition_boost</param>
4594+
<param>atr.transition_boost</param>
45954595
<param>::sep::Advanced</param>
45964596
<param>atr_amps_accel_ratio</param>
45974597
<param>atr_amps_decel_ratio</param>

‎src/filters/smooth_setpoint.c‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ void smooth_setpoint_reset(SmoothSetpoint *st) {
6868
st->value = 0.0f;
6969
}
7070

71-
void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, float dt) {
71+
void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, float mult, float dt) {
7272
if (st->is_winddown) {
7373
st->is_winddown = false;
7474
st->v1 = st->value;
@@ -77,14 +77,14 @@ void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, floa
7777

7878
bool is_up = (st->value >= 0.0f) == forward;
7979

80-
st->v1 += st->alpha * (target - st->v1);
81-
float delta = st->alpha * (st->v1 - st->value);
80+
st->v1 += mult * st->alpha * (target - st->v1);
81+
float delta = mult * st->alpha * (st->v1 - st->value);
8282

8383
if (fabsf(delta) > fabsf(st->step) || sign(delta) != sign(st->step)) {
8484
if (sign(st->value) == sign(delta)) {
85-
st->step += st->on_speed_alpha * (delta - st->step);
85+
st->step += mult * st->on_speed_alpha * (delta - st->step);
8686
} else {
87-
st->step += st->off_speed_alpha * (delta - st->step);
87+
st->step += mult * st->off_speed_alpha * (delta - st->step);
8888
}
8989
} else {
9090
st->step = delta;
@@ -101,7 +101,7 @@ void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, floa
101101
speed_limit = on_speed;
102102
}
103103

104-
st->value += sign(st->step) * min(fabsf(st->step), speed_limit * dt);
104+
st->value += sign(st->step) * min(fabsf(st->step), mult * speed_limit * dt);
105105
}
106106

107107
void smooth_setpoint_winddown(SmoothSetpoint *st) {

‎src/filters/smooth_setpoint.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,6 @@ void smooth_setpoint_configure(
5454

5555
void smooth_setpoint_reset(SmoothSetpoint *st);
5656

57-
void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, float dt);
57+
void smooth_setpoint_update(SmoothSetpoint *st, float target, bool forward, float mult, float dt);
5858

5959
void smooth_setpoint_winddown(SmoothSetpoint *st);

‎src/main.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1591,7 +1591,7 @@ static void cmd_runtime_tune(Data *d, unsigned char *cfg, int len) {
15911591

15921592
split(cfg[8], &h1, &h2);
15931593
d->float_conf.atr_response_boost = ((float) h1) / 10 + 1;
1594-
d->float_conf.atr_transition_boost = ((float) h2) / 5 + 1;
1594+
d->float_conf.atr.transition_boost = ((float) h2) / 5 + 1;
15951595

15961596
split(cfg[9], &h1, &h2);
15971597
d->float_conf.atr_amps_accel_ratio = h1 + 5;
@@ -1669,7 +1669,7 @@ static void cmd_tune_defaults(Data *d) {
16691669
d->float_conf.atr_on_speed = CFG_DFLT_ATR_ON_SPEED;
16701670
d->float_conf.atr_off_speed = CFG_DFLT_ATR_OFF_SPEED;
16711671
d->float_conf.atr_response_boost = CFG_DFLT_ATR_RESPONSE_BOOST;
1672-
d->float_conf.atr_transition_boost = CFG_DFLT_ATR_TRANSITION_BOOST;
1672+
d->float_conf.atr.transition_boost = CFG_DFLT_ATR_TRANSITION_BOOST;
16731673
d->float_conf.atr_filter = CFG_DFLT_ATR_FILTER;
16741674
d->float_conf.atr_amps_accel_ratio = CFG_DFLT_ATR_AMPS_ACCEL_RATIO;
16751675
d->float_conf.atr_amps_decel_ratio = CFG_DFLT_ATR_AMPS_DECEL_RATIO;

‎src/remote.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,5 +91,5 @@ void remote_update(Remote *remote, const State *state, const RefloatConfig *conf
9191
}
9292

9393
// The `forward` argument doesn't matter, as up and down speeds are the same
94-
smooth_setpoint_update(&remote->setpoint, target, true, dt);
94+
smooth_setpoint_update(&remote->setpoint, target, true, 1.0f, dt);
9595
}

‎src/rt_data.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@
7070
R(balance_current.value, "balance_current") \
7171
S(atr.accel_diff, "atr.accel_diff") \
7272
S(atr.speed_boost, "atr.speed_boost") \
73+
R(atr.transition_boost, "atr.transition_boost") \
7374
S(booster.torque.value, "booster.torque")
7475

7576
#define RT_DATA_ALL_ITEMS(S, R) RT_DATA_ITEMS(S, R) RT_DATA_RUNTIME_ITEMS(S, R)

0 commit comments

Comments
 (0)