AP_HAL: added servorail_voltage and power_status_flags() API on AnalogIn

libraries/AP_HAL/AnalogIn.h

@@ -44,6 +44,12 @@ public:
 
     // board 5V rail voltage in volts
     virtual float board_voltage(void) = 0;
+
+    // servo rail voltage in volts, or 0 if unknown
+    virtual float servorail_voltage(void) { return 0; }
+
+    // power supply status flags, see MAV_POWER_STATUS
+    virtual uint16_t power_status_flags(void) { return 0; }
 };
 
 #define ANALOG_INPUT_BOARD_VCC 254

libraries/AP_HAL_PX4/AnalogIn.cpp

@@ -15,6 +15,9 @@
 #include <arch/board/board.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/servorail_status.h>
+#include <uORB/topics/system_power.h>
+#include <GCS_MAVLink.h>
+#include <errno.h>
 
 #define ANLOGIN_DEBUGGING 0
 
@@ -181,7 +184,9 @@ void PX4AnalogSource::_add_value(float v, float vcc5V)
 
 
 PX4AnalogIn::PX4AnalogIn() :
-    _board_voltage(0)
+	_board_voltage(0),
+    _servorail_voltage(0),
+    _power_flags(0)    
 {}
 
 void PX4AnalogIn::init(void* machtnichts)
@@ -192,6 +197,7 @@ void PX4AnalogIn::init(void* machtnichts)
 	}
     _battery_handle   = orb_subscribe(ORB_ID(battery_status));
     _servorail_handle = orb_subscribe(ORB_ID(servorail_status));
+    _system_power_handle = orb_subscribe(ORB_ID(system_power));
 }
 
 /*
@@ -239,8 +245,10 @@ void PX4AnalogIn::_timer_tick(void)
     // check for new battery data on FMUv1
     if (_battery_handle != -1) {
         struct battery_status_s battery;
-        if (orb_copy(ORB_ID(battery_status), _battery_handle, &battery) == OK &&
-            battery.timestamp != _battery_timestamp) {
+        bool updated = false;
+        if (orb_check(_battery_handle, &updated) == 0 && updated) {
+            orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
+            if (battery.timestamp != _battery_timestamp) {
                 _battery_timestamp = battery.timestamp;
                 for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
                     PX4::PX4AnalogSource *c = _channels[j];
@@ -256,15 +264,19 @@ void PX4AnalogIn::_timer_tick(void)
                 }
             }
         }
+    }
 #endif
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
     // check for new servorail data on FMUv2
     if (_servorail_handle != -1) {
         struct servorail_status_s servorail;
-        if (orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail) == OK &&
-            servorail.timestamp != _servorail_timestamp) {
+        bool updated = false;
+        if (orb_check(_servorail_handle, &updated) == 0 && updated) {
+            orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail);
+            if (servorail.timestamp != _servorail_timestamp) {
                 _servorail_timestamp = servorail.timestamp;
+                _servorail_voltage = servorail.voltage_v;
                 for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
                     PX4::PX4AnalogSource *c = _channels[j];
                     if (c == NULL) continue;
@@ -277,6 +289,25 @@ void PX4AnalogIn::_timer_tick(void)
                 }
             }
         }
+    }
+    if (_system_power_handle != -1) {
+        struct system_power_s system_power;
+        bool updated = false;
+        if (orb_check(_system_power_handle, &updated) == 0 && updated) {
+            orb_copy(ORB_ID(system_power), _system_power_handle, &system_power);
+            uint16_t flags = 0;
+            if (system_power.usb_connected) flags |= MAV_POWER_STATUS_USB_CONNECTED;
+            if (system_power.brick_valid)   flags |= MAV_POWER_STATUS_BRICK_VALID;
+            if (system_power.servo_valid)   flags |= MAV_POWER_STATUS_SERVO_VALID;
+            if (system_power.periph_5V_OC)  flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
+            if (system_power.hipower_5V_OC) flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
+            if (_power_flags != 0 && _power_flags != flags) {
+                // the power status has changed since boot
+                flags |= MAV_POWER_STATUS_CHANGED;
+            }
+            _power_flags = flags;
+        }
+    }
 #endif
 
 }

libraries/AP_HAL_PX4/AnalogIn.h

@@ -57,15 +57,20 @@ public:
     AP_HAL::AnalogSource* channel(int16_t pin);
     void _timer_tick(void);
     float board_voltage(void) { return _board_voltage; }
+    float servorail_voltage(void) { return _servorail_voltage; }
+    uint16_t power_status_flags(void) { return _power_flags; }
 
 private:
     int _adc_fd;
     int _battery_handle;
     int _servorail_handle;
+    int _system_power_handle;
     uint64_t _battery_timestamp;
     uint64_t _servorail_timestamp;
     PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS];
     uint32_t _last_run;
     float _board_voltage;
+    float _servorail_voltage;
+    uint16_t _power_flags;
 };
 #endif // __AP_HAL_PX4_ANALOGIN_H__