PX4: support getting servorail voltage from ORB

libraries/AP_HAL_PX4/AnalogIn.cpp

@@ -13,6 +13,8 @@
 #include <nuttx/analog/adc.h>
 #include <nuttx/config.h>
 #include <arch/board/board.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 
 #define ANLOGIN_DEBUGGING 0
 
@@ -163,7 +165,8 @@ void PX4AnalogIn::init(void* machtnichts)
     if (_adc_fd == -1) {
         hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH);
 	}
-    _battery_handle = orb_subscribe(ORB_ID(battery_status));
+    _battery_handle   = orb_subscribe(ORB_ID(battery_status));
+    _servorail_handle = orb_subscribe(ORB_ID(servorail_status));
 }
 
 /*
@@ -208,10 +211,10 @@ void PX4AnalogIn::_timer_tick(void)
             for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
                 PX4::PX4AnalogSource *c = _channels[j];
                 if (c == NULL) continue;
-                if (c->_pin == PX4_ANALOG_BATTERY_VOLTAGE_PIN) {
+                if (c->_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
                     c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING);
                 }
-                if (c->_pin == PX4_ANALOG_BATTERY_CURRENT_PIN) {
+                if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) {
                     // scale it back to voltage, knowing that the
                     // px4io code scales by 90.0/5.0
                     c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING);
@@ -221,6 +224,27 @@ 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) {
+            _servorail_timestamp = servorail.timestamp;
+            for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
+                PX4::PX4AnalogSource *c = _channels[j];
+                if (c == NULL) continue;
+                if (c->_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) {
+                    c->_add_value(servorail.voltage_v / PX4_VOLTAGE_SCALING);
+                }
+                if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) {
+                    c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING);
+                }
+            }
+        }
+    }
+#endif
+
 }
 
 AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin) 

libraries/AP_HAL_PX4/AnalogIn.h

@@ -6,7 +6,6 @@
 #include <AP_HAL_PX4.h>
 #include <pthread.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/battery_status.h>
 
 #define PX4_ANALOG_MAX_CHANNELS 16
 
@@ -17,6 +16,8 @@
 #define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN     101
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 #define PX4_ANALOG_VCC_5V_PIN                4
+#define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN       102
+#define PX4_ANALOG_ORB_SERVO_VRSSI_PIN         103
 #endif
 
 class PX4::PX4AnalogSource : public AP_HAL::AnalogSource {
@@ -57,7 +58,9 @@ public:
 private:
     int _adc_fd;
     int _battery_handle;
+    int _servorail_handle;
     uint64_t _battery_timestamp;
+    uint64_t _servorail_timestamp;
     PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS];
     uint32_t _last_run;
 };