From 36bd7a797b6116b2f4b1c3fe358ee56eb7982b1d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 11:46:26 +0100 Subject: [PATCH 1/3] navigator: use bearing to home for RTL --- src/modules/navigator/navigator_main.cpp | 30 +++++++++++++++++++----- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..d7f6fd16ab 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; From a43c7c488eaba5022a18f05e590059cad6f580da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 18:54:58 +0400 Subject: [PATCH 2/3] navigator: "reached" flags reset fixed --- src/modules/navigator/navigator_main.cpp | 39 +++++++++++++++++------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ed12d1062..af1b9aac02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -321,6 +321,11 @@ private: */ bool onboard_mission_available(unsigned relative_index); + /** + * Reset all "reached" flags. + */ + void reset_reached(); + /** * Check if current mission item has been reached. */ @@ -855,9 +860,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - - /* reset time counter on state changes */ - _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -1009,6 +1011,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -1024,6 +1028,8 @@ Navigator::start_none() void Navigator::start_ready() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1046,6 +1052,8 @@ Navigator::start_ready() void Navigator::start_loiter() { + reset_reached(); + _do_takeoff = false; /* set loiter position if needed */ @@ -1091,6 +1099,8 @@ Navigator::start_mission() void Navigator::set_mission_item() { + reset_reached(); + /* copy current mission to previous item */ memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); @@ -1104,9 +1114,6 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - /* reset time counter for new item */ - _time_first_inside_orbit = 0; - _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1224,6 +1231,8 @@ Navigator::start_rtl() void Navigator::start_land() { + reset_reached(); + /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ _do_takeoff = false; @@ -1255,6 +1264,8 @@ Navigator::start_land() void Navigator::start_land_home() { + reset_reached(); + /* land to home position, should be called when hovering above home, from RTL state */ _do_takeoff = false; _reset_loiter_pos = true; @@ -1285,8 +1296,7 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { - /*reset time counter for new RTL item */ - _time_first_inside_orbit = 0; + reset_reached(); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -1550,9 +1560,7 @@ Navigator::check_mission_item_reached() /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - _time_first_inside_orbit = 0; - _waypoint_yaw_reached = false; - _waypoint_position_reached = false; + reset_reached(); return true; } } @@ -1561,6 +1569,15 @@ Navigator::check_mission_item_reached() } +void +Navigator::reset_reached() +{ + _time_first_inside_orbit = 0; + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + +} + void Navigator::on_mission_item_reached() { From 7d8f08ad9a733111a9d8b1512a85592a2fb9b045 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 19:05:32 +0400 Subject: [PATCH 3/3] navigator: check if yaw reached only when position reached --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index af1b9aac02..4cd60d8d89 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1533,7 +1533,7 @@ Navigator::check_mission_item_reached() } } - if (!_waypoint_yaw_reached) { + if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);