diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-07 13:36:55 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-07 13:36:55 +0400 |
commit | db37d3a4959c6f0888708bae4b4efd66c668e5b1 (patch) | |
tree | a6cec68bde14095366acc838ba423935ca112432 /src | |
parent | 04bca061a2bc722b22e37c8ea4f3bced7757db29 (diff) | |
download | px4-firmware-db37d3a4959c6f0888708bae4b4efd66c668e5b1.tar.gz px4-firmware-db37d3a4959c6f0888708bae4b4efd66c668e5b1.tar.bz2 px4-firmware-db37d3a4959c6f0888708bae4b4efd66c668e5b1.zip |
fmu driver: bugs fixed
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 45 |
1 files changed, 23 insertions, 22 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9a1da39cb..8aa4473d4 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -233,6 +233,7 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -584,28 +585,6 @@ PX4FMU::task_main() } } - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; - - if (_servo_armed != set_armed) - _servo_armed = set_armed; - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } - } - /* can we mix? */ if (_mixers != nullptr) { @@ -668,6 +647,28 @@ PX4FMU::task_main() } } + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + if (_servo_armed != set_armed) + _servo_armed = set_armed; + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } + } + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data |