[electrical] min bat level check

This allows for detecting when voltage is too low to be a real reading and treats it as invalid. The GCS shows BAT LOW INVALID. The ap does not set bat_low or bat_critical if the reading was never valid. But once a valid voltage is read then low level checking is always done. closes #745
paparazzi · Jun 10, 2014 · ebfb5ad · ebfb5ad
1 parent 9a51425
commit ebfb5ad
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 21 deletions.
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
@@ -52,8 +52,13 @@
 
 #define ELECTRICAL_PERIODIC_FREQ 10
 
+#ifndef MIN_BAT_LEVEL
+#define MIN_BAT_LEVEL 3
+#endif
+
 PRINT_CONFIG_VAR(LOW_BAT_LEVEL)
 PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL)
+PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
 
 struct Electrical electrical;
 
@@ -106,6 +111,7 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
 void electrical_periodic(void) {
   static uint8_t bat_low_counter = 0;
   static uint8_t bat_critical_counter = 0;
+  static bool_t vsupply_check_started = FALSE;
 
 #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
   electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample));
@@ -139,28 +145,35 @@ void electrical_periodic(void) {
   // mAh = mA * dt (10Hz -> hours)
   electrical.energy += ((float)electrical.current) / 3600.0f / ELECTRICAL_PERIODIC_FREQ;
 
-  if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
-    if (bat_low_counter > 0)
-      bat_low_counter--;
-    if (bat_low_counter == 0)
-      electrical.bat_low = TRUE;
-  }
-  else {
-    // reset battery low status and counter
-    bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
-    electrical.bat_low = FALSE;
+  /*if valid voltage is seen then start checking. Set min level to 0 to always start*/
+  if (electrical.vsupply >= MIN_BAT_LEVEL * 10) {
+      vsupply_check_started = TRUE;
   }
 
-  if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
-    if (bat_critical_counter > 0)
-      bat_critical_counter--;
-    if (bat_critical_counter == 0)
-      electrical.bat_critical = TRUE;
-  }
-  else {
-    // reset battery critical status and counter
-    bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
-    electrical.bat_critical = FALSE;
+  if (vsupply_check_started) {
+    if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
+      if (bat_low_counter > 0)
+        bat_low_counter--;
+      if (bat_low_counter == 0)
+        electrical.bat_low = TRUE;
+    }
+    else {
+      // reset battery low status and counter
+      bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+      electrical.bat_low = FALSE;
+    }
+
+    if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
+      if (bat_critical_counter > 0)
+        bat_critical_counter--;
+      if (bat_critical_counter == 0)
+        electrical.bat_critical = TRUE;
+    }
+    else {
+      // reset battery critical status and counter
+      bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+      electrical.bat_critical = FALSE;
+    }
   }
 
 }
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
@@ -511,7 +511,8 @@ let check_alerts = fun a ->
                "level", Pprz.String level;
                "value", Pprz.Float a.bat] in
     Alerts_Pprz.message_send my_id "BAT_LOW" vs in
-  if a.bat < catastrophic_level then send "CATASTROPHIC"
+  if a.bat < 1. then send "INVALID"
+  else if a.bat < catastrophic_level then send "CATASTROPHIC"
   else if a.bat < critic_level then send "CRITIC"
   else if a.bat < warning_level then send "WARNING"