Reverting 2e8accc6ff infavor of IO init changes

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);
 		}
 	}