diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8b930d2213..d4de6c18dd 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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()->in_transition_to_fw) { + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _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) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index fbd0703972..c129208fab 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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_LAND - || 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_AUTO_TAKEOFF + || _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_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(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 7cfb6e09e0..2c96f3a385 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/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();