Implement undervoltage lockout

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)

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)
+    if (enableL)
-        LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
-    else
         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()

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