take rtl path into account for remaining rtl time estimation

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h

@@ -59,6 +59,16 @@ struct PlannedPath {
 		current_index = 0;
 	}
 
+	matrix::Vector2d getPoint(int index)
+	{
+		if (index < num_points) {
+			return points[index];
+
+		} else {
+			return matrix::Vector2d{(double)NAN, (double)NAN};
+		}
+	}
+
 	bool hasNextPoint()
 	{
 		return current_index < num_points;

src/modules/navigator/rtl.cpp

@@ -182,6 +182,14 @@ void RTL::on_inactive()
 	if ((now - _destination_check_time) > 2_s) {
 		_destination_check_time = now;
 		setRtlTypeAndDestination();
+		const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0
+				&& hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s;
+
+		if (global_position_recently_updated) {
+			_navigator->updateStartOfRTLPathPlanner(_navigator->getRtlPlanningStart());
+			_rtl_direct.updatePlannedPath();
+		}
+
 		publishRemainingTimeEstimate();
 	}
 

src/modules/navigator/rtl_direct.cpp

@@ -492,20 +492,48 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 			}
 
 		// FALLTHROUGH
-		case RTLState::MOVE_TO_INTERMEDIATE_POINT:
+		case RTLState::MOVE_TO_INTERMEDIATE_POINT: {
+				const int num_points = _geofence_aware_return_path.num_points;
+
+				for (int i = 0; i < num_points - 1; ++i) {
+					matrix::Vector2d start = _geofence_aware_return_path.getPoint(i);
+					matrix::Vector2d end = _geofence_aware_return_path.getPoint(i + 1);
+
+					matrix::Vector2f direction{};
+					get_vector_to_next_waypoint(start(0), start(1), end(0), end(1), &direction(0),
+								    &direction(1));
+					float dist{get_distance_to_next_waypoint(start(0), start(1),
+							end(0), end(1))};
+
+					_rtl_time_estimator.addDistance(dist, direction, 0.f);
+				}
+			}
 
 		// FALLTHROUGH
 		case RTLState::MOVE_TO_LOITER: {
 				matrix::Vector2f direction{};
-				get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
-							    land_approach.lon, &direction(0), &direction(1));
-				float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)};
 
-				if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-					move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m);
+				float dist{0.f};
+
+				if (_geofence_aware_return_path.num_points > 0) {
+					const matrix::Vector2d last_point = _geofence_aware_return_path.getPoint(_geofence_aware_return_path.num_points - 1);
+					get_vector_to_next_waypoint(last_point(0), last_point(1), land_approach.lat,
+								    land_approach.lon, &direction(0), &direction(1));
+					dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon);
+
+				} else {
+
+					get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat,
+								    land_approach.lon, &direction(0), &direction(1));
+					dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon);
 				}
 
-				_rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f);
+
+				if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+					dist = max(0.f, dist - land_approach.loiter_radius_m);
+				}
+
+				_rtl_time_estimator.addDistance(dist, direction, 0.f);
 			}
 
 		// FALLTHROUGH
@@ -654,3 +682,8 @@ void RtlDirect::publish_rtl_direct_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
+
+void RtlDirect::updatePlannedPath()
+{
+	_geofence_aware_return_path =  _navigator->planPath();
+}

src/modules/navigator/rtl_direct.h

@@ -112,6 +112,8 @@ public:
 
 	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
 
+	void updatePlannedPath();
+
 private:
 	/**
 	 * @brief Return to launch state machine.