diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h
index 39a85ea0..5a95b7d5 100644
--- a/src/conf/datatypes.h
+++ b/src/conf/datatypes.h
@@ -199,6 +199,12 @@ typedef struct {
int8_t bms_ht_threshold;
} CfgBMS;
+typedef struct {
+ float alpha;
+ float in_alpha_away;
+ float in_alpha_back;
+} CfgTargetFilter;
+
typedef struct {
bool is_default;
} CfgMeta;
diff --git a/src/data.h b/src/data.h
index cbdf1b11..819ee0ec 100644
--- a/src/data.h
+++ b/src/data.h
@@ -104,6 +104,7 @@ typedef struct {
float noseangling_interpolated;
time_t alert_timer;
time_t nag_timer;
+ float dt;
float idle_voltage;
time_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer,
fault_switch_half_timer;
diff --git a/src/main.c b/src/main.c
index 4ba4f000..9bb7e33c 100644
--- a/src/main.c
+++ b/src/main.c
@@ -186,6 +186,8 @@ static void configure(Data *d) {
lcm_configure(&d->lcm, &d->float_conf.leds);
+ d->dt = 1.0f / d->float_conf.hertz;
+
// Loop time in microseconds
d->loop_time_us = 1e6 / d->float_conf.hertz;
@@ -874,7 +876,7 @@ static void refloat_thd(void *arg) {
);
d->setpoint = d->setpoint_target_interpolated;
- remote_update(&d->remote, &d->state, &d->float_conf);
+ remote_update(&d->remote, &d->state, &d->float_conf, d->dt);
d->setpoint += d->remote.setpoint;
if (!d->state.darkride) {
diff --git a/src/remote.c b/src/remote.c
index 578323df..96c3739e 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -28,10 +28,25 @@ void remote_init(Remote *remote) {
void remote_reset(Remote *remote) {
remote->setpoint = 0;
remote->ramped_step_size = 0;
+
+ smooth_target_reset(&remote->smooth_target, 0.0f);
}
void remote_configure(Remote *remote, const RefloatConfig *config) {
remote->step_size = config->inputtilt_speed / config->hertz;
+
+ // Hardcoded target filter for remote
+ static const CfgTargetFilter remote_target_filter = {
+ .alpha = 0.020, .in_alpha_away = 0.050, .in_alpha_back = 0.015
+ };
+
+ smooth_target_configure(
+ &remote->smooth_target,
+ &remote_target_filter,
+ config->inputtilt_speed,
+ config->inputtilt_speed,
+ config->hertz
+ );
}
void remote_input(Remote *remote, const RefloatConfig *config) {
@@ -72,37 +87,13 @@ void remote_input(Remote *remote, const RefloatConfig *config) {
remote->input = value;
}
-void remote_update(Remote *remote, const State *state, const RefloatConfig *config) {
+void remote_update(Remote *remote, const State *state, const RefloatConfig *config, float dt) {
float target = remote->input * config->inputtilt_angle_limit;
if (state->darkride) {
target = -target;
}
- float target_diff = target - remote->setpoint;
-
- // Smoothen changes in tilt angle by ramping the step size
- const float smoothing_factor = 0.02;
-
- // Within X degrees of Target Angle, start ramping down step size
- if (fabsf(target_diff) < 2.0f) {
- // Target step size is reduced the closer to center you are (needed for smoothly
- // transitioning away from center)
- remote->ramped_step_size = smoothing_factor * remote->step_size * target_diff / 2 +
- (1 - smoothing_factor) * remote->ramped_step_size;
- // Linearly ramped down step size is provided as minimum to prevent overshoot
- float centering_step_size =
- fminf(fabsf(remote->ramped_step_size), fabsf(target_diff / 2) * remote->step_size) *
- sign(target_diff);
- if (fabsf(target_diff) < fabsf(centering_step_size)) {
- remote->setpoint = target;
- } else {
- remote->setpoint += centering_step_size;
- }
- } else {
- // Ramp up step size until the configured tilt speed is reached
- remote->ramped_step_size = smoothing_factor * remote->step_size * sign(target_diff) +
- (1 - smoothing_factor) * remote->ramped_step_size;
- remote->setpoint += remote->ramped_step_size;
- }
+ smooth_target_update(&remote->smooth_target, target);
+ remote->setpoint = remote->smooth_target.value;
}
diff --git a/src/remote.h b/src/remote.h
index 00926f12..efd89776 100644
--- a/src/remote.h
+++ b/src/remote.h
@@ -18,6 +18,7 @@
#pragma once
#include "conf/datatypes.h"
+#include "smooth_target.h"
#include "state.h"
typedef struct {
@@ -25,6 +26,7 @@ typedef struct {
float input;
float ramped_step_size;
+ SmoothTarget smooth_target;
float setpoint;
} Remote;
@@ -37,4 +39,4 @@ void remote_configure(Remote *remote, const RefloatConfig *config);
void remote_input(Remote *remote, const RefloatConfig *config);
-void remote_update(Remote *remote, const State *state, const RefloatConfig *config);
+void remote_update(Remote *remote, const State *state, const RefloatConfig *config, float dt);
diff --git a/src/smooth_target.c b/src/smooth_target.c
new file mode 100644
index 00000000..a54c1368
--- /dev/null
+++ b/src/smooth_target.c
@@ -0,0 +1,61 @@
+// Copyright 2024 Lukas Hrazky
+//
+// This file is part of the Refloat VESC package.
+//
+// Refloat VESC package is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or (at your
+// option) any later version.
+//
+// Refloat VESC package is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+// more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see .
+
+#include "smooth_target.h"
+
+#include "utils.h"
+
+#include
+
+void smooth_target_configure(
+ SmoothTarget *st, const CfgTargetFilter *cfg, float on_speed, float off_speed, float hertz
+) {
+ st->cfg = *cfg;
+ st->on_speed = on_speed / hertz;
+ st->off_speed = off_speed / hertz;
+}
+
+void smooth_target_reset(SmoothTarget *st, float value) {
+ st->v1 = 0;
+ st->step = 0;
+ st->value = value;
+}
+
+void smooth_target_update(SmoothTarget *st, float target) {
+ st->v1 += st->cfg.alpha * (target - st->v1);
+
+ float delta = st->cfg.alpha * (st->v1 - st->value);
+
+ if (fabsf(delta) > fabsf(st->step) || sign(delta) != sign(st->step)) {
+ if (sign(st->value) == sign(delta)) {
+ st->step += st->cfg.in_alpha_away * (delta - st->step);
+ } else {
+ st->step += st->cfg.in_alpha_back * (delta - st->step);
+ }
+ } else {
+ st->step = delta;
+ }
+
+ float speed_limit;
+ if (sign(st->step) == sign(st->value)) {
+ speed_limit = st->on_speed;
+ } else {
+ speed_limit = st->off_speed;
+ }
+
+ st->value += sign(st->step) * min(fabsf(st->step), speed_limit);
+}
diff --git a/src/smooth_target.h b/src/smooth_target.h
new file mode 100644
index 00000000..50eb30e0
--- /dev/null
+++ b/src/smooth_target.h
@@ -0,0 +1,38 @@
+// Copyright 2024 Lukas Hrazky
+//
+// This file is part of the Refloat VESC package.
+//
+// Refloat VESC package is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or (at your
+// option) any later version.
+//
+// Refloat VESC package is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+// or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+// more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see .
+
+#pragma once
+
+#include "conf/datatypes.h"
+
+typedef struct {
+ CfgTargetFilter cfg;
+ float on_speed;
+ float off_speed;
+
+ float v1;
+ float step;
+ float value;
+} SmoothTarget;
+
+void smooth_target_configure(
+ SmoothTarget *st, const CfgTargetFilter *cfg, float on_speed, float off_speed, float hertz
+);
+
+void smooth_target_reset(SmoothTarget *st, float value);
+
+void smooth_target_update(SmoothTarget *st, float target);