From b4aeffbaed7af5baf2f2542d5be40655b979fbf8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 8 Apr 2018 19:30:32 +1000
Subject: [PATCH] HAL_ChibiOS: added power status flags reporting

---
 libraries/AP_HAL_ChibiOS/AnalogIn.cpp         | 47 +++++++++++++++++++
 libraries/AP_HAL_ChibiOS/AnalogIn.h           |  3 ++
 .../AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat      |  8 ++++
 3 files changed, 58 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
index 8245efb640..f9202f1238 100644
--- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
+++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
@@ -256,6 +256,9 @@ void AnalogIn::_timer_tick(void)
 
     /* read all channels available */
     read_adc(buf_adc);
+
+    // update power status flags
+    update_power_flags();
     
     // match the incoming channels to the currently active pins
     for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
@@ -310,3 +313,47 @@ AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin)
     return nullptr;
 }
 
+/*
+  update power status flags
+ */
+void AnalogIn::update_power_flags(void)
+{
+    uint16_t flags = 0;
+
+#ifdef HAL_GPIO_PIN_VDD_BRICK_VALID
+    if (!palReadLine(HAL_GPIO_PIN_VDD_BRICK_VALID)) {
+        flags |= MAV_POWER_STATUS_BRICK_VALID;
+    }
+#endif
+    
+#ifdef HAL_GPIO_PIN_VDD_SERVO_VALID
+    if (!palReadLine(HAL_GPIO_PIN_VDD_SERVO_VALID)) {
+        flags |= MAV_POWER_STATUS_SERVO_VALID;
+    }
+#endif
+    
+#ifdef HAL_GPIO_PIN_VBUS
+	if (palReadLine(HAL_GPIO_PIN_VBUS)) {
+        flags |= MAV_POWER_STATUS_USB_CONNECTED;
+    }
+#endif
+    
+#ifdef HAL_GPIO_PIN_VDD_5V_HIPOWER_OC
+    if (!palReadLine(HAL_GPIO_PIN_VDD_5V_HIPOWER_OC)) {
+        flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
+    }    
+#endif
+
+#ifdef HAL_GPIO_PIN_VDD_5V_PERIPH_OC
+    if (!palReadLine(HAL_GPIO_PIN_VDD_5V_PERIPH_OC)) {
+        flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
+    }    
+#endif
+    if (_power_flags != 0 && 
+        _power_flags != flags && 
+        hal.util->get_soft_armed()) {
+        // the power status has changed while armed
+        flags |= MAV_POWER_STATUS_CHANGED;
+    }
+    _power_flags = flags;
+}
diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h
index 02e7655f41..86bca94e9c 100644
--- a/libraries/AP_HAL_ChibiOS/AnalogIn.h
+++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h
@@ -62,8 +62,11 @@ public:
     float servorail_voltage(void) override { return _servorail_voltage; }
     uint16_t power_status_flags(void) override { return _power_flags; }
     static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n);
+
 private:
     void read_adc(uint32_t *val);
+    void update_power_flags(void);
+    
     int _battery_handle;
     int _servorail_handle;
     int _system_power_handle;
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
index 2a3b46aecd..6b61ceaea6 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat
@@ -374,6 +374,14 @@ PE15 VDD_5V_PERIPH_OC INPUT
 # define a LED, mapping it to GPIO(0)
 PE12 FMU_LED_AMBER OUTPUT GPIO(0)
 
+# power flag pins. These tell the MCU the status of the various power
+# supplies that are available. The pin names need to exactly match the
+# names used in AnalogIn.cpp. 
+PB5 VDD_BRICK_VALID INPUT PULLUP
+PB7 VDD_SERVO_VALID INPUT PULLUP
+PE10 VDD_5V_HIPOWER_OC INPUT PULLUP
+PE15 VDD_5V_PERIPH_OC INPUT PULLUP
+
 # now the SPI device table. This table creates all accessible SPI
 # devices, giving the name of the device (which is used by device
 # drivers to open the device), plus which SPI bus it it on, what