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

@@ -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 && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) {
-
-	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
 		/* 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;

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 &&

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 */

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)) {
+		if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
+			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");