Implement 3-Stage Smoothing for Remote Tilt (Hardcoded)

Feature: Add 3-Stage Smoothing for Remote Tilt;
 >
 Parameters are hard-coded to serve remotes well

Author: NicoAleman

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;

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;

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) {

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;
 }

src/remote.h

@@ -18,13 +18,15 @@
 #pragma once
 
 #include "conf/datatypes.h"
+#include "smooth_target.h"
 #include "state.h"
 
 typedef struct {
     float step_size;
 
     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);

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 <http://www.gnu.org/licenses/>.
+
+#include "smooth_target.h"
+
+#include "utils.h"
+
+#include <math.h>
+
+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);
+}

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 <http://www.gnu.org/licenses/>.
+
+#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);