diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 76084c85ae..d8a4fecb2b 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/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 diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index 446f5acfee..680a369722 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -4,6 +4,7 @@ #include #include +#include #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); };