From 1843eea917591033daeeb93b2bd8e1f0e70ac66d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 25 Jan 2016 10:43:50 -1000 Subject: [PATCH] Reverting 2e8accc6ffd5e1c2f37d31b09d6968a1e4af31ba infavor of IO init changes --- 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 67f9bec9ad..7c125d75a1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -161,8 +161,6 @@ private: volatile bool _initialized; bool _throttle_armed; bool _pwm_on; - bool _pwm_init; - uint8_t _pwm_channel_mask; MixerGroup *_mixers; @@ -315,8 +313,6 @@ 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), @@ -422,7 +418,10 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - _pwm_channel_mask = 0x3; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -433,7 +432,10 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - _pwm_channel_mask = 0xf; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -444,7 +446,10 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - _pwm_channel_mask = 0x3f; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3f); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -456,7 +461,10 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - _pwm_channel_mask = 0xff; + + /* XXX magic numbers */ + up_pwm_servo_init(0xff); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; #endif @@ -469,7 +477,6 @@ PX4FMU::set_mode(Mode mode) /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); - _pwm_init = false; break; @@ -866,13 +873,6 @@ 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); } }