abort front transition in vtol module instead of in navigator/rtl.cpp

src/modules/navigator/rtl.cpp

@@ -229,8 +229,7 @@ void RTL::find_RTL_destination()
 	}
 
 	if (_param_rtl_cone_half_angle_deg.get() > 0
-	    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+	    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-	    && !_navigator->get_vstatus()->in_transition_to_fw) {
 		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
 	} else {
@@ -242,15 +241,6 @@ void RTL::on_activation()
 {
 	_rtl_state = RTL_STATE_NONE;
 
-	// send a back transition command in case we're doing a front transition to abort the FT
-	if (_navigator->get_vstatus()->in_transition_to_fw) {
-		vehicle_command_s cmd {};
-		cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
-		cmd.param1 = (float)(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-		cmd.param2 = 0.0f;
-		_navigator->publish_vehicle_cmd(&cmd);
-	}
-
 	// if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode
 	// In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission
 	_should_engange_mission_for_landing = (_destination.type == RTL_DESTINATION_MISSION_LANDING)

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -105,6 +105,19 @@ VtolAttitudeControl::init()
 	return true;
 }
 
+void VtolAttitudeControl::vehicle_status_poll()
+{
+	_vehicle_status_sub.copy(&_vehicle_status);
+
+	// abort front transition when RTL is triggered
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+	    && _nav_state_prev != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _vtol_type->get_mode() == mode::TRANSITION_TO_FW) {
+		_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+	}
+
+	_nav_state_prev = _vehicle_status.nav_state;
+}
+
 void VtolAttitudeControl::action_request_poll()
 {
 	while (_action_request_sub.updated()) {
@@ -132,8 +145,6 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 
 	while (_vehicle_cmd_sub.update(&vehicle_command)) {
 		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
 
 			uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -141,10 +152,10 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 
 			// deny transition from MC to FW in Takeoff, Land, RTL and Orbit
 			if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
-			    (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
+			    (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
-			     || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
-			     || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+			     || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
-			     ||  vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
+			     ||  _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
 
 				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
@@ -309,6 +320,7 @@ VtolAttitudeControl::Run()
 		_airspeed_validated_sub.update(&_airspeed_validated);
 		_tecs_status_sub.update(&_tecs_status);
 		_land_detected_sub.update(&_land_detected);
+		vehicle_status_poll();
 		action_request_poll();
 		vehicle_cmd_poll();
 

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -210,6 +210,7 @@ private:
 	vehicle_land_detected_s			_land_detected{};
 	vehicle_local_position_s		_local_pos{};
 	vehicle_local_position_setpoint_s	_local_pos_sp{};
+	vehicle_status_s 			_vehicle_status{};
 	vtol_vehicle_status_s 			_vtol_vehicle_status{};
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};	// [kg/m^3]
@@ -222,12 +223,16 @@ private:
 	int		_transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC};
 	bool		_immediate_transition{false};
 
+	uint8_t _nav_state_prev;
+
 	VtolType	*_vtol_type{nullptr};	// base class for different vtol types
 
 	bool		_initialized{false};
 
 	perf_counter_t	_loop_perf;		// loop performance counter
 
+	void		vehicle_status_poll();
+
 	void		action_request_poll();
 
 	void		vehicle_cmd_poll();