Merge pull request #342 from bobbycar-graz/add-ble-fence

main/CMakeLists.txt

@@ -215,6 +215,7 @@ set(headers
     modes/tempomatmode.h
     modes/wheelchairmode.h
     mosfets.h
+    motorpwmlimiter.h
     newsettings.h
     ota.h
     potis.h
@@ -226,6 +227,7 @@ set(headers
     serial_bobby.h
     settingspersister.h
     settingsutils.h
+    softpwmlimiter.h
     statistics.h
     statustexthelper.h
     taskmanager.h
@@ -458,6 +460,7 @@ set(sources
     modes/tempomatmode.cpp
     modes/wheelchairmode.cpp
     mosfets.cpp
+    motorpwmlimiter.cpp
     newsettings.cpp
     ota.cpp
     potis.cpp
@@ -469,6 +472,7 @@ set(sources
     serial_bobby.cpp
     settingspersister.cpp
     settingsutils.cpp
+    softpwmlimiter.cpp
     statistics.cpp
     statustexthelper.cpp
     taskmanager.cpp

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,8 @@
 
 // local includes
 #include "globals.h"
+#include "motorpwmlimiter.h"
+#include "utils.h"
 
 void initDrivingMode()
 {
@@ -21,4 +23,10 @@ void updateDrivingMode()
 
     if (currentMode)
         currentMode->update();
+
+    fixCommonParams();
+    motor_pwm_limiter::update();
+
+    // Last, send values to motor controllers
+    sendCommands();
 }

main/modes/defaultmode.cpp

@@ -258,8 +258,6 @@ void DefaultMode::update()
                 motor.cruiseCtrlEna = false;
                 motor.nCruiseMotTgt = 0;
             }
-            fixCommonParams();
         }
     }
-    sendCommands();
 }

main/modes/gametrakmode.cpp

@@ -76,9 +76,5 @@ void GametrakMode::update()
             motor.nCruiseMotTgt = 0;
         }
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }
 #endif

main/modes/ignoreinputmode.cpp

@@ -14,8 +14,4 @@ void IgnoreInputMode::update()
         motor.cruiseCtrlEna = false;
         motor.nCruiseMotTgt = 0;
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }

main/modes/larsmmode.cpp

@@ -105,8 +105,4 @@ void LarsmMode::update()
             motor.nCruiseMotTgt = 0;
         }
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }

main/modes/mickmode.cpp

@@ -78,8 +78,4 @@ void MickMode::update()
             motor.nCruiseMotTgt = 0;
         }
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }

main/modes/motortestmode.cpp

@@ -34,7 +34,4 @@ void MotortestMode::update()
         motor.cruiseCtrlEna = false;
         motor.nCruiseMotTgt = 0;
     }
-
-    fixCommonParams();
-    sendCommands();
 }

main/modes/remotecontrolmode.cpp

@@ -42,10 +42,6 @@ void RemoteControlMode::update()
         controllers.back.command.left.pwm = m_remoteCommand->backLeft;
         controllers.back.command.right.pwm = m_remoteCommand->backRight;
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }
 
 void RemoteControlMode::setCommand(const RemoteCommand &command)

main/modes/tempomatmode.cpp

@@ -53,8 +53,4 @@ void TempomatMode::update()
             motor.nCruiseMotTgt = nCruiseMotTgt;
         }
     }
-
-    fixCommonParams();
-
-    sendCommands();
 }

main/modes/wheelchairmode.cpp

@@ -219,9 +219,7 @@ void WheelchairMode::update()
                 controller.command.right.cruiseCtrlEna = false;
                 controller.command.right.nCruiseMotTgt = 0;
             }
-            fixCommonParams();
         }
     }
-    sendCommands();
 }
 #endif

main/motorpwmlimiter.cpp

@@ -0,0 +1,47 @@
+#include "motorpwmlimiter.h"
+
+// local includes
+#include "ble_bobby.h"
+#include "globals.h"
+#include "softpwmlimiter.h"
+#include "utils.h"
+
+namespace motor_pwm_limiter {
+void update()
+{
+    if (!configs.bleSettings.bleEnabled.value() || !configs.bleSettings.bleFenceEnabled.value() || (pServer && !pServer->getPeerDevices().empty()))
+    {
+        soft_pwm_limiter::trigger = false;
+        soft_pwm_limiter::update();
+        return;
+    }
+
+    soft_pwm_limiter::trigger = true;
+
+    const auto mot = motors();
+
+    int max_pwm = 0;
+    for (const 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();
+    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 *= ratio;
+    }
+}
+}

main/motorpwmlimiter.h

@@ -0,0 +1,5 @@
+#pragma once
+
+namespace motor_pwm_limiter {
+void update();
+} // namespace motor_pwm_limiter

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) \
@@ -784,8 +785,8 @@ public:
     x(feature.ota.isEnabled) \
     x(feature.udpcloud.isEnabled) \
     x(feature.webserver.isEnabled) \
-    x(feature.webserver_disable_lock.isEnabled)
-    //x(bleSettings.bleEnabled)
+    x(feature.webserver_disable_lock.isEnabled) \
+    x(bleSettings.bleEnabled)
 
 #define FEATURES(x) \
     x(feature.ble) \
@@ -807,7 +808,7 @@ public:
 #define HELPER(x) callback(x);
         NEW_SETTINGS(HELPER)
 #undef HELPER
-        callback(bleSettings.bleEnabled);
+        callback(bleSettings.bleFenceEnabled);
     }
 
     auto getAllConfigParams()
@@ -816,7 +817,7 @@ public:
 #define HELPER(x) std::ref<ConfigWrapperInterface>(x),
             NEW_SETTINGS(HELPER)
 #undef HELPER
-            std::ref<ConfigWrapperInterface>(bleSettings.bleEnabled)
+            std::ref<ConfigWrapperInterface>(bleSettings.bleFenceEnabled)
         );
     }
 

main/softpwmlimiter.cpp

@@ -0,0 +1,55 @@
+#include "softpwmlimiter.h"
+
+// system includes
+#include <algorithm>
+
+// 3rdparty lib includes
+#include <espchrono.h>
+
+namespace soft_pwm_limiter {
+constexpr float MAX_LIMIT = 1500.f;
+
+namespace {
+float actual_limit = MAX_LIMIT;
+bool active = false;
+espchrono::millis_clock::time_point last_update;
+} // namespace
+
+bool trigger = false;
+
+void update()
+{
+    using namespace std::chrono_literals;
+
+    if (trigger)
+    {
+        const auto now = espchrono::millis_clock::now();
+        if (!active)
+        {
+            last_update = now;
+            active = true;
+            return;
+        }
+
+        // int time_delta = std::chrono::floor<std::chrono::milliseconds>(now - last_update).count();
+        int time_delta = espchrono::ago(last_update) / 1ms;
+        last_update = now;
+        time_delta = std::max(time_delta, 0);
+
+        // 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);
+    }
+    else
+    {
+        actual_limit = MAX_LIMIT;
+        active = false;
+    }
+}
+
+float limit(float pwm)
+{
+    return std::clamp(pwm, -actual_limit, actual_limit);
+}
+
+} // namespace soft_pwm_limiter

main/softpwmlimiter.h

@@ -0,0 +1,11 @@
+#pragma once
+
+namespace soft_pwm_limiter {
+
+extern bool trigger;
+
+void update();
+
+float limit(float pwm);
+
+} // namespace soft_pwm_limiter