From 5a666090a07c43c7ce0c97b5a9b4577b5f5508df Mon Sep 17 00:00:00 2001
From: Michael Ehrenreich <michideep@gmail.com>
Date: Thu, 13 Apr 2023 02:33:41 +0200
Subject: [PATCH] Implement undervoltage lockout

---
 CMakeLists.txt |  2 ++
 main.cpp       | 28 ++++++++++++++++++++++------
 uvlo.h         | 36 ++++++++++++++++++++++++++++++++++++
 3 files changed, 60 insertions(+), 6 deletions(-)
 create mode 100644 uvlo.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7ed119f..e45f235 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -276,6 +276,7 @@ target_compile_options(pcbv2_front.elf PRIVATE
         #    -DCAN_LOG_UNKNOWN_ADDR
         #     -DIS_BACK
         -DAMPERE2BIT_CONV=28
+        -DFEATURE_UVLO
 )
 add_custom_command(OUTPUT pcbv2_front.hex COMMAND arm-none-eabi-objcopy -O ihex pcbv2_front.elf pcbv2_front.hex DEPENDS pcbv2_front.elf)
 add_custom_command(OUTPUT pcbv2_front.bin COMMAND arm-none-eabi-objcopy -O binary -S pcbv2_front.elf pcbv2_front.bin DEPENDS pcbv2_front.elf)
@@ -303,6 +304,7 @@ target_compile_options(pcbv2_back.elf PRIVATE
         #    -DCAN_LOG_UNKNOWN_ADDR
         -DIS_BACK
         -DAMPERE2BIT_CONV=28
+        -DFEATURE_UVLO
 )
 add_custom_command(OUTPUT pcbv2_back.hex COMMAND arm-none-eabi-objcopy -O ihex pcbv2_back.elf pcbv2_back.hex DEPENDS pcbv2_back.elf)
 add_custom_command(OUTPUT pcbv2_back.bin COMMAND arm-none-eabi-objcopy -O binary -S pcbv2_back.elf pcbv2_back.bin DEPENDS pcbv2_back.elf)
diff --git a/main.cpp b/main.cpp
index c229ac9..933c84b 100644
--- a/main.cpp
+++ b/main.cpp
@@ -40,6 +40,10 @@
 #include "bobbycar-can.h"
 #endif
 
+#ifdef FEATURE_UVLO
+#include "uvlo.h"
+#endif
+
 extern "C" {
 #include "BLDC_controller.h"
 extern const P rtP_Left; // default settings defined in BLDC_controller_data.c
@@ -508,17 +512,25 @@ void updateMotors()
     const bool leftEnable = left.enable.load();
     const bool rightEnable = right.enable.load();
 
+    bool enableAny = timeoutVal <= 500;
+#ifdef FEATURE_UVLO
+    enableAny = enableAny && !UVLO::isLockedOut();
+#endif
+
+    bool enableL = enableAny && leftEnable && !chopL;
+    bool enableR = enableAny && rightEnable && !chopR;
+
     // Disable PWM when current limit is reached (current chopping)
     // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
-    if (chopL || timeoutVal > 500 || !leftEnable)
-        LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
-    else
+    if (enableL)
         LEFT_TIM->BDTR |= TIM_BDTR_MOE;
-
-    if (chopR || timeoutVal > 500 || !rightEnable)
-        RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
     else
+        LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
+
+    if (enableR)
         RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
+    else
+        RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
 
     // ############################### MOTOR CONTROL ###############################
 
@@ -1824,6 +1836,10 @@ void updateSensors()
     static int32_t batVoltageFixdt  = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20;
     filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, batVoltageFixdt);
     batVoltage = (int16_t)(batVoltageFixdt >> 20);  // convert fixed-point to integer
+
+#ifdef FEATURE_UVLO
+    UVLO::update(adc_buffer.batt1);
+#endif
 }
 
 void applyDefaultSettings()
diff --git a/uvlo.h b/uvlo.h
new file mode 100644
index 0000000..1ceae7c
--- /dev/null
+++ b/uvlo.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "config.h"
+
+namespace UVLO
+{
+static constexpr uint32_t NUM_MEASUREMENTS = 50;
+static constexpr uint32_t MIN_VOLTAGE = 1500 * BAT_CALIB_ADC / BAT_CALIB_REAL_VOLTAGE;
+
+static uint32_t measurements = 0;
+static uint32_t sum = 0;
+static bool locked_out = true;
+
+static void update(uint16_t voltage)
+{
+    if (measurements < NUM_MEASUREMENTS)
+    {
+        sum += voltage;
+        measurements++;
+
+        if (measurements == NUM_MEASUREMENTS && sum / NUM_MEASUREMENTS >= MIN_VOLTAGE)
+            locked_out = false;
+    }
+}
+
+static bool isDone()
+{
+    return measurements >= NUM_MEASUREMENTS;
+}
+
+static bool isLockedOut()
+{
+    return locked_out;
+}
+
+} // namespace