diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 2f223fbc2e..6ac66d22c7 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -50,6 +50,7 @@ uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 244b8c1d92..1cf771ba3c 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -79,6 +79,13 @@ uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 # Vtol with six engines uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22 # Vtol with eight engines uint8 VEHICLE_TYPE_ENUM_END = 23 +# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE +uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VEHICLE_VTOL_STATE_MC = 3 +uint8 VEHICLE_VTOL_STATE_FW = 4 + uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2 # alerting of critical voltage diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 12744a86dd..3fb69c7086 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -794,6 +794,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: + case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: /* ignore commands that handled in low prio loop */ break; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 8725647849..a625a6e3ae 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 (_manual_control_sp->aux1 < 0.0f) { + if (!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 46fd7b9c3d..c8c2ea0c8e 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -58,9 +58,9 @@ Tailsitter::~Tailsitter() void Tailsitter::update_vtol_state() { // simply switch between the two modes - if (_manual_control_sp->aux1 < 0.0f) { + if (!is_fixed_wing_requested()) { _vtol_mode = ROTARY_WING; - } else if (_manual_control_sp->aux1 > 0.0f) { + } else { _vtol_mode = FIXED_WING; } } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 06cde02a70..746a5fb8f3 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 (_manual_control_sp->aux1 < 0.0f) { + if (!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 6f4405d7c8..a42e7d066c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -69,6 +69,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _local_pos_sub(-1), _airspeed_sub(-1), _battery_status_sub(-1), + _vehicle_cmd_sub(-1), //init publication handlers _actuators_0_pub(nullptr), @@ -95,6 +96,8 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_local_pos,0,sizeof(_local_pos)); 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; @@ -312,6 +315,22 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for command updates. +*/ +void +VtolAttitudeControl::vehicle_cmd_poll() { + bool updated; + orb_check(_vehicle_cmd_sub, &updated); + + 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); + } + } +} + /** * Update parameters. */ @@ -412,6 +431,7 @@ void VtolAttitudeControl::task_main() _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -483,6 +503,7 @@ void VtolAttitudeControl::task_main() vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); vehicle_battery_poll(); + vehicle_cmd_poll(); // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); 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 bf9a69d5c1..c7d4047b4e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -115,6 +116,7 @@ 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;} @@ -136,6 +138,7 @@ private: int _local_pos_sub; // sensor subscription int _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription + int _vehicle_cmd_sub; int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -162,6 +165,8 @@ private: 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_cmd; + struct vehicle_command_s _vehicle_transition_cmd; // stores transition commands only Params _params; // struct holding the parameters @@ -206,6 +211,7 @@ private: void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed void vehicle_battery_poll(); // Check for battery updates + void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed int parameters_update(); //Update local paraemter cache void fill_mc_att_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7e8d3217f1..0f43b6e453 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -63,6 +63,7 @@ _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; @@ -130,4 +131,17 @@ void VtolType::set_idle_fw() if (ret != OK) {errx(ret, "failed setting min values");} close(fd); -} \ No newline at end of file +} + +/* + * Return true if fixed-wing mode is requested. + * 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 a797201e01..a38f808e21 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -87,6 +87,8 @@ public: mode get_mode () {return _vtol_mode;}; + bool is_fixed_wing_requested(); + protected: VtolAttitudeControl *_attc; mode _vtol_mode; @@ -107,6 +109,7 @@ 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;