vtol_att_control: move handling of motor state and idle speed selection

to VtolType class Signed-off-by: Roman <[email protected]>
PX4 · Apr 12, 2018 · 48302f7 · 48302f7
1 parent bfb39b6
commit 48302f7
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 39 deletions.
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
@@ -314,11 +314,6 @@ void Standard::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	// enable MC motors here in case we transitioned directly to MC mode
-	if (_motor_state != ENABLED) {
-		_motor_state = set_motor_state(_motor_state, ENABLED);
-	}
-
 	// if the thrust scale param is zero or the drone is on manual mode,
 	// then the pusher-for-pitch strategy is disabled and we can return
 	if (_params_standard.forward_thrust_scale < FLT_EPSILON ||
@@ -394,11 +389,6 @@ void Standard::update_mc_state()
 void Standard::update_fw_state()
 {
 	VtolType::update_fw_state();
-
-	// stop MC motors in FW mode
-	if (_motor_state != DISABLED) {
-		_motor_state = VtolType::set_motor_state(_motor_state, DISABLED);
-	}
 }
 
 /**

diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp
@@ -257,20 +257,11 @@ void Tailsitter::waiting_on_tecs()
 void Tailsitter::update_mc_state()
 {
 	VtolType::update_mc_state();
-
-	// set idle speed for rotary wing mode
-	if (!flag_idle_mc) {
-		flag_idle_mc = set_idle_mc();
-	}
 }
 
 void Tailsitter::update_fw_state()
 {
 	VtolType::update_fw_state();
-
-	if (flag_idle_mc) {
-		flag_idle_mc = !set_idle_fw();
-	}
 }
 
 /**

diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -216,16 +216,6 @@ void Tiltrotor::update_mc_state()
 
 	// make sure motors are not tilted
 	_tilt_control = _params_tiltrotor.tilt_mc;
-
-	// enable rear motors
-	if (_motor_state != ENABLED) {
-		_motor_state = set_motor_state(_motor_state, ENABLED);
-	}
-
-	// set idle speed for rotary wing mode
-	if (!flag_idle_mc) {
-		flag_idle_mc = set_idle_mc();
-	}
 }
 
 void Tiltrotor::update_fw_state()
@@ -234,16 +224,6 @@ void Tiltrotor::update_fw_state()
 
 	// make sure motors are tilted forward
 	_tilt_control = _params_tiltrotor.tilt_fw;
-
-	// disable rear motors
-	if (_motor_state != DISABLED) {
-		_motor_state = set_motor_state(_motor_state, DISABLED);
-	}
-
-	// adjust idle for fixed wing flight
-	if (flag_idle_mc) {
-		flag_idle_mc = !set_idle_fw();
-	}
 }
 
 void Tiltrotor::update_transition_state()

diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
@@ -115,6 +115,14 @@ bool VtolType::init()
 
 void VtolType::update_mc_state()
 {
+	if (!flag_idle_mc) {
+		flag_idle_mc = set_idle_mc();
+	}
+
+	if (_motor_state != ENABLED) {
+		_motor_state = VtolType::set_motor_state(_motor_state, ENABLED);
+	}
+
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 
@@ -143,6 +151,14 @@ void VtolType::update_mc_state()
 
 void VtolType::update_fw_state()
 {
+	if (flag_idle_mc) {
+		flag_idle_mc = !set_idle_fw();
+	}
+
+	if (_motor_state != DISABLED) {
+		_motor_state = VtolType::set_motor_state(_motor_state, DISABLED);
+	}
+
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 	_mc_roll_weight = 0.0f;