HAL_PX4: use px4io generated values for servo output readback

this makes it easier to observe the behaviour of IO failsafe
This commit is contained in:
Andrew Tridgell 2014-11-05 21:38:12 +11:00
parent 95827e59c1
commit 7e3b8a30f5
2 changed files with 21 additions and 0 deletions

View File

@ -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

View File

@ -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);
};