From a713fd4197ee17cd1081bcbcba58e63aa8e846b3 Mon Sep 17 00:00:00 2001 From: sander Date: Wed, 10 Feb 2016 22:25:44 +0100 Subject: [PATCH] Implemented VTOL_TAKEOFF and VTOL_LAND commands --- msg/vehicle_command.msg | 2 ++ src/modules/mavlink/mavlink_mission.cpp | 1 + src/modules/navigator/mission.cpp | 35 ++++++++++++++++++- src/modules/navigator/mission.h | 1 + .../navigator/mission_feasibility_checker.cpp | 2 ++ src/modules/navigator/navigation.h | 2 ++ 6 files changed, 42 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 3369ae8900..f310a0181f 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -11,6 +11,8 @@ uint32 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desi uint32 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| uint32 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| uint32 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| +uint32 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint32 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| uint32 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| uint32 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92 # hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index dfa5dcfa31..929e67a0c1 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -865,6 +865,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * // specific item handling switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: + case NAV_CMD_VTOL_TAKEOFF: mavlink_mission_item->param1 = mission_item->pitch_min; break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c74b5aa5cd..3133fc4117 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include "mission.h" #include "navigator.h" @@ -77,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _need_takeoff(true), + _takeoff_vtol_transition(false), _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), @@ -460,7 +462,21 @@ Mission::set_mission_items() } /* if we just did a takeoff navigate to the actual waypoint now */ - if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { + if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { + + /* handle VTOL TAKEOFF command */ + if(_takeoff_vtol_transition){ + struct vehicle_command_s cmd = {}; + cmd.command = NAV_CMD_DO_VTOL_TRANSITION; + cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_FW; + if (_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + } else { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + } + _takeoff_vtol_transition = false; + } + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ @@ -647,11 +663,16 @@ Mission::do_need_takeoff() if (_need_takeoff && ( _mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + if(_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF){ + _takeoff_vtol_transition = true; + } + _need_takeoff = false; return true; } @@ -663,6 +684,18 @@ Mission::do_need_takeoff() bool Mission::do_need_move_to_land() { + if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND){ + struct vehicle_command_s cmd = {}; + cmd.command = NAV_CMD_DO_VTOL_TRANSITION; + cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_MC; + if (_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + } else { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + } + _mission_item.nav_cmd = NAV_CMD_LAND; + } + if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_LAND) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 83b917a22a..f782ce8293 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -225,6 +225,7 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff_vtol_transition; /**< if true, a vtol transition will be performed after takeoff */ enum { MISSION_TYPE_NONE, diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b5cf16f6ac..63c441d8b8 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -242,6 +242,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && missionitem.nav_cmd != NAV_CMD_LAND && missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_VTOL_LAND && + missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF && missionitem.nav_cmd != NAV_CMD_PATHPLANNING && missionitem.nav_cmd != NAV_CMD_DO_JUMP && missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 82d87d09d3..a7da600330 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -63,6 +63,8 @@ enum NAV_CMD { NAV_CMD_PATHPLANNING = 81, NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder NAV_CMD_GOTO_TAREGT = 195, + NAV_CMD_VTOL_TAKEOFF = 84, + NAV_CMD_VTOL_LAND = 85, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183,