VTOL attitude controller: run update_vtol_state only if mc or fw att …

…sp changed, such that pusher support in hover works again

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -354,25 +354,33 @@ VtolAttitudeControl::Run()
 		_land_detected_sub.update(&_land_detected);
 		vehicle_cmd_poll();
 
-		// update the vtol state machine which decides which mode we are in
-		_vtol_type->update_vtol_state();
-
-		// reset transition command if not auto control
-		if (_v_control_mode.flag_control_manual_enabled) {
-			if (_vtol_type->get_mode() == mode::ROTARY_WING) {
-				_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-
-			} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
-				_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+		// check if mc and fw sp were updated
+		bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
+		bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
 
-			} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
-				/* We want to make sure that a mode change (manual>auto) during the back transition
-				 * doesn't result in an unsafe state. This prevents the instant fall back to
-				 * fixed-wing on the switch from manual to auto */
-				_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		// update the vtol state machine which decides which mode we are in
+		if (mc_att_sp_updated || fw_att_sp_updated) {
+			_vtol_type->update_vtol_state();
+
+			// reset transition command if not auto control
+			if (_v_control_mode.flag_control_manual_enabled) {
+				if (_vtol_type->get_mode() == mode::ROTARY_WING) {
+					_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+
+				} else if (_vtol_type->get_mode() == mode::FIXED_WING) {
+					_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+
+				} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
+					/* We want to make sure that a mode change (manual>auto) during the back transition
+					 * doesn't result in an unsafe state. This prevents the instant fall back to
+					 * fixed-wing on the switch from manual to auto */
+					_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+				}
 			}
 		}
 
+
+
 		// check in which mode we are in and call mode specific functions
 		switch (_vtol_type->get_mode()) {
 		case mode::TRANSITION_TO_FW:
@@ -384,7 +392,7 @@ VtolAttitudeControl::Run()
 
 			_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
 
-			if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 
 				// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
 				if (!_v_control_mode.flag_armed) {
@@ -406,21 +414,19 @@ VtolAttitudeControl::Run()
 			_vtol_vehicle_status.vtol_in_trans_mode = false;
 			_vtol_vehicle_status.in_transition_to_fw = false;
 
-			// got data from mc attitude controller
-			if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) {
-
+			if (mc_att_sp_updated) {
 				// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
 				if (!_v_control_mode.flag_armed) {
 					Quatf().copyTo(_mc_virtual_att_sp.q_d);
 					Vector3f().copyTo(_mc_virtual_att_sp.thrust_body);
 					Quatf().copyTo(_v_att_sp.q_d);
 					Vector3f().copyTo(_v_att_sp.thrust_body);
 				}
-
-				_vtol_type->update_mc_state();
-				_v_att_sp_pub.publish(_v_att_sp);
 			}
 
+			_vtol_type->update_mc_state();
+			_v_att_sp_pub.publish(_v_att_sp);
+
 			break;
 
 		case mode::FIXED_WING:
@@ -429,7 +435,7 @@ VtolAttitudeControl::Run()
 			_vtol_vehicle_status.vtol_in_trans_mode = false;
 			_vtol_vehicle_status.in_transition_to_fw = false;
 
-			if (_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp)) {
+			if (fw_att_sp_updated) {
 				_vtol_type->update_fw_state();
 				_v_att_sp_pub.publish(_v_att_sp);
 			}