diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 0444a577e1..cdc348b499 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -55,6 +55,7 @@ bool is_rotary_wing # True if system is in rotary wing configuration, so for a bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW bool rc_signal_lost # true if RC reception lost uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 5334e291ca..1e544ddc9a 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -7,6 +7,7 @@ uint8 VEHICLE_VTOL_STATE_FW = 4 bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode bool vtol_in_trans_mode +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW bool vtol_transition_failsafe # vtol in transition failsafe mode bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode float32 airspeed_tot # Estimated airspeed over control surfaces diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7271ff93e0..f94753e7fd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1946,6 +1946,7 @@ int commander_thread_main(int argc, char *argv[]) if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; + status.in_transition_to_fw = vtol_status.in_transition_to_fw; status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe; status_flags.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5db897e99a..59ac1ecd9f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3196,21 +3196,17 @@ protected: updated = true; if (status.is_vtol) { - if (status.is_rotary_wing) { - if (status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; + if (!status.in_transition_mode && status.is_rotary_wing) { + _msg.vtol_state = MAV_VTOL_STATE_MC; - } else { - _msg.vtol_state = MAV_VTOL_STATE_MC; - } + } else if (!status.in_transition_mode){ + _msg.vtol_state = MAV_VTOL_STATE_FW; - } else { - if (status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; + } else if (status.in_transition_mode && status.in_transition_to_fw) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; - } else { - _msg.vtol_state = MAV_VTOL_STATE_FW; - } + } else if (status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; } } } diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 443e9916a9..e455770411 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -223,8 +223,11 @@ void Standard::update_vtol_state() break; case TRANSITION_TO_FW: + _vtol_mode = mode::TRANSITION_TO_MC; + break; + case TRANSITION_TO_MC: - _vtol_mode = TRANSITION; + _vtol_mode = mode::TRANSITION_TO_FW; break; } } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index ec707ce27d..ba31477541 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -215,15 +215,17 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION; + _vtol_mode = TRANSITION_TO_FW; _vtol_vehicle_status->vtol_in_trans_mode = true; + break; case TRANSITION_FRONT_P2: - _vtol_mode = TRANSITION; + _vtol_mode = TRANSITION_TO_FW; _vtol_vehicle_status->vtol_in_trans_mode = true; + break; case TRANSITION_BACK: - _vtol_mode = TRANSITION; + _vtol_mode = TRANSITION_TO_MC; _vtol_vehicle_status->vtol_in_trans_mode = true; break; } @@ -482,7 +484,8 @@ void Tailsitter::fill_actuator_outputs() _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle break; - case TRANSITION: + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: // in transition engines are mixed by weight (BACK TRANSITION ONLY) _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_1->timestamp = _actuators_mc_in->timestamp; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 65f14b1e7c..62322ad675 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -244,8 +244,11 @@ void Tiltrotor::update_vtol_state() case TRANSITION_FRONT_P1: case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION_TO_MC; + break; + case TRANSITION_BACK: - _vtol_mode = TRANSITION; + _vtol_mode = TRANSITION_TO_MC; break; } } 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 a97770fbe1..4f004cf837 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -735,13 +735,12 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == FIXED_WING) { _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - } - /* 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 */ } else if (_vtol_type->get_mode() == TRANSITION_TO_MC) { - _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_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; } } @@ -775,10 +774,11 @@ void VtolAttitudeControl::task_main() fill_fw_att_rates_sp(); } - } else if (_vtol_type->get_mode() == TRANSITION) { + } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition + _vtol_vehicle_status.in_transition_to_fw = _vtol_type->get_mode() == TRANSITION_TO_FW; bool got_new_data = false; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 96f11fd51e..caf4aebd63 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -65,7 +65,8 @@ struct Params { enum mode { ROTARY_WING = 0, FIXED_WING, - TRANSITION, + TRANSITION_TO_FW, + TRANSITION_TO_MC, EXTERNAL };