Skip to content

Commit fc3d704

Browse files
lukashlukash
committed
Add experimental setpoint filters
1 parent 4e44dba commit fc3d704

14 files changed

Lines changed: 561 additions & 42 deletions

‎src/atr.c‎

Lines changed: 29 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,9 @@ void atr_reset(ATR *atr) {
2828
atr->target = 0;
2929
atr->setpoint = 0;
3030
atr->ramped_step_size = 0;
31+
32+
smooth_target_reset(&atr->smooth_target, 0.0f);
33+
ema_filter_reset(&atr->ema_target, 0.0f, 0.0f);
3134
}
3235

3336
void atr_configure(ATR *atr, const RefloatConfig *config) {
@@ -40,9 +43,20 @@ void atr_configure(ATR *atr, const RefloatConfig *config) {
4043
// most +6000 for 100% speed boost
4144
atr->speed_boost_mult = 1.0f / ((fabsf(config->atr_speed_boost) - 0.4f) * 5000 + 3000.0f);
4245
}
46+
47+
smooth_target_configure(
48+
&atr->smooth_target,
49+
&config->target_filter,
50+
config->atr_on_speed,
51+
config->atr_off_speed,
52+
config->hertz
53+
);
54+
ema_filter_configure(
55+
&atr->ema_target, &config->target_filter, config->atr_on_speed, config->atr_off_speed
56+
);
4357
}
4458

45-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
59+
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt) {
4660
float abs_torque = fabsf(motor->filt_current);
4761
float torque_offset = 8; // hard-code to 8A for now (shouldn't really be changed much anyways)
4862
float atr_threshold = motor->braking ? config->atr_threshold_down : config->atr_threshold_up;
@@ -185,8 +199,20 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config) {
185199
atr_step_size /= 2;
186200
}
187201

188-
// Smoothen changes in tilt angle by ramping the step size
189-
smooth_rampf(&atr->setpoint, &atr->ramped_step_size, atr->target, atr_step_size, 0.05, 1.5);
202+
if (config->target_filter.type == SFT_NONE) {
203+
rate_limitf(&atr->setpoint, atr->target, atr_step_size);
204+
} else if (config->target_filter.type == SFT_EMA3) {
205+
ema_filter_update(&atr->ema_target, atr->target, dt);
206+
atr->setpoint = atr->ema_target.value;
207+
} else if (config->target_filter.type == SFT_THREE_STAGE) {
208+
smooth_target_update(&atr->smooth_target, atr->target);
209+
atr->setpoint = atr->smooth_target.value;
210+
} else {
211+
// 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+
);
215+
}
190216
}
191217

192218
void atr_winddown(ATR *atr) {

‎src/atr.h‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,9 @@
1919
#pragma once
2020

2121
#include "conf/datatypes.h"
22+
#include "ema_filter.h"
2223
#include "motor_data.h"
24+
#include "smooth_target.h"
2325

2426
typedef struct {
2527
float on_step_size;
@@ -33,12 +35,15 @@ typedef struct {
3335
float setpoint;
3436

3537
float speed_boost_mult;
38+
39+
SmoothTarget smooth_target;
40+
EMAFilter ema_target;
3641
} ATR;
3742

3843
void atr_reset(ATR *atr);
3944

4045
void atr_configure(ATR *atr, const RefloatConfig *config);
4146

42-
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config);
47+
void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, float dt);
4348

4449
void atr_winddown(ATR *atr);

‎src/conf/datatypes.h‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -179,6 +179,24 @@ typedef struct {
179179
float current_threshold;
180180
} CfgHapticFeedback;
181181

182+
typedef enum {
183+
SFT_NONE = 0,
184+
SFT_THREE_STAGE,
185+
SFT_EMA3,
186+
SFT_NICO
187+
} TargetFilterType;
188+
189+
typedef struct {
190+
TargetFilterType type;
191+
TargetFilterType tt_type;
192+
TargetFilterType it_type;
193+
float alpha;
194+
float in_alpha_away;
195+
float in_alpha_back;
196+
float ema_half_time;
197+
float ema_return_multiplier;
198+
} CfgTargetFilter;
199+
182200
typedef struct {
183201
bool is_default;
184202
} CfgMeta;
@@ -192,6 +210,7 @@ typedef struct {
192210
float mahony_kp_roll;
193211
float kp_brake;
194212
float kp2_brake;
213+
CfgTargetFilter target_filter;
195214
uint16_t hertz;
196215
float fault_pitch;
197216
float fault_roll;

‎src/conf/settings.xml‎

Lines changed: 214 additions & 8 deletions
Large diffs are not rendered by default.

‎src/data.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,7 @@ typedef struct {
9999
float setpoint, setpoint_target, setpoint_target_interpolated;
100100
float noseangling_interpolated, inputtilt_interpolated;
101101
time_t nag_timer;
102+
float dt;
102103
float idle_voltage;
103104
time_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer,
104105
fault_switch_half_timer;

‎src/ema_filter.c‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
// Copyright 2024 Lukas Hrazky
2+
//
3+
// This file is part of the Refloat VESC package.
4+
//
5+
// Refloat VESC package is free software: you can redistribute it and/or modify
6+
// it under the terms of the GNU General Public License as published by the
7+
// Free Software Foundation, either version 3 of the License, or (at your
8+
// option) any later version.
9+
//
10+
// Refloat VESC package is distributed in the hope that it will be useful, but
11+
// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12+
// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
13+
// more details.
14+
//
15+
// You should have received a copy of the GNU General Public License along with
16+
// this program. If not, see <http://www.gnu.org/licenses/>.
17+
18+
#include "ema_filter.h"
19+
20+
#include "utils.h"
21+
22+
#include <math.h>
23+
24+
void ema_filter_configure(
25+
EMAFilter *filter, const CfgTargetFilter *cfg, float on_speed, float off_speed
26+
) {
27+
filter->cfg = *cfg;
28+
filter->k = 0.693f / max(cfg->ema_half_time, 0.001f);
29+
filter->on_speed = on_speed;
30+
filter->off_speed = off_speed;
31+
}
32+
33+
void ema_filter_reset(EMAFilter *filter, float value, float speed) {
34+
filter->value = value;
35+
filter->speed = speed;
36+
filter->accel = 0.0f;
37+
}
38+
39+
void ema_filter_update(EMAFilter *filter, float target, float dt) {
40+
float k = filter->k;
41+
if (fabsf(target) < fabsf(filter->value)) {
42+
k *= filter->cfg.ema_return_multiplier;
43+
}
44+
45+
// Coefficients chosen to approximate Gaussian filter and preserve half time
46+
float speed = 1.34f * k * (target - filter->value);
47+
float accel = 3.62f * k * (speed - filter->speed);
48+
float jerk = 9.77f * k * (accel - filter->accel);
49+
50+
filter->accel += dt * jerk;
51+
filter->speed += dt * filter->accel;
52+
53+
uint16_t speed_limit;
54+
if (sign(filter->speed) == sign(filter->value)) {
55+
speed_limit = filter->on_speed;
56+
} else {
57+
speed_limit = filter->off_speed;
58+
}
59+
filter->value += dt * sign(filter->speed) * min(fabsf(filter->speed), speed_limit);
60+
}

‎src/ema_filter.h‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
// Copyright 2024 Lukas Hrazky
2+
//
3+
// This file is part of the Refloat VESC package.
4+
//
5+
// Refloat VESC package is free software: you can redistribute it and/or modify
6+
// it under the terms of the GNU General Public License as published by the
7+
// Free Software Foundation, either version 3 of the License, or (at your
8+
// option) any later version.
9+
//
10+
// Refloat VESC package is distributed in the hope that it will be useful, but
11+
// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12+
// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
13+
// more details.
14+
//
15+
// You should have received a copy of the GNU General Public License along with
16+
// this program. If not, see <http://www.gnu.org/licenses/>.
17+
18+
#pragma once
19+
20+
#include "conf/datatypes.h"
21+
22+
typedef struct {
23+
CfgTargetFilter cfg;
24+
float on_speed;
25+
float off_speed;
26+
27+
float value;
28+
float speed;
29+
float accel;
30+
float k;
31+
float dt;
32+
} EMAFilter;
33+
34+
void ema_filter_configure(
35+
EMAFilter *filter, const CfgTargetFilter *cfg, float on_speed, float off_speed
36+
);
37+
38+
void ema_filter_reset(EMAFilter *filter, float value, float speed);
39+
40+
void ema_filter_update(EMAFilter *filter, float target, float dt);

‎src/main.c‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,8 @@ static void configure(Data *d) {
171171

172172
lcm_configure(&d->lcm, &d->float_conf.leds);
173173

174+
d->dt = 1.0f / d->float_conf.hertz;
175+
174176
// Loop time in microseconds
175177
d->loop_time_us = 1e6 / d->float_conf.hertz;
176178

@@ -794,7 +796,7 @@ static void refloat_thd(void *arg) {
794796
);
795797
d->setpoint = d->setpoint_target_interpolated;
796798

797-
remote_update(&d->remote, &d->state, &d->float_conf);
799+
remote_update(&d->remote, &d->state, &d->float_conf, d->dt);
798800
d->setpoint += d->remote.setpoint;
799801

800802
if (!d->state.darkride) {
@@ -818,8 +820,8 @@ static void refloat_thd(void *arg) {
818820
);
819821
d->setpoint += d->turn_tilt.setpoint;
820822

821-
torque_tilt_update(&d->torque_tilt, &d->motor, &d->float_conf);
822-
atr_update(&d->atr, &d->motor, &d->float_conf);
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);
823825
brake_tilt_update(
824826
&d->brake_tilt,
825827
&d->motor,

‎src/remote.c‎

Lines changed: 43 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,19 @@ void remote_init(Remote *remote) {
2727

2828
void remote_configure(Remote *remote, const RefloatConfig *config) {
2929
remote->step_size = config->inputtilt_speed / config->hertz;
30+
smooth_target_configure(
31+
&remote->smooth_target,
32+
&config->target_filter,
33+
config->inputtilt_speed,
34+
config->inputtilt_speed,
35+
config->hertz
36+
);
37+
ema_filter_configure(
38+
&remote->ema_target,
39+
&config->target_filter,
40+
config->inputtilt_speed,
41+
config->inputtilt_speed
42+
);
3043
}
3144

3245
void remote_input(Remote *remote, const RefloatConfig *config) {
@@ -67,37 +80,45 @@ void remote_input(Remote *remote, const RefloatConfig *config) {
6780
remote->input = value;
6881
}
6982

70-
void remote_update(Remote *remote, const State *state, const RefloatConfig *config) {
83+
void remote_update(Remote *remote, const State *state, const RefloatConfig *config, float dt) {
7184
float target = remote->input * config->inputtilt_angle_limit;
7285

7386
if (state->darkride) {
7487
target = -target;
7588
}
7689

77-
float target_diff = target - remote->setpoint;
90+
if (config->target_filter.it_type == SFT_EMA3) {
91+
ema_filter_update(&remote->ema_target, target, dt);
92+
remote->setpoint = remote->ema_target.value;
93+
} else if (config->target_filter.it_type == SFT_THREE_STAGE) {
94+
smooth_target_update(&remote->smooth_target, target);
95+
remote->setpoint = remote->smooth_target.value;
96+
} else {
97+
float target_diff = target - remote->setpoint;
7898

79-
// Smoothen changes in tilt angle by ramping the step size
80-
const float smoothing_factor = 0.02;
99+
// Smoothen changes in tilt angle by ramping the step size
100+
const float smoothing_factor = 0.02;
81101

82-
// Within X degrees of Target Angle, start ramping down step size
83-
if (fabsf(target_diff) < 2.0f) {
84-
// Target step size is reduced the closer to center you are (needed for smoothly
85-
// transitioning away from center)
86-
remote->ramped_step_size = smoothing_factor * remote->step_size * target_diff / 2 +
87-
(1 - smoothing_factor) * remote->ramped_step_size;
88-
// Linearly ramped down step size is provided as minimum to prevent overshoot
89-
float centering_step_size =
90-
fminf(fabsf(remote->ramped_step_size), fabsf(target_diff / 2) * remote->step_size) *
91-
sign(target_diff);
92-
if (fabsf(target_diff) < fabsf(centering_step_size)) {
93-
remote->setpoint = target;
102+
// Within X degrees of Target Angle, start ramping down step size
103+
if (fabsf(target_diff) < 2.0f) {
104+
// Target step size is reduced the closer to center you are (needed for smoothly
105+
// transitioning away from center)
106+
remote->ramped_step_size = smoothing_factor * remote->step_size * target_diff / 2 +
107+
(1 - smoothing_factor) * remote->ramped_step_size;
108+
// Linearly ramped down step size is provided as minimum to prevent overshoot
109+
float centering_step_size =
110+
fminf(fabsf(remote->ramped_step_size), fabsf(target_diff / 2) * remote->step_size) *
111+
sign(target_diff);
112+
if (fabsf(target_diff) < fabsf(centering_step_size)) {
113+
remote->setpoint = target;
114+
} else {
115+
remote->setpoint += centering_step_size;
116+
}
94117
} else {
95-
remote->setpoint += centering_step_size;
118+
// Ramp up step size until the configured tilt speed is reached
119+
remote->ramped_step_size = smoothing_factor * remote->step_size * sign(target_diff) +
120+
(1 - smoothing_factor) * remote->ramped_step_size;
121+
remote->setpoint += remote->ramped_step_size;
96122
}
97-
} else {
98-
// Ramp up step size until the configured tilt speed is reached
99-
remote->ramped_step_size = smoothing_factor * remote->step_size * sign(target_diff) +
100-
(1 - smoothing_factor) * remote->ramped_step_size;
101-
remote->setpoint += remote->ramped_step_size;
102123
}
103124
}

‎src/remote.h‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,17 @@
1818
#pragma once
1919

2020
#include "conf/datatypes.h"
21+
#include "ema_filter.h"
22+
#include "smooth_target.h"
2123
#include "state.h"
2224

2325
typedef struct {
2426
float step_size;
2527

2628
float input;
2729
float ramped_step_size;
30+
SmoothTarget smooth_target;
31+
EMAFilter ema_target;
2832

2933
float setpoint;
3034
} Remote;
@@ -35,4 +39,4 @@ void remote_configure(Remote *remote, const RefloatConfig *config);
3539

3640
void remote_input(Remote *remote, const RefloatConfig *config);
3741

38-
void remote_update(Remote *remote, const State *state, const RefloatConfig *config);
42+
void remote_update(Remote *remote, const State *state, const RefloatConfig *config, float dt);

0 commit comments

Comments
 (0)