Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: cope with multi-instance actuator_outputs uORB

Browse Source 
we need to subscribe to all of them
This commit is contained in:
Andrew Tridgell 2015-06-04 11:00:34 +10:00
parent a3334e0602
commit 6a33aeef67
2 changed files with 21 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -41,10 +41,9 @@ void PX4RCOutput::init(void* unused)
return;
}
_pwm_sub = orb_subscribe(ORB_ID(actuator_outputs));
// mark number of outputs given by px4io as zero
_outputs.noutputs = 0;
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
_outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
}
_alt_fd = open("/dev/px4fmu", O_RDWR);
if (_alt_fd == -1) {
@ -251,8 +250,12 @@ uint16_t PX4RCOutput::read(uint8_t ch)
// 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];
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
if (_outputs[i].pwm_sub >= 0 &&
ch < _outputs[i].outputs.noutputs &&
_outputs[i].outputs.output[ch] > 0) {
return _outputs[i].outputs.output[ch];
}
}
return _period[ch];
}
@ -362,9 +365,13 @@ void PX4RCOutput::_timer_tick(void)
}
update_pwm:
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
bool rc_updated = false;
if (_pwm_sub >= 0 && orb_check(_pwm_sub, &rc_updated) == 0 && rc_updated) {
orb_copy(ORB_ID(actuator_outputs), _pwm_sub, &_outputs);
if (_outputs[i].pwm_sub >= 0 &&
orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 &&
rc_updated) {
orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs);
}
}
}

6
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -45,8 +45,10 @@ private:
unsigned _alt_servo_count;
uint32_t _rate_mask;
uint16_t _enabled_channels;
int _pwm_sub;
actuator_outputs_s _outputs;
struct {
int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = NULL;