From 022ce0c3bb73b1ca09a75434759b3c96a40e271c Mon Sep 17 00:00:00 2001
From: chfriedrich98 <chfriedrich@student.ethz.ch>
Date: Tue, 27 Aug 2024 15:29:17 +0200
Subject: [PATCH] differential: add support for speed waypoint

---
 .../RoverDifferentialGuidance.cpp                      | 10 +++++++++-
 .../RoverDifferentialGuidance.hpp                      |  2 ++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
index 7d9b0b7188c8..e6895a23c0b3 100644
--- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
@@ -148,7 +148,7 @@ RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::comp
 	// Guidance logic
 	switch (_currentState) {
 	case GuidanceState::DRIVING: {
-			desired_speed = _param_rd_miss_spd_def.get();
+			desired_speed = _wp_max_desired_vel;
 
 			if (_theta < SLOW_DOWN_THRESHOLD) {
 				if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
@@ -253,4 +253,12 @@ void RoverDifferentialGuidance::updateWaypoints()
 	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
 	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
 	_theta = acosf(cosin);
+
+	// Waypoint cruising speed
+	if (position_setpoint_triplet.current.cruising_speed > 0.f) {
+		_wp_max_desired_vel = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
+
+	} else {
+		_wp_max_desired_vel = _param_rd_miss_spd_def.get();
+	}
 }
diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
index 3870ac565803..1a6727c98aac 100644
--- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
+++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
@@ -135,7 +135,9 @@ class RoverDifferentialGuidance : public ModuleParams
 	Vector2d _curr_wp{};
 	Vector2f _curr_wp_ned{};
 	Vector2d _next_wp{};
+	Vector2f _next_wp_ned{};
 	Vector2d _home_position{};
+	float _wp_max_desired_vel{0.f};
 
 	// Controllers
 	PID_t _pid_heading; // The PID controller for the heading