Add BLE fence

main/accessors/settingsaccessors.h

@@ -39,6 +39,7 @@ struct PhaseAdvMaxAccessor : public RefAccessorSaveSettings<int16_t> { int16_t &
 
 // Bluetooth Low Energy
 struct BleEnabledAccessor : public NewSettingsAccessor<bool> { ConfigWrapper<bool> &getConfig() const override { return configs.bleSettings.bleEnabled; } };
+struct BleFenceEnabledAccessor : public NewSettingsAccessor<bool> { ConfigWrapper<bool> &getConfig() const override { return configs.bleSettings.bleFenceEnabled; } };
 
 // Cloud
 struct CloudEnabledAccessor : public NewSettingsAccessor<bool> { ConfigWrapper<bool> &getConfig() const override { return configs.cloudSettings.cloudEnabled; } };

main/displays/menus/blesettingsmenu.cpp

@@ -19,6 +19,7 @@
 namespace {
 constexpr char TEXT_BLESETTINGS[] = "BLE settings";
 constexpr char TEXT_ENABLED[] = "Enabled";
+constexpr char TEXT_FENCE_ENABLED[] = "Fence enabled";
 constexpr char TEXT_NAME[] = "Name";
 constexpr char TEXT_NAME_FORMATTED[] = "Name: &s";
 constexpr char TEXT_BACK[] = "Back";
@@ -36,6 +37,7 @@ BleSettingsMenu::BleSettingsMenu()
 {
     using namespace espgui;
     constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_ENABLED>, BobbyCheckbox, BleEnabledAccessor>>();
+    constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_FENCE_ENABLED>, BobbyCheckbox, BleFenceEnabledAccessor>>();
     constructMenuItem<makeComponent<MenuItem, BleServerPeerDevicesText,    DisabledColor, DummyAction>>();
     constructMenuItem<makeComponent<MenuItem, BleCharacSubscribedText,     DisabledColor, DummyAction>>();
     constructMenuItem<makeComponent<MenuItem, TextWithValueHelper<TEXT_NAME_FORMATTED, BluetoothNameAccessor>, PushScreenAction<ApSsidChangeScreen>>>();

main/modes.cpp

@@ -2,6 +2,7 @@
 
 // local includes
 #include "globals.h"
+#include "motorpwmlimiter.h"
 
 void initDrivingMode()
 {
@@ -23,6 +24,7 @@ void updateDrivingMode()
         currentMode->update();
 
     fixCommonParams();
+    motor_pwm_limiter::update();
 
     // Last, send values to motor controllers
     sendCommands();

main/motorpwmlimiter.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "globals.h"
+#include "utils.h"
+#include "softpwmlimiter.h"
+#include "ble_bobby.h"
+
+namespace motor_pwm_limiter {
+    void update() {
+        if (!configs.bleSettings.bleFenceEnabled.value() || pServer->getPeerDevices().size()) {
+            soft_pwm_limiter::trigger = false;
+            soft_pwm_limiter::update(1500.f);
+            return;
+        }
+
+        soft_pwm_limiter::trigger = true;
+
+        auto mot = motors();
+        int max_pwm = 0;
+        for (bobbycar::protocol::serial::MotorState &motor : mot) {
+            max_pwm = std::max(max_pwm, std::abs(motor.pwm));
+        }
+
+        // Scale PWM proportionally to avoid loss of steering in case of wheelchair or RC mode
+        soft_pwm_limiter::update(max_pwm);
+        float new_max = soft_pwm_limiter::limit(max_pwm);
+        float ratio;
+        if (max_pwm > 0.f)
+            ratio = new_max / max_pwm;
+        else
+            ratio = 0.f;
+
+        // Should not happen
+        if (ratio > 1.f)
+            ratio = 1.f;
+
+        for (bobbycar::protocol::serial::MotorState &motor : mot) {
+            motor.pwm = motor.pwm * ratio;
+        }
+    }
+}

main/newsettings.h

@@ -471,6 +471,7 @@ public:
 
     struct {
         ConfigWrapperLegacy<bool> bleEnabled            {true,                                   DoReset,   {},                         "bleEnabled"          };
+        ConfigWrapperLegacy<bool> bleFenceEnabled       {false,                                  DoReset,   {},                         "bleFenceEnabled"     };
     } bleSettings;
 
 #define NEW_SETTINGS(x) \
@@ -808,6 +809,7 @@ public:
         NEW_SETTINGS(HELPER)
 #undef HELPER
         callback(bleSettings.bleEnabled);
+        callback(bleSettings.bleFenceEnabled);
     }
 
     auto getAllConfigParams()
@@ -816,7 +818,8 @@ public:
 #define HELPER(x) std::ref<ConfigWrapperInterface>(x),
             NEW_SETTINGS(HELPER)
 #undef HELPER
-            std::ref<ConfigWrapperInterface>(bleSettings.bleEnabled)
+            std::ref<ConfigWrapperInterface>(bleSettings.bleEnabled),
+            std::ref<ConfigWrapperInterface>(bleSettings.bleFenceEnabled)
         );
     }
 

main/softpwmlimiter.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+
+#include <espchrono.h>
+
+namespace soft_pwm_limiter {
+    namespace {
+        constexpr float MAX_LIMIT = 1500.f;
+        //inline constexpr float TAU = 5000.f;
+
+        inline float actual_limit = MAX_LIMIT;
+        inline bool active = false;
+        inline espchrono::millis_clock::time_point last_update;
+    }
+
+    inline bool trigger = false;
+
+    void update(float current_pwm) {
+        if (trigger) {
+            auto now = espchrono::millis_clock::now();
+            if (!active) {
+                last_update = now;
+                active = true;
+                return;
+            }
+
+            float time_delta = std::chrono::floor<std::chrono::milliseconds>(now - last_update).count();
+            last_update = now;
+            time_delta = std::max(time_delta, 0.f);
+
+            //float smoothness = expf(-time_delta / TAU);
+
+            //actual_limit = actual_limit * smoothness;
+            // Exit field weakening area (]1000, 1500]) within 2 seconds
+            actual_limit -= 500.f * time_delta / 2000.f;
+            actual_limit = std::max(actual_limit, 0.f);
+            //if (current_pwm >= -1500.f && current_pwm <= 1500.f)
+            //    actual_limit = std::min(actual_limit, std::abs(current_pwm));
+        } else {
+            actual_limit = MAX_LIMIT;
+            active = false;
+        }
+    }
+
+    float limit(float pwm) {
+        return std::clamp(pwm, -actual_limit, actual_limit);
+    }
+}