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 a42e7d066c..9bea8e8c76 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -325,9 +325,6 @@ VtolAttitudeControl::vehicle_cmd_poll() { if (updated) { orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_cmd); - if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { - orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_transition_cmd); - } } } @@ -505,9 +502,25 @@ void VtolAttitudeControl::task_main() vehicle_battery_poll(); vehicle_cmd_poll(); + // update transition command if necessary + if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_transition_cmd); + } + // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); + // reset transition command if not in offboard control + if (!_v_control_mode.flag_control_offboard_enabled) + { + if (_vtol_type->get_mode() == ROTARY_WING) { + _vehicle_transition_cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_MC; + } + else if (_vtol_type->get_mode() == FIXED_WING) { + _vehicle_transition_cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_FW; + } + } + // check in which mode we are in and call mode specific functions if (_vtol_type->get_mode() == ROTARY_WING) { // vehicle is in rotary wing mode 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 c7d4047b4e..3933963835 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -99,6 +99,7 @@ public: ~VtolAttitudeControl(); int start(); /* start the task and return OK on success */ + bool is_fixed_wing_requested(); struct vehicle_attitude_s* get_att () {return &_v_att;} struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;} diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 0f43b6e453..1c10c29d97 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -134,8 +134,8 @@ void VtolType::set_idle_fw() } /* - * Return true if fixed-wing mode is requested. - * Either via switch or via command. + * Returns true if fixed-wing mode is requested. + * Changed either via switch or via command. */ bool VtolType::is_fixed_wing_requested() {