Use battery charge to set QRP

3 files changed, 50 insertions, 5 deletions

diff --git a/src/common/Core/main.c b/src/common/Core/main.c
index bb072c8..f2aaf93 100644
--- a/src/common/Core/main.c
+++ b/src/common/Core/main.c
@@ -301,15 +301,30 @@ static void launcher_task(void __attribute__ ((unused))*pvParameters)
             }
         }
         else {
-            const int qrp_from_supply = analog_supply_too_low();
             if (swr_error_flag) {
                 pio_set_qrp(1);
             }
-            else if (qrp_from_supply != last_qrp_from_supply) {
-                usart_debug("QRP = %d\r\n", qrp_from_supply);
-                last_qrp_from_supply = qrp_from_supply;
+            else {
+                const uint32_t charge_qrp = batterycharge_too_low();
+
+                if (charge_qrp != -1) {
+                    if (charge_qrp != last_qrp_from_supply) {
+                        usart_debug("QRP CC = %d\r\n", charge_qrp);
+                        last_qrp_from_supply = charge_qrp;
+
+                        pio_set_qrp(charge_qrp);
+                    }
+                }
+                else {
+                    /* Read the voltage when battery capacity is not available */
+                    const int qrp_from_supply = analog_supply_too_low();
+                    if (qrp_from_supply != last_qrp_from_supply) {
+                        usart_debug("QRP U = %d\r\n", qrp_from_supply);
+                        last_qrp_from_supply = qrp_from_supply;
 
-                pio_set_qrp(qrp_from_supply);
+                        pio_set_qrp(qrp_from_supply);
+                    }
+                }
             }
         }
 
diff --git a/src/common/GPIO/batterycharge.c b/src/common/GPIO/batterycharge.c
index 95176cd..d68a805 100644
--- a/src/common/GPIO/batterycharge.c
+++ b/src/common/GPIO/batterycharge.c
@@ -29,8 +29,13 @@
 #include <stdlib.h>
 #include <errno.h>
 
+// Hysteresis:
+#define CHARGE_QRP 1300000
+#define CHARGE_QRO 1350000
+
 static int charge_valid;
 static uint32_t last_charge;
+static int charge_qrp;
 
 void batterycharge_init()
 {
@@ -39,6 +44,7 @@ void batterycharge_init()
      */
     charge_valid = 0;
     last_charge = 0;
+    charge_qrp = 0;
 }
 
 void batterycharge_push_message(const char *ccounter_msg)
@@ -85,3 +91,24 @@ uint32_t batterycharge_retrieve_last_capacity()
     taskENABLE_INTERRUPTS();
     return ret;
 }
+
+int batterycharge_too_low()
+{
+    taskDISABLE_INTERRUPTS();
+    uint32_t c = 0;
+    if (charge_valid) {
+        c = last_charge;
+    }
+    taskENABLE_INTERRUPTS();
+
+    if (c == 0) {
+        return -1;
+    }
+    else if (charge_qrp == 0 && c < CHARGE_QRP) {
+        charge_qrp = 1;
+    }
+    else if (charge_qrp == 1 && c >= CHARGE_QRO) {
+        charge_qrp = 0;
+    }
+    return charge_qrp;
+}
diff --git a/src/common/GPIO/batterycharge.h b/src/common/GPIO/batterycharge.h
index 2d1e198..6785d79 100644
--- a/src/common/GPIO/batterycharge.h
+++ b/src/common/GPIO/batterycharge.h
@@ -33,3 +33,6 @@ void batterycharge_push_message(const char *ccounter_msg);
 
 // Get the last received battery capacity in mAh, or 0 if not valid
 uint32_t batterycharge_retrieve_last_capacity(void);
+
+// Return 1 if battery charge is too low, -1 if unknown
+int batterycharge_too_low(void);