diff options
author | Julian Oes <julian@oes.ch> | 2013-10-11 12:11:25 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-10-11 12:11:25 +0200 |
commit | 326f241185f45d9e2d4377e8096a8a2f05f65b0d (patch) | |
tree | 3db0232487aeb8ffdf0783d24a3a874bcb15d06a /src | |
parent | 9cd3c40606f023a7943b1418a748abb046e36ecb (diff) | |
download | px4-firmware-326f241185f45d9e2d4377e8096a8a2f05f65b0d.tar.gz px4-firmware-326f241185f45d9e2d4377e8096a8a2f05f65b0d.tar.bz2 px4-firmware-326f241185f45d9e2d4377e8096a8a2f05f65b0d.zip |
Enable PWM when disarmed on the fmu side
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 27 |
1 files changed, 23 insertions, 4 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7f30487bf..8e9e8103f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -118,6 +118,7 @@ private: volatile bool _task_should_exit; bool _armed; + bool _pwm_on; MixerGroup *_mixers; @@ -127,6 +128,7 @@ private: uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + unsigned _num_disarmed_set; static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -209,10 +211,12 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), + _pwm_on(false), _mixers(nullptr), _disarmed_pwm({0}), _min_pwm({PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}), - _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}) + _max_pwm({PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX}), + _num_disarmed_set(0) { _debug_enabled = true; } @@ -585,11 +589,16 @@ PX4FMU::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* update PWM servo armed status if armed and not locked down */ + /* update the armed status and check that we're not locked down */ bool set_armed = aa.armed && !aa.lockdown; - if (set_armed != _armed) { + if (_armed != set_armed) _armed = set_armed; - up_pwm_servo_arm(set_armed); + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); } } @@ -738,6 +747,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) _disarmed_pwm[i] = pwm->values[i]; } } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } break; } case PWM_SERVO_GET_DISARMED_PWM: { |