From 36bd7a797b6116b2f4b1c3fe358ee56eb7982b1d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 11:46:26 +0100 Subject: navigator: use bearing to home for RTL --- src/modules/navigator/navigator_main.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd..d7f6fd16a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1343,7 +1343,14 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; // don't change altitude - _mission_item.yaw = NAN; // TODO set heading to home + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -1409,17 +1416,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); + + } else { + /* else use current position */ + sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); + } + sp->loiter_radius = _parameters.loiter_radius; + sp->loiter_direction = 1; + sp->pitch_min = 0.0f; + } else { sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; } - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; -- cgit v1.2.3