HAL_PX4: use px4io generated values for servo output readback

this makes it easier to observe the behaviour of IO failsafe

libraries/AP_HAL_PX4/RCOutput.cpp

@@ -39,6 +39,11 @@ void PX4RCOutput::init(void* unused)
         return;
     }
 
+	_pwm_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
+
+    // mark number of outputs given by px4io as zero
+    _outputs.noutputs = 0;
+
     _alt_fd = open("/dev/px4fmu", O_RDWR);
     if (_alt_fd == -1) {
         hal.console->printf("RCOutput: failed to open /dev/px4fmu");
@@ -218,6 +223,12 @@ uint16_t PX4RCOutput::read(uint8_t ch)
     if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
         return 0;
     }
+    // if px4io has given us a value for this channel use that,
+    // otherwise use the value we last sent. This makes it easier to
+    // observe the behaviour of failsafe in px4io
+    if (ch < _outputs.noutputs) {
+        return _outputs.output[ch];
+    }
     return _period[ch];
 }
 
@@ -257,6 +268,13 @@ void PX4RCOutput::_timer_tick(void)
         perf_end(_perf_rcout);
         _last_output = now;
     }
+
+update_pwm:
+	bool rc_updated = false;
+	if (_pwm_sub >= 0 && orb_check(_pwm_sub, &rc_updated) == 0 && rc_updated) {
+        orb_copy(ORB_ID_VEHICLE_CONTROLS, _pwm_sub, &_outputs);
+	}
+
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/RCOutput.h

@@ -4,6 +4,7 @@
 
 #include <AP_HAL_PX4.h>
 #include <systemlib/perf_counter.h>
+#include <uORB/topics/actuator_outputs.h>
 
 #define PX4_NUM_OUTPUT_CHANNELS 16
 
@@ -39,6 +40,8 @@ private:
     unsigned _alt_servo_count;
     uint32_t _rate_mask;
     uint16_t _enabled_channels;
+    int _pwm_sub;
+    actuator_outputs_s _outputs;
 
     void _init_alt_channels(void);
 };