Refactor Reverse Stop

Replaces the ERPM-based implementation with a distance-based one,
 implemented in a separate module.

Feature: New and more consistent Reverse Stop implementation

Author: lukash

src/data.h

@@ -36,6 +36,7 @@
 #include "motor_data.h"
 #include "pid.h"
 #include "remote.h"
+#include "reverse_stop.h"
 #include "state.h"
 #include "time.h"
 #include "torque_tilt.h"
@@ -72,6 +73,7 @@ typedef struct {
     FootpadSensor footpad;
     HapticFeedback haptic_feedback;
     AlertTracker alert_tracker;
+    ReverseStop reverse_stop;
 
     Leds leds;
     LcmData lcm;
@@ -121,10 +123,6 @@ typedef struct {
     // Feature: Flywheel
     bool flywheel_abort;
 
-    // Feature: Reverse Stop
-    float reverse_tolerance, reverse_total_erpm;
-    time_t reverse_timer;
-
     // Feature: Soft Start
     float softstart_pid_limit;
 

src/main.c

@@ -97,7 +97,6 @@ static void cmd_flywheel_toggle(Data *d, unsigned char *cfg, int len);
 
 const VESC_PIN beeper_pin = VESC_PIN_PPM;
 
-#define REVSTOP_ERPM_INCR 0.00008
 #define EXT_BEEPER_ON() VESC_IF->io_write(beeper_pin, 1)
 #define EXT_BEEPER_OFF() VESC_IF->io_write(beeper_pin, 0)
 
@@ -172,6 +171,8 @@ static void main_freq_update_reconfigure(float frequency) {
 
     ema_configure(&d->balance_current, 25.0f, frequency);
 
+    reverse_stop_configure(&d->reverse_stop, frequency);
+
     log_msg(
         "Main freq reconfgure old: %dHz new: %dHz",
         (int32_t) d->main_t.filter_frequency,
@@ -228,9 +229,6 @@ static void configure(Data *d) {
         VESC_IF->set_cfg_float(CFG_PARAM_IMU_accel_confidence_decay, 0.1);
     }
 
-    // Feature: Reverse Stop
-    d->reverse_tolerance = 20000;
-
     // Speed above which to warn users about an impending full switch fault
     d->switch_warn_beep_erpm = d->float_conf.is_footbeep_enabled ? 2000 : 100000;
 
@@ -260,6 +258,7 @@ static void reset_runtime_vars(Data *d) {
     turn_tilt_reset(&d->turn_tilt);
     remote_reset(&d->remote);
     booster_reset(&d->booster);
+    reverse_stop_reset(&d->reverse_stop, d->motor.distance);
 
     ema_reset(&d->balance_current, 0.0f);
 
@@ -442,34 +441,17 @@ static bool check_faults(Data *d) {
             timer_refresh(&d->time, &d->fault_switch_timer);
         }
 
-        // Feature: Reverse-Stop
         if (d->state.sat == SAT_REVERSESTOP) {
-            //  Taking your foot off entirely while reversing? Ignore delays
+            // ignore delays if sensor is completely disengaged while reversing
             if (d->footpad.state == FS_NONE) {
                 state_stop(&d->state, STOP_SWITCH_FULL);
                 return true;
             }
-            if (fabsf(d->imu.pitch) > 18) {
-                state_stop(&d->state, STOP_REVERSE_STOP);
-                return true;
-            }
-            // Above 10 degrees for a full second? Switch it off
-            if (fabsf(d->imu.pitch) > 10 && timer_older(&d->time, d->reverse_timer, 1)) {
-                state_stop(&d->state, STOP_REVERSE_STOP);
-                return true;
-            }
-            // Above 5 degrees for 2 seconds? Switch it off
-            if (fabsf(d->imu.pitch) > 5 && timer_older(&d->time, d->reverse_timer, 2)) {
-                state_stop(&d->state, STOP_REVERSE_STOP);
-                return true;
-            }
-            if (fabsf(d->reverse_total_erpm) > d->reverse_tolerance * 10) {
+
+            if (reverse_stop_stop(&d->reverse_stop, &d->time)) {
                 state_stop(&d->state, STOP_REVERSE_STOP);
                 return true;
             }
-            if (fabsf(d->imu.pitch) < 5) {
-                timer_refresh(&d->time, &d->reverse_timer);
-            }
         }
 
         // Switch partially open and stopped
@@ -536,34 +518,15 @@ static void calculate_setpoint_target(Data *d) {
             d->state.sat = SAT_NONE;
         }
     } else if (d->state.sat == SAT_REVERSESTOP) {
-        // accumalete erpms:
-        d->reverse_total_erpm += d->motor.erpm;
-        if (fabsf(d->reverse_total_erpm) > d->reverse_tolerance) {
-            // tilt down by 10 degrees after exceeding aggregate erpm
-            d->setpoint_target =
-                (fabsf(d->reverse_total_erpm) - d->reverse_tolerance) * REVSTOP_ERPM_INCR;
+        if (reverse_stop_active(&d->reverse_stop)) {
+            d->setpoint_target = reverse_stop_setpoint(&d->reverse_stop);
         } else {
-            if (fabsf(d->reverse_total_erpm) <= d->reverse_tolerance * 0.5) {
-                if (d->motor.erpm >= 0) {
-                    d->state.sat = SAT_NONE;
-                    d->reverse_total_erpm = 0;
-                    d->setpoint_target = 0;
-                }
-            }
+            d->state.sat = SAT_NONE;
         }
-    } else if (d->float_conf.fault_reversestop_enabled && d->motor.erpm < -200 &&
+    } else if (d->float_conf.fault_reversestop_enabled && reverse_stop_active(&d->reverse_stop) &&
                !d->state.darkride) {
-        // Detecting reverse stop takes priority over any error condition SAT
-        if (d->state.sat >= SAT_PB_HIGH_VOLTAGE) {
-            // If this happens while in Error-Tiltback (LV/HV/TEMP) then we need to
-            // take the already existing setpoint into account
-            d->reverse_total_erpm =
-                -(d->reverse_tolerance + d->setpoint_target_interpolated / REVSTOP_ERPM_INCR);
-        } else {
-            d->reverse_total_erpm = 0;
-        }
+        d->setpoint_target = reverse_stop_setpoint(&d->reverse_stop);
         d->state.sat = SAT_REVERSESTOP;
-        timer_refresh(&d->time, &d->reverse_timer);
     } else if (d->state.mode != MODE_FLYWHEEL &&
                // not normal, either wheelslip or wheel getting stuck
                fabsf(d->motor.acceleration.value) > 10000 &&
@@ -924,6 +887,14 @@ static void refloat_thd(void *arg) {
             break;
 
         case (STATE_RUNNING):
+            reverse_stop_update(
+                &d->reverse_stop,
+                d->motor.distance,
+                d->setpoint_target_interpolated,
+                &d->time,
+                d->float_conf.fault_reversestop_enabled
+            );
+
             // Check for faults
             if (check_faults(d)) {
                 if (d->state.stop_condition == STOP_SWITCH_FULL && !d->state.darkride) {
@@ -1263,6 +1234,7 @@ static void data_init(Data *d) {
     footpad_sensor_init(&d->footpad);
     haptic_feedback_init(&d->haptic_feedback);
     alert_tracker_init(&d->alert_tracker);
+    reverse_stop_init(&d->reverse_stop);
 
     leds_init(&d->leds);
     lcm_init(&d->lcm, &d->float_conf.hardware.leds);

src/reverse_stop.c

@@ -0,0 +1,129 @@
+// Copyright 2025 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 "reverse_stop.h"
+
+#include "lib/transitions.h"
+#include "lib/utils.h"
+#include <math.h>
+
+#define REVERSE_STOP_DISTANCE 0.5f
+#define TARGET_STOP_ANGLE 15.0f
+#define DISTANCE_PER_DEGREE (REVERSE_STOP_DISTANCE / TARGET_STOP_ANGLE)
+#define TIMER_ANGLE_THRESHOLD (TARGET_STOP_ANGLE / 2)
+
+// Distance of travel for one motor step, could be calculated, for the purposes
+// of Reverse Stop an approximate value is enough. Calculated as: wheel_dia *
+// pi / (3 * motor_poles)
+#define MOTOR_STEP_DIST 0.01f
+
+void reverse_stop_init(ReverseStop *rs) {
+    ema_init(&rs->progress);
+    reverse_stop_reset(rs, 0.0f);
+}
+
+void reverse_stop_reset(ReverseStop *rs, float distance) {
+    rs->start_setpoint = 0.0f;
+    rs->target_setpoint = 0.0f;
+    rs->start_distance = distance;
+    rs->current_distance = 0.0f;
+    rs->target_distance = 0.0f;
+    ema_reset(&rs->progress, 1.0f);
+}
+
+void reverse_stop_configure(ReverseStop *rs, float frequency) {
+    ema_configure(&rs->progress, 1.0f, frequency);
+}
+
+void reverse_stop_update(
+    ReverseStop *rs, float distance, float setpoint, const Time *time, bool enabled
+) {
+    if (!enabled && rs->progress.value >= 1.0f) {
+        rs->start_distance = distance;
+        return;
+    }
+
+    float new_distance = distance - rs->start_distance;
+    float distance_diff = (new_distance - rs->current_distance) * sign(rs->target_distance);
+
+    if (distance_diff < -2 * MOTOR_STEP_DIST) {
+        rs->target_setpoint = rs->target_setpoint > 0.0f ? 0.0f : TARGET_STOP_ANGLE;
+        rs->start_setpoint = setpoint;
+        rs->start_distance = distance;
+        rs->current_distance = 0.0f;
+        rs->target_distance = fabsf(rs->start_setpoint - rs->target_setpoint) * DISTANCE_PER_DEGREE;
+        // determine sign according to target_setpoint for the case we somehow go into
+        // a reverse stop from a negative setpoint and then start going back out of it
+        if (rs->target_setpoint > 0.0f) {
+            rs->target_distance *= -1;
+        }
+
+        float new_progress = 0.0f;
+        if (fabsf(rs->target_distance) < MOTOR_STEP_DIST) {
+            new_progress = 1.0f;
+            rs->target_distance = 0.0f;
+        }
+        ema_reset(&rs->progress, new_progress);
+
+        return;
+    }
+
+    if (rs->progress.value >= 1.0f) {
+        if (distance_diff > 0.0f) {
+            rs->start_distance = distance;
+        }
+        return;
+    }
+
+    if (distance_diff > 0.0f) {
+        rs->current_distance = new_distance;
+    }
+
+    ema_update(&rs->progress, rs->current_distance / rs->target_distance);
+
+    if (rs->progress.value >= 1.0f) {
+        rs->target_distance = 0.0f;
+        rs->current_distance = 0.0f;
+    }
+
+    // update the timer if returning from reverse or the angle is above threshold
+    if (rs->target_setpoint == 0.0f || setpoint < TIMER_ANGLE_THRESHOLD) {
+        timer_refresh(time, &rs->timer);
+    }
+}
+
+float reverse_stop_setpoint(ReverseStop *rs) {
+    float prog = smoothstep(rs->progress.value);
+    return rs->start_setpoint + prog * (rs->target_setpoint - rs->start_setpoint);
+}
+
+bool reverse_stop_active(ReverseStop *rs) {
+    return rs->target_setpoint > 0.0f || rs->progress.value < 1.0f;
+}
+
+bool reverse_stop_stop(ReverseStop *rs, const Time *time) {
+    float progress = rs->progress.value;
+
+    // the timer only starts aging below a certain setpoint angle threshold
+    // the further we get progress-wise, the sooner we disengage: 3s at 0%, 1s at 100%
+    float time_threshold = 3.0f - 2 * progress;
+    if (timer_older(time, rs->timer, time_threshold)) {
+        return true;
+    }
+
+    return rs->target_setpoint > 0.0f && progress >= 1.0f;
+}

src/reverse_stop.h

@@ -0,0 +1,50 @@
+// Copyright 2025 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 "filters/ema.h"
+#include "time.h"
+
+#include <stdbool.h>
+
+typedef struct {
+    float start_setpoint;  // absolute setpoint from which we started
+    float target_setpoint;  // absolute target setpoint, either 0 or TARGET_STOP_ANGLE
+    float start_distance;  // absolute distance from which we started
+    float target_distance;  // relative target distance to go, negative when going into reverse stop
+    float current_distance;  // relative distance we're at right now
+    EMA progress;
+
+    time_t timer;
+} ReverseStop;
+
+void reverse_stop_init(ReverseStop *rs);
+
+void reverse_stop_reset(ReverseStop *rs, float distance);
+
+void reverse_stop_configure(ReverseStop *rs, float frequency);
+
+void reverse_stop_update(
+    ReverseStop *rs, float distance, float setpoint, const Time *time, bool enabled
+);
+
+float reverse_stop_setpoint(ReverseStop *rs);
+
+bool reverse_stop_active(ReverseStop *rs);
+
+bool reverse_stop_stop(ReverseStop *rs, const Time *time);