From 903a0cd689263d391f7965d86bde43fe3f5d3c73 Mon Sep 17 00:00:00 2001 From: sander Date: Thu, 24 Mar 2016 10:47:21 +0100 Subject: [PATCH] Vtol takeoff/land handling in work items --- src/modules/navigator/mission.cpp | 131 ++++++++++++++++-------------- src/modules/navigator/mission.h | 20 +++-- 2 files changed, 82 insertions(+), 69 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 58f2c410ad..296e5ca13f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -62,7 +62,6 @@ #include #include #include -#include #include "mission.h" #include "navigator.h" @@ -80,7 +79,6 @@ 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), @@ -464,27 +462,46 @@ Mission::set_mission_items() } /* if we just did a takeoff navigate to the actual waypoint now */ - 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; - } + if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; + + } else { + 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 */ _mission_item.yaw = NAN; } + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF + && _navigator->get_vstatus()->is_rotary_wing + && !_navigator->get_vstatus()->condition_landed + && has_next_position_item) { + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + set_align_mission_item(&_mission_item, &mission_item_next_position); + } + + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WORK_ITEM_TYPE_ALIGN) { + + _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; + _mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW; + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + } + + if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND || + (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){ + + _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; + _mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC; + new_work_item_type = WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND; + } + /* move to landing waypoint before descent if necessary */ if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) { new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; @@ -535,17 +552,7 @@ Mission::set_mission_items() && has_next_position_item) { new_work_item_type = WORK_ITEM_TYPE_ALIGN; - - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - copy_positon_if_valid(_mission_item, pos_sp_triplet->current); - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - mission_item_next_position.lat, - mission_item_next_position.lon); - _mission_item.force_heading = true; + set_align_mission_item(&_mission_item, &mission_item_next_position); } /* yaw is aligned now */ @@ -570,7 +577,7 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - copy_positon_if_valid(_mission_item, pos_sp_triplet->current); + copy_positon_if_valid(&_mission_item, &(pos_sp_triplet->current)); _mission_item.autocontinue = true; _mission_item.time_inside = 0; } @@ -629,23 +636,6 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } -void -Mission::copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint) -{ - if (setpoint.valid) { - _mission_item.lat = setpoint.lat; - _mission_item.lon = setpoint.lon; - _mission_item.altitude = setpoint.alt; - - } else { - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - - _mission_item.altitude_is_relative = false; -} - bool Mission::do_need_takeoff() { @@ -671,10 +661,6 @@ Mission::do_need_takeoff() _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; } @@ -686,18 +672,6 @@ Mission::do_need_takeoff() bool Mission::do_need_move_to_land() { - if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND || (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){ - 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, @@ -709,6 +683,39 @@ Mission::do_need_move_to_land() return false; } +void +Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) +{ + if (setpoint->valid) { + mission_item->lat = setpoint->lat; + mission_item->lon = setpoint->lon; + mission_item->altitude = setpoint->alt; + + } else { + mission_item->lat = _navigator->get_global_position()->lat; + mission_item->lon = _navigator->get_global_position()->lon; + mission_item->altitude = _navigator->get_global_position()->alt; + } + + mission_item->altitude_is_relative = false; +} + +void +Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) +{ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + copy_positon_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); + mission_item->altitude_is_relative = false; + mission_item->autocontinue = true; + mission_item->time_inside = 0; + mission_item->yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + mission_item_next->lat, + mission_item_next->lon); + mission_item->force_heading = true; +} + float Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e91a85516f..6d42cb9db5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -119,11 +119,6 @@ private: */ void set_mission_items(); - /** - * Copies position from setpoint if valid, otherwise copies current position - */ - void copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint); - /** * Returns true if we need to do a takeoff at the current state */ @@ -134,6 +129,16 @@ private: */ bool do_need_move_to_land(); + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); + /** * Calculate takeoff height for mission item considering ground clearance */ @@ -226,7 +231,6 @@ 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, @@ -252,7 +256,9 @@ private: WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ - WORK_ITEM_TYPE_CMD_BEFORE_MOVE /**< */ + WORK_ITEM_TYPE_CMD_BEFORE_MOVE, /**< */ + WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, /**< */ + WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND /**< */ } _work_item_type; /**< current type of work to do (sub mission item) */ };