ackermann: fix RTL

chfriedrich98 · Sep 30, 2024 · d53ff22 · d53ff22
1 parent 8eebec0
commit d53ff22
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
@@ -60,12 +60,12 @@ void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
 					  _prev_wp(0), _prev_wp(1));
 	const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
 					  _curr_wp(0), _curr_wp(1));
-	const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-					  _next_wp(0), _next_wp(1));
+	// const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+	// 				  _next_wp(0), _next_wp(1));
 
 	// Catch return to launch
 	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-		_mission_finished = distance_to_next_wp < _acceptance_radius;
+		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
 	}
 
 	// Guidance logic