diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index a625a6e3ae..649c4b526f 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -105,7 +105,7 @@ void Standard::update_vtol_state() * For the back transition the pusher motor is immediately stopped and rotors reactivated. */ - if (!is_fixed_wing_requested()) { + if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index c8c2ea0c8e..93a0f25f56 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -58,7 +58,7 @@ Tailsitter::~Tailsitter() void Tailsitter::update_vtol_state() { // simply switch between the two modes - if (!is_fixed_wing_requested()) { + if (!_attc->is_fixed_wing_requested()) { _vtol_mode = ROTARY_WING; } else { _vtol_mode = FIXED_WING; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 746a5fb8f3..ea9c688f2d 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -158,7 +158,7 @@ void Tiltrotor::update_vtol_state() * forward completely. For the backtransition the motors simply rotate back. */ - if (!is_fixed_wing_requested()) { + if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode switch(_vtol_schedule.flight_mode) { 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 9bea8e8c76..785ff7aab5 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -97,7 +97,6 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_airspeed,0,sizeof(_airspeed)); memset(&_batt_status,0,sizeof(_batt_status)); memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd)); - memset(&_vehicle_transition_cmd,0, sizeof(_vehicle_cmd)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -325,9 +324,35 @@ VtolAttitudeControl::vehicle_cmd_poll() { if (updated) { orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_cmd); + handle_command(); } } +/** +* Check received command +*/ +void +VtolAttitudeControl::handle_command() { + // update transition command if necessary + if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + _transition_command = int(_vehicle_cmd.param1 + 0.5f); + } +} + +/* + * Returns true if fixed-wing mode is requested. + * Changed either via switch or via command. + */ +bool +VtolAttitudeControl::is_fixed_wing_requested() +{ + bool to_fw = _manual_control_sp.aux1 > 0.0f; + if (_v_control_mode.flag_control_offboard_enabled) { + to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; + } + return to_fw; +} + /** * Update parameters. */ @@ -502,11 +527,6 @@ 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(); @@ -514,10 +534,10 @@ void VtolAttitudeControl::task_main() 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; + _transition_command = 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; + _transition_command = vehicle_status_s::VEHICLE_VTOL_STATE_FW; } } 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 3933963835..c0777fdeeb 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -117,7 +117,6 @@ public: struct vehicle_local_position_s* get_local_pos () {return &_local_pos;} struct airspeed_s* get_airspeed () {return &_airspeed;} struct battery_status_s* get_batt_status () {return &_batt_status;} - struct vehicle_command_s* get_vehicle_transition_cmd () {return &_vehicle_transition_cmd;} struct Params* get_params () {return &_params;} @@ -167,7 +166,6 @@ private: struct airspeed_s _airspeed; // airspeed struct battery_status_s _batt_status; // battery status struct vehicle_command_s _vehicle_cmd; - struct vehicle_command_s _vehicle_transition_cmd; // stores transition commands only Params _params; // struct holding the parameters @@ -191,6 +189,7 @@ private: * to waste energy when gliding. */ unsigned _motor_count; // number of motors float _airspeed_tot; + int _transition_command; VtolType * _vtol_type; // base class for different vtol types Tiltrotor * _tiltrotor; // tailsitter vtol type @@ -217,6 +216,7 @@ private: int parameters_update(); //Update local paraemter cache void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); + void handle_command(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 1c10c29d97..c6917e3ced 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -63,7 +63,6 @@ _vtol_mode(ROTARY_WING) _local_pos = _attc->get_local_pos(); _airspeed = _attc->get_airspeed(); _batt_status = _attc->get_batt_status(); - _vehicle_transition_cmd = _attc->get_vehicle_transition_cmd(); _params = _attc->get_params(); flag_idle_mc = true; @@ -132,16 +131,3 @@ void VtolType::set_idle_fw() close(fd); } - -/* - * Returns true if fixed-wing mode is requested. - * Changed either via switch or via command. - */ -bool VtolType::is_fixed_wing_requested() -{ - bool to_fw = _manual_control_sp->aux1 > 0.0f; - if (_v_control_mode->flag_control_offboard_enabled) { - to_fw = fabsf(_vehicle_transition_cmd->param1 - vehicle_status_s::VEHICLE_VTOL_STATE_FW) < FLT_EPSILON; - } - return to_fw; -} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index a38f808e21..a797201e01 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -87,8 +87,6 @@ public: mode get_mode () {return _vtol_mode;}; - bool is_fixed_wing_requested(); - protected: VtolAttitudeControl *_attc; mode _vtol_mode; @@ -109,7 +107,6 @@ protected: struct vehicle_local_position_s *_local_pos; struct airspeed_s *_airspeed; // airspeed struct battery_status_s *_batt_status; // battery status - struct vehicle_command_s *_vehicle_transition_cmd; struct Params *_params;