From 1bd3979241097503602e57a9be6241548438cbaa Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Tue, 20 Jul 2021 21:45:00 +0200 Subject: [PATCH] BLE remote control preperations and faster livedata --- components/ArduinoJson | 2 +- main/CMakeLists.txt | 1 + main/ble.h | 208 ++++++++++++++------------- main/displays/menus/selectmodemenu.h | 7 +- main/main.cpp | 2 +- main/modes/remotecontrolmode.h | 82 +++++++++++ main/texts.h | 1 + 7 files changed, 197 insertions(+), 106 deletions(-) create mode 100644 main/modes/remotecontrolmode.h diff --git a/components/ArduinoJson b/components/ArduinoJson index d66159f..73a34f7 160000 --- a/components/ArduinoJson +++ b/components/ArduinoJson @@ -1 +1 @@ -Subproject commit d66159f51a7b958789ab83aabd72f270a04af066 +Subproject commit 73a34f7d92125dd9e789ef499e13498ad0d69807 diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 55ec31b..b7e12ea 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -130,6 +130,7 @@ set(headers modes/gametrakmode.h modes/ignoreinputmode.h modes/larsmmode.h + modes/remotecontrolmode.h modes/tempomatmode.h rotary.h screens.h diff --git a/main/ble.h b/main/ble.h index a1ed861..fc1701c 100644 --- a/main/ble.h +++ b/main/ble.h @@ -1,6 +1,7 @@ #pragma once // 3rdparty lib includes +#include #ifdef FEATURE_BLE #include #endif @@ -13,20 +14,8 @@ namespace { #ifdef FEATURE_BLE BLEServer *pServer{}; BLEService *pService{}; -int bleIndex{}; - -struct { - struct { - BLECharacteristic *voltage{}; - BLECharacteristic *temperature{}; - - struct { - BLECharacteristic *error{}; - BLECharacteristic *speed{}; - BLECharacteristic *dcLink{}; - } left, right; - } front, back; -} characteristics; +BLECharacteristic *livestatsCharacteristic{}; +BLECharacteristic *remotecontrolCharacteristic{}; void initBle() { @@ -38,26 +27,8 @@ void initBle() pService = pServer->createService(serviceUuid); - characteristics.front.voltage = pService->createCharacteristic("a48321ea-329f-4eab-a401-30e247211524", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.voltage = pService->createCharacteristic("4201def0-a264-43e6-946b-6b2d9612dfed", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - - characteristics.front.temperature = pService->createCharacteristic("4799e23f-6448-4786-900b-b5c3f3c17a9c", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.temperature = pService->createCharacteristic("3c32b7bb-8d9b-4055-8ea0-5b6764111024", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - - characteristics.front.left.error = pService->createCharacteristic("f84b3a9b-1b2c-4075-acbe-016a2166976c", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.front.right.error = pService->createCharacteristic("eed4b709-5a65-4a5b-8e07-512f9661533d", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.left.error = pService->createCharacteristic("89d143f5-9ae2-4f7e-9235-643a3a7e21df", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.right.error = pService->createCharacteristic("0fb377f1-7527-4966-aaf0-8bd56f2ddd3f", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - - characteristics.front.left.speed = pService->createCharacteristic("c6f959e8-0ec3-4bdd-88ad-6ad993fc81e9", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.front.right.speed = pService->createCharacteristic("ce53f135-8f20-4b80-abb9-31da81d62716", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.left.speed = pService->createCharacteristic("9a1dd1fe-3f14-4af1-bc5e-3f70edcae54b", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.right.speed = pService->createCharacteristic("7de1a823-682e-438f-9201-3a80c3911f1a", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - - characteristics.front.left.dcLink = pService->createCharacteristic("f404416f-2a77-41c6-a35f-7d10ec38376d", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.front.right.dcLink = pService->createCharacteristic("452dd012-3f12-428c-8746-40c6b6c73c40", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.left.dcLink = pService->createCharacteristic("9dc455a3-718e-4d62-b0e7-1c0cb2a8bbd3", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); - characteristics.back.right.dcLink = pService->createCharacteristic("90a66506-1d78-4ba2-b074-e1153fbf5216", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); + livestatsCharacteristic = pService->createCharacteristic("a48321ea-329f-4eab-a401-30e247211524", NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY); + remotecontrolCharacteristic = pService->createCharacteristic("4201def0-a264-43e6-946b-6b2d9612dfed", NIMBLE_PROPERTY::WRITE); pService->start(); @@ -69,77 +40,110 @@ void initBle() void handleBle() { - switch (bleIndex++) { - default: - bleIndex = 1; - [[fallthrough]]; - case 0: - characteristics.front.voltage->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", fixBatVoltage(controllers.front.feedback.batVoltage)) : ""); - characteristics.front.voltage->notify(); - break; - case 1: - characteristics.back.voltage->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", fixBatVoltage(controllers.back.feedback.batVoltage)) : ""); - characteristics.back.voltage->notify(); - break; - case 2: - characteristics.front.temperature->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", fixBoardTemp(controllers.front.feedback.boardTemp)) : ""); - characteristics.front.temperature->notify(); - break; - case 3: - characteristics.back.temperature->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", fixBoardTemp(controllers.back.feedback.boardTemp)) : ""); - characteristics.back.temperature->notify(); - break; + if (livestatsCharacteristic->getSubscribedCount()) + { + StaticJsonDocument<1024> doc; + { + auto arr = doc.createNestedArray("v"); + if (controllers.front.feedbackValid) + arr.add(fixBatVoltage(controllers.front.feedback.batVoltage)); + else + arr.add(nullptr); + if (controllers.back.feedbackValid) + arr.add(fixBatVoltage(controllers.back.feedback.batVoltage)); + else + arr.add(nullptr); + } - case 4: - characteristics.front.left.error->setValue(controllers.front.feedbackValid ? fmt::format("{}", controllers.front.feedback.left.error) : ""); - characteristics.front.left.error->notify(); - break; - case 5: - characteristics.front.right.error->setValue(controllers.front.feedbackValid ? fmt::format("{}", controllers.front.feedback.right.error) : ""); - characteristics.front.right.error->notify(); - break; - case 6: - characteristics.back.left.error->setValue(controllers.back.feedbackValid ? fmt::format("{}", controllers.back.feedback.left.error) : ""); - characteristics.back.left.error->notify(); - break; - case 7: - characteristics.back.right.error->setValue(controllers.back.feedbackValid ? fmt::format("{}", controllers.back.feedback.right.error) : ""); - characteristics.back.right.error->notify(); - break; + { + auto arr = doc.createNestedArray("t"); + if (controllers.front.feedbackValid) + arr.add(fixBoardTemp(controllers.front.feedback.boardTemp)); + else + arr.add(nullptr); + if (controllers.back.feedbackValid) + arr.add(fixBoardTemp(controllers.back.feedback.boardTemp)); + else + arr.add(nullptr); + } - case 8: - characteristics.front.left.speed->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", convertToKmh(controllers.front.feedback.left.speed * (settings.controllerHardware.invertFrontLeft ? -1 : 1))) : ""); - characteristics.front.left.speed->notify(); - break; - case 9: - characteristics.front.right.speed->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", convertToKmh(controllers.front.feedback.right.speed * (settings.controllerHardware.invertFrontRight ? -1 : 1))) : ""); - characteristics.front.right.speed->notify(); - break; - case 10: - characteristics.back.left.speed->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", convertToKmh(controllers.back.feedback.left.speed * (settings.controllerHardware.invertBackLeft ? -1 : 1))) : ""); - characteristics.back.left.speed->notify(); - break; - case 11: - characteristics.back.right.speed->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", convertToKmh(controllers.back.feedback.right.speed * (settings.controllerHardware.invertBackRight ? -1 : 1))) : ""); - characteristics.back.right.speed->notify(); - break; + { + auto arr = doc.createNestedArray("e"); + if (controllers.front.feedbackValid) + { + arr.add(controllers.front.feedback.left.error); + arr.add(controllers.front.feedback.right.error); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + if (controllers.back.feedbackValid) + { + arr.add(controllers.back.feedback.left.error); + arr.add(controllers.back.feedback.right.error); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + } - case 12: - characteristics.front.left.dcLink->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", fixCurrent(controllers.front.feedback.left.dcLink)) : ""); - characteristics.front.left.dcLink->notify(); - break; - case 13: - characteristics.front.right.dcLink->setValue(controllers.front.feedbackValid ? fmt::format("{:.2f}", fixCurrent(controllers.front.feedback.right.dcLink)) : ""); - characteristics.front.right.dcLink->notify(); - break; - case 14: - characteristics.back.left.dcLink->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", fixCurrent(controllers.back.feedback.left.dcLink)) : ""); - characteristics.back.left.dcLink->notify(); - break; - case 15: - characteristics.back.right.dcLink->setValue(controllers.back.feedbackValid ? fmt::format("{:.2f}", fixCurrent(controllers.back.feedback.right.dcLink)) : ""); - characteristics.back.right.dcLink->notify(); - break; + { + auto arr = doc.createNestedArray("s"); + if (controllers.front.feedbackValid) + { + arr.add(convertToKmh(controllers.front.feedback.left.speed * (settings.controllerHardware.invertFrontLeft ? -1 : 1))); + arr.add(convertToKmh(controllers.front.feedback.right.speed * (settings.controllerHardware.invertFrontRight ? -1 : 1))); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + if (controllers.back.feedbackValid) + { + arr.add(convertToKmh(controllers.back.feedback.left.speed * (settings.controllerHardware.invertBackLeft ? -1 : 1))); + arr.add(convertToKmh(controllers.back.feedback.right.speed * (settings.controllerHardware.invertBackRight ? -1 : 1))); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + } + + { + auto arr = doc.createNestedArray("a"); + if (controllers.front.feedbackValid) + { + arr.add(fixCurrent(controllers.front.feedback.left.dcLink)); + arr.add(fixCurrent(controllers.front.feedback.right.dcLink)); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + if (controllers.back.feedbackValid) + { + arr.add(fixCurrent(controllers.back.feedback.left.dcLink)); + arr.add(fixCurrent(controllers.back.feedback.right.dcLink)); + } + else + { + arr.add(nullptr); + arr.add(nullptr); + } + } + + std::string json; + serializeJson(doc, json); + + livestatsCharacteristic->setValue(json); + livestatsCharacteristic->notify(); } } #endif diff --git a/main/displays/menus/selectmodemenu.h b/main/displays/menus/selectmodemenu.h index a98b5e6..9c033fd 100644 --- a/main/displays/menus/selectmodemenu.h +++ b/main/displays/menus/selectmodemenu.h @@ -12,6 +12,7 @@ #include "modes/defaultmode.h" #include "modes/tempomatmode.h" #include "modes/larsmmode.h" +#include "modes/remotecontrolmode.h" #include "modes/gametrakmode.h" // forward declares @@ -29,6 +30,7 @@ public: using SetDefaultModeAction = SetterAction; using SetTempomatModeAction = SetterAction; using SetLarsmModeAction = SetterAction; +using SetRemoteControlModeAction = SetterAction; #ifdef FEATURE_GAMETRAK using SetGametrakModeAction = SetterAction; #endif @@ -43,9 +45,10 @@ class SelectModeMenu : public: SelectModeMenu() { - constructMenuItem, MultiAction>>>(); + constructMenuItem, MultiAction>>>(); constructMenuItem, MultiAction>>>(); - constructMenuItem, MultiAction>>>(); + constructMenuItem, MultiAction>>>(); + constructMenuItem, MultiAction>>>(); #ifdef FEATURE_GAMETRAK constructMenuItem, MultiAction>>>(); #endif diff --git a/main/main.cpp b/main/main.cpp index e7e88f8..7102c52 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -374,7 +374,7 @@ extern "C" void app_main() #endif #ifdef FEATURE_BLE - if (!lastBleUpdate || now - *lastBleUpdate >= 1000ms/16) + if (!lastBleUpdate || now - *lastBleUpdate >= 250ms) { handleBle(); diff --git a/main/modes/remotecontrolmode.h b/main/modes/remotecontrolmode.h new file mode 100644 index 0000000..d96c004 --- /dev/null +++ b/main/modes/remotecontrolmode.h @@ -0,0 +1,82 @@ +#pragma once + +// system includes +#include +#include + +// 3rdparty lib includes +#include + +// local includes +#include "bobbycar-common.h" + +#include "modeinterface.h" +#include "globals.h" +#include "utils.h" +#include "defaultmode.h" + +using namespace std::chrono_literals; + +namespace { +struct RemoteCommand { + int16_t frontLeft{}; + int16_t frontRight{}; + int16_t backLeft{}; + int16_t backRight{}; +}; + +class RemoteControlMode : public ModeInterface +{ + using Base = ModeInterface; + +public: + void update() override; + + const char *displayName() const override { return "RemoteControl"; } + + void setCommand(const RemoteCommand &command); + + std::optional m_remoteCommand; + espchrono::millis_clock::time_point m_timestamp; +}; + +namespace modes { +RemoteControlMode remoteControlMode; +} + +void RemoteControlMode::update() +{ + if (!m_remoteCommand || espchrono::ago(m_timestamp) > 1s) + { + start(); + + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + motor.pwm = 0; + motor.cruiseCtrlEna = false; + motor.nCruiseMotTgt = 0; + } + } + else + { + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::Torque; + motor.cruiseCtrlEna = false; + motor.nCruiseMotTgt = 0; + } + + 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.right.pwm = m_remoteCommand->backRight; + } + + fixCommonParams(); + + sendCommands(); +} +} diff --git a/main/texts.h b/main/texts.h index aea5805..2cee1d2 100644 --- a/main/texts.h +++ b/main/texts.h @@ -294,6 +294,7 @@ constexpr char TEXT_RACE[] = "Race"; constexpr char TEXT_DEFAULT[] = "Default"; constexpr char TEXT_TEMPOMAT[] = "Tempomat"; constexpr char TEXT_LARSM[] = "Larsm"; +constexpr char TEXT_REMOTECONTROL[] = "Remote control"; constexpr char TEXT_GAMETRAK[] = "Gametrak"; //constexpr char TEXT_BACK[] = "Back";