From 6a2720c437925a0d5d4864f27fb7d22d9c7cee07 Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Thu, 8 Sep 2022 23:25:50 +0200
Subject: [PATCH] Apply ble remote control command also in default driving mode

---
 main/ble_bobby.cpp               | 11 +++++++----
 main/ble_bobby.h                 |  7 +++++++
 main/modes/defaultmode.cpp       | 14 ++++++++++++++
 main/modes/defaultmode.h         |  6 ++++++
 main/modes/remotecontrolmode.cpp |  2 +-
 main/modes/remotecontrolmode.h   | 11 +++--------
 6 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/main/ble_bobby.cpp b/main/ble_bobby.cpp
index adfe5e9..8c75cd8 100644
--- a/main/ble_bobby.cpp
+++ b/main/ble_bobby.cpp
@@ -10,6 +10,7 @@
 // local includes
 #include "ledstrip.h"
 #include "globals.h"
+#include "modes/defaultmode.h"
 #include "modes/remotecontrolmode.h"
 #include "utils.h"
 #include "newsettings.h"
@@ -246,12 +247,14 @@ void RemoteControlCallbacks::onWrite(NimBLECharacteristic* pCharacteristic)
 
     if (!simplified)
     {
-        modes::remoteControlMode.setCommand(RemoteCommand{
+        RemoteCommand cmd {
             .frontLeft = doc[isInverted ? "fr":"fl"].as<int16_t>(),
             .frontRight = doc[isInverted ? "fl":"fr"].as<int16_t>(),
             .backLeft = doc["bl"].as<int16_t>(),
             .backRight = doc["br"].as<int16_t>()
-        });
+        };
+        modes::defaultMode.setRemoteCommand(cmd);
+        modes::remoteControlMode.setRemoteCommand(cmd);
     }
 }
 
@@ -270,7 +273,7 @@ void WirelessSettingsCallbacks::onWrite(NimBLECharacteristic* pCharacteristic)
 
     if (write_type == "wifi") {
         const int index = doc["wifi_index"].as<int>();
-        ESP_LOGI(TAG, "[ble_config]: Set wifi%i: WiFi-SSID: %s, WiFi-Password: ***", doc["wifi_index"].as<int>(), doc["wifi_ssid"].as<const char*>());
+        ESP_LOGI(TAG, "Set wifi%i: WiFi-SSID: %s, WiFi-Password: ***", doc["wifi_index"].as<int>(), doc["wifi_ssid"].as<const char*>());
         configs.write_config(configs.wifi_configs[index].ssid, doc["wifi_ssid"].as<std::string>());
         configs.write_config(configs.wifi_configs[index].key, doc["wifi_pass"].as<std::string>());
     } else {
@@ -283,7 +286,7 @@ void WiFiListCallbacks::onRead(NimBLECharacteristic *pCharacteristic)
 {
     StaticJsonDocument<768> responseDoc;
     auto wifiArray = responseDoc.createNestedArray("wifis");
-    ESP_LOGI(TAG, "[ble_wifilist] Got request for listing wifi ssids.");
+    ESP_LOGI(TAG, "Got request for listing wifi ssids.");
     for (const auto &wifi : configs.wifi_configs)
     {
         wifiArray.add(wifi.ssid.value());
diff --git a/main/ble_bobby.h b/main/ble_bobby.h
index 06cc769..5c59442 100644
--- a/main/ble_bobby.h
+++ b/main/ble_bobby.h
@@ -13,3 +13,10 @@ extern BLECharacteristic *getwifilist;
 void initBle();
 
 void handleBle();
+
+struct RemoteCommand {
+    int16_t frontLeft{};
+    int16_t frontRight{};
+    int16_t backLeft{};
+    int16_t backRight{};
+};
diff --git a/main/modes/defaultmode.cpp b/main/modes/defaultmode.cpp
index 9750a26..93c28cf 100644
--- a/main/modes/defaultmode.cpp
+++ b/main/modes/defaultmode.cpp
@@ -260,4 +260,18 @@ void DefaultMode::update()
             }
         }
     }
+
+    if (m_remoteCommand && espchrono::ago(m_timestamp) < 500ms)
+    {
+        controllers.front.command.left.pwm += m_remoteCommand->frontLeft;
+        controllers.front.command.right.pwm += m_remoteCommand->frontRight;
+        controllers.back.command.left.pwm += m_remoteCommand->backLeft;
+        controllers.back.command.left.pwm += m_remoteCommand->backRight;
+    }
+}
+
+void DefaultMode::setRemoteCommand(const RemoteCommand &command)
+{
+    m_remoteCommand = command;
+    m_timestamp = espchrono::millis_clock::now();
 }
diff --git a/main/modes/defaultmode.h b/main/modes/defaultmode.h
index 267147f..dcc6b3e 100644
--- a/main/modes/defaultmode.h
+++ b/main/modes/defaultmode.h
@@ -11,6 +11,7 @@
 #include "modeinterface.h"
 #include "globals.h"
 #include "utils.h"
+#include "ble_bobby.h"
 
 class DefaultMode : public ModeInterface
 {
@@ -25,9 +26,14 @@ public:
     bool waitForGasLoslass{false};
     bool waitForBremsLoslass{false};
 
+    void setRemoteCommand(const RemoteCommand &command);
+
 private:
     espchrono::millis_clock::time_point m_lastTime{espchrono::millis_clock::now()};
     float m_lastPwm{0};
+
+    std::optional<RemoteCommand> m_remoteCommand;
+    espchrono::millis_clock::time_point m_timestamp;
 };
 
 namespace modes {
diff --git a/main/modes/remotecontrolmode.cpp b/main/modes/remotecontrolmode.cpp
index 772f86c..d2f1f51 100644
--- a/main/modes/remotecontrolmode.cpp
+++ b/main/modes/remotecontrolmode.cpp
@@ -44,7 +44,7 @@ void RemoteControlMode::update()
     }
 }
 
-void RemoteControlMode::setCommand(const RemoteCommand &command)
+void RemoteControlMode::setRemoteCommand(const RemoteCommand &command)
 {
     m_remoteCommand = command;
     m_timestamp = espchrono::millis_clock::now();
diff --git a/main/modes/remotecontrolmode.h b/main/modes/remotecontrolmode.h
index a29f69f..31feea0 100644
--- a/main/modes/remotecontrolmode.h
+++ b/main/modes/remotecontrolmode.h
@@ -10,13 +10,7 @@
 // local includes
 #include "bobbycar-common.h"
 #include "modeinterface.h"
-
-struct RemoteCommand {
-    int16_t frontLeft{};
-    int16_t frontRight{};
-    int16_t backLeft{};
-    int16_t backRight{};
-};
+#include "ble_bobby.h"
 
 class RemoteControlMode : public ModeInterface
 {
@@ -27,8 +21,9 @@ public:
 
     const char *displayName() const override { return "RemoteControl"; }
 
-    void setCommand(const RemoteCommand &command);
+    void setRemoteCommand(const RemoteCommand &command);
 
+private:
     std::optional<RemoteCommand> m_remoteCommand;
     espchrono::millis_clock::time_point m_timestamp;
 };