differential: add support for speed waypoint

chfriedrich98 · Aug 27, 2024 · 022ce0c · 022ce0c
1 parent beaa8bb
commit 022ce0c
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git 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
@@ -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