From 05bc9acfb776e7b53aecb745086933e35f58085b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Oct 2016 11:40:08 +0200 Subject: [PATCH] navigator: Wrap get time inside into function call and set time inside to zero This ensures that the dual-use of the pitch_min / time_inside field is handled properly between takeoff and non-takeoff items. Flight tested in SITL. --- src/modules/navigator/mission.cpp | 15 +++++++-------- src/modules/navigator/mission_block.cpp | 3 ++- src/modules/navigator/navigator.h | 2 ++ src/modules/navigator/rtl.cpp | 4 ++-- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eedade0ffe..f50ba79edd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -519,7 +519,7 @@ Mission::set_mission_items() _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.time_inside = 0.0f; } /* if we just did a takeoff navigate to the actual waypoint now */ @@ -548,6 +548,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; + _mission_item.time_inside = 0.0f; } } @@ -581,7 +582,7 @@ Mission::set_mission_items() _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.time_inside = 0.0f; } /* transition to MC */ @@ -620,7 +621,7 @@ Mission::set_mission_items() _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.time_inside = 0.0f; } /* we just moved to the landing waypoint, now descend */ @@ -687,9 +688,7 @@ Mission::set_mission_items() set_current_offboard_mission_item(); } - // TODO: report onboard mission item somehow - - if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { + if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) { /* try to process next mission item */ if (has_next_position_item) { @@ -806,7 +805,7 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss 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->time_inside = 0.0f; mission_item->yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -854,7 +853,7 @@ Mission::heading_sp_update() } /* set yaw angle for the waypoint if a loiter time has been specified */ - if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { + if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) { // XXX: should actually be param4 from mission item // at the moment it will just keep the heading it has //_mission_item.yaw = _on_arrival_yaw; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 59cebc2c93..2d4ad6ab12 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -325,7 +325,8 @@ MissionBlock::is_mission_item_reached() } /* check if the MAV was long enough inside the waypoint orbit */ - if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) || + (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { // exit xtrack location if (_mission_item.loiter_exit_xtrack && diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7a80cf3e74..80675c22e9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -215,6 +215,8 @@ public: bool abort_landing(); + static float get_time_inside(struct mission_item_s& item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e2cfb8501a..cb234aacd1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -271,8 +271,8 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); - if (autoland && (_mission_item.time_inside > FLT_EPSILON)) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); + if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item)); } else { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");