Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -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

27
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -41,10 +41,9 @@ void PX4RCOutput::init(void* unused)
return; return;
} }
_pwm_sub = orb_subscribe(ORB_ID(actuator_outputs)); for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
_outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
// mark number of outputs given by px4io as zero }
_outputs.noutputs = 0;
_alt_fd = open("/dev/px4fmu", O_RDWR); _alt_fd = open("/dev/px4fmu", O_RDWR);
if (_alt_fd == -1) { 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, // if px4io has given us a value for this channel use that,
// otherwise use the value we last sent. This makes it easier to // otherwise use the value we last sent. This makes it easier to
// observe the behaviour of failsafe in px4io // observe the behaviour of failsafe in px4io
if (ch < _outputs.noutputs) { for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
return _outputs.output[ch]; 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]; return _period[ch];
} }
@ -362,10 +365,14 @@ void PX4RCOutput::_timer_tick(void)
} }
update_pwm: update_pwm:
bool rc_updated = false; for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
if (_pwm_sub >= 0 && orb_check(_pwm_sub, &rc_updated) == 0 && rc_updated) { bool rc_updated = false;
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; unsigned _alt_servo_count;
uint32_t _rate_mask; uint32_t _rate_mask;
uint16_t _enabled_channels; uint16_t _enabled_channels;
int _pwm_sub; struct {
actuator_outputs_s _outputs; int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed; actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = NULL; orb_advert_t _actuator_direct_pub = NULL;