From b61dfcf53d03f35af496638118020cf2f282254a Mon Sep 17 00:00:00 2001 From: CommanderRedYT Date: Sun, 18 Dec 2022 00:18:50 +0100 Subject: [PATCH] Added acceleration detection for brake lights --- main/bobbyblinker.cpp | 27 +++++++++++---------------- main/displays/menus/debugmenu.cpp | 10 ++++++++++ main/globals.cpp | 4 ++++ main/globals.h | 4 ++++ main/ledstrip.cpp | 10 ++++++++-- main/ledstrip.h | 2 ++ main/utils.cpp | 22 ++++++++++++++++++++++ 7 files changed, 61 insertions(+), 18 deletions(-) diff --git a/main/bobbyblinker.cpp b/main/bobbyblinker.cpp index 3754a77..20d7c0e 100644 --- a/main/bobbyblinker.cpp +++ b/main/bobbyblinker.cpp @@ -60,23 +60,18 @@ namespace bobbyblinker { } if (configs.ledstrip.enableBrakeLights.value() && espchrono::ago(*brake_last_time_sent) > 500ms) { - float avgPwm{}; - for (const Controller &controller: controllers) { - avgPwm += - controller.command.left.pwm * (controller.invertLeft ? -1 : 1) + - controller.command.right.pwm * (controller.invertRight ? -1 : 1); - } - avgPwm /= 4; - - if (avgPwm < -1.f) + if (brakeLightsStatus == brakeLightsOffSent) { - sendState("BRAKELIGHTSON"); - brakeLightsOffSent = false; - } - else if (!brakeLightsOffSent && avgPwm > -1.f) - { - sendState("BRAKELIGHTSOFF"); - brakeLightsOffSent = true; + if (brakeLightsStatus) + { + sendState("BRAKELIGHTSON"); + brakeLightsOffSent = false; + } + else if (!brakeLightsOffSent) + { + sendState("BRAKELIGHTSOFF"); + brakeLightsOffSent = true; + } } } } diff --git a/main/displays/menus/debugmenu.cpp b/main/displays/menus/debugmenu.cpp index 4eadaf9..327a546 100644 --- a/main/displays/menus/debugmenu.cpp +++ b/main/displays/menus/debugmenu.cpp @@ -62,6 +62,15 @@ constexpr char TEXT_ERASENVS[] = "Erase NVS (old)"; constexpr char TEXT_RESET_NVS_NEW[] = "Reset NVS (new)"; constexpr char TEXT_DYNAMICMENU[] = "GUI experiments"; constexpr char TEXT_BACK[] = "Back"; + +class AccelerationText : public virtual espgui::TextInterface +{ +public: + std::string text() const override + { + return fmt::format("Accel: {:.6f}", avgAccel); + } +}; } // namespace DebugMenu::DebugMenu() @@ -82,6 +91,7 @@ DebugMenu::DebugMenu() #ifdef FEATURE_CAN constructMenuItem, DisabledColor, DummyAction>>(); #endif + constructMenuItem, DisabledColor, DummyAction>>(); constructMenuItem>(); constructMenuItem, PushScreenAction>>(); constructMenuItem, PushScreenAction>>(); diff --git a/main/globals.cpp b/main/globals.cpp index 30ec1b1..279e2b9 100644 --- a/main/globals.cpp +++ b/main/globals.cpp @@ -15,8 +15,12 @@ float gametrakDist; float avgSpeed{}; float avgSpeedKmh{}; float sumCurrent{}; +float lastAvgSpeedKmh{}; +float avgAccel{}; bool isLocked{}; +espchrono::millis_clock::time_point lastAvgSpeedKmhTs{}; + std::optional sunrise; std::optional sunset; std::optional sunrise_dt; diff --git a/main/globals.h b/main/globals.h index 1a92e71..ecf7c5e 100644 --- a/main/globals.h +++ b/main/globals.h @@ -53,8 +53,12 @@ extern esp_pm_config_esp32_t pm_config; extern float avgSpeed; extern float avgSpeedKmh; +extern float lastAvgSpeedKmh; +extern float avgAccel; extern float sumCurrent; +extern espchrono::millis_clock::time_point lastAvgSpeedKmhTs; + extern bool isLocked; #ifdef GLOBALS_PLUGIN diff --git a/main/ledstrip.cpp b/main/ledstrip.cpp index 91a775c..6de2b05 100644 --- a/main/ledstrip.cpp +++ b/main/ledstrip.cpp @@ -19,6 +19,8 @@ std::vector leds; uint8_t gHue = 0; float gLedPosition = 0; // yes, this is intendet as a float value! Do NOT change! +bool brakeLightsStatus; + uint16_t blinkAnimation = LEDSTRIP_OVERWRITE_NONE; namespace { @@ -149,11 +151,12 @@ void updateLedStrip() } avgPwm /= 4; - if (avgPwm < -1.f) + // avgAccel in m/s/s + if (avgPwm < -1.f || (avgAccel < -0.001f && avgSpeedKmh > 5.f)) { auto color = avgSpeedKmh < -0.1f ? CRGB{255, 255, 255} : CRGB{255, 0, 0}; - const auto center = (std::begin(leds) + (leds.size() / 2) + configs.ledstrip.centerOffset.value()); + brakeLightsStatus = true; std::fill(std::begin(leds), std::end(leds), CRGB{0, 0, 0}); if (configs.ledstrip.enableFullBlink.value()) @@ -162,12 +165,15 @@ void updateLedStrip() } else if(!configs.ledstrip.enableAnimBlink.value()) { + const auto center = (std::begin(leds) + (leds.size() / 2) + configs.ledstrip.centerOffset.value()); + std::fill(center - configs.ledstrip.bigOffset.value() - 2, center - configs.ledstrip.smallOffset.value() + 2, color); std::fill(center + configs.ledstrip.smallOffset.value() - 2, center + configs.ledstrip.bigOffset.value() + 2, color); } } else { + brakeLightsStatus = false; showAnimation(); } } diff --git a/main/ledstrip.h b/main/ledstrip.h index 1f263c3..f9f7691 100644 --- a/main/ledstrip.h +++ b/main/ledstrip.h @@ -44,6 +44,8 @@ extern float gLedPosition; // yes, this is intendet as a float value! Do NOT cha extern uint16_t blinkAnimation; +extern bool brakeLightsStatus; + void showDefaultLedstrip(); void showAnimation(); void showBetterRainbow(); diff --git a/main/utils.cpp b/main/utils.cpp index 5ed372f..28ecb37 100644 --- a/main/utils.cpp +++ b/main/utils.cpp @@ -248,6 +248,28 @@ void updateAccumulators() sumCurrent = fixCurrent(sumCurrent); avgSpeedKmh = convertToKmh(avgSpeed); + + // accel + EVERY_N_MILLIS(100) + { + const auto now = espchrono::millis_clock::now(); + const auto delta = now - lastAvgSpeedKmhTs; + const auto deltaSpeedKmh = avgSpeedKmh - lastAvgSpeedKmh; + + // ESP_LOGI("utils.cpp", "delta: %lli ms, deltaSpeedKmh: %f", delta.count() / 1000, deltaSpeedKmh); + + const auto deltaMilliseconds = delta.count() / 1000.f; + + const auto kmh_s = deltaSpeedKmh / deltaMilliseconds; // km/h / s + const auto m_s2 = kmh_s * 1000.f / 3600.f; // m/s^2 + + // ESP_LOGI("utils.cpp", "m_s2: %f", m_s2); + + avgAccel = m_s2; + + lastAvgSpeedKmh = avgSpeedKmh; + lastAvgSpeedKmhTs = espchrono::millis_clock::now(); + } } float wattToAmpere(float watt) {