From 2e8accc6ffd5e1c2f37d31b09d6968a1e4af31ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 12:08:08 +0100 Subject: [PATCH] FMU driver: Only init pins right before using the for PWM. Prevents accidental pulses --- src/drivers/px4fmu/fmu.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7c125d75a1..67f9bec9ad 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -161,6 +161,8 @@ private: volatile bool _initialized; bool _throttle_armed; bool _pwm_on; + bool _pwm_init; + uint8_t _pwm_channel_mask; MixerGroup *_mixers; @@ -313,6 +315,8 @@ PX4FMU::PX4FMU() : _initialized(false), _throttle_armed(false), _pwm_on(false), + _pwm_init(false), + _pwm_channel_mask(0), _mixers(nullptr), _groups_required(0), _groups_subscribed(0), @@ -418,10 +422,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3; break; @@ -432,10 +433,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xf); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xf; break; @@ -446,10 +444,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3f); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3f; break; @@ -461,10 +456,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xff); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xff; break; #endif @@ -477,6 +469,7 @@ PX4FMU::set_mode(Mode mode) /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); + _pwm_init = false; break; @@ -873,6 +866,13 @@ PX4FMU::cycle() if (_pwm_on != pwm_on) { _pwm_on = pwm_on; + + if (!_pwm_init && _pwm_on) { + up_pwm_servo_init(_pwm_channel_mask); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_init = true; + } + up_pwm_servo_arm(pwm_on); } }