Fixed bug in the guidance logic

After smoothing the linera velocity setpoint, the EKF has trouble initializing, becuase the acceleration is too smooth, to combat this issue, there is a 1 second delay when initializing the mission mode

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -36,6 +36,7 @@
 #include <mathlib/math/Limits.hpp>
 
 using namespace matrix;
+using namespace time_literals;
 
 DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
 {
@@ -66,12 +67,6 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 				current_waypoint(1));
 	float heading_error = matrix::wrap_pi(desired_heading - yaw);
 
-	const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
-				   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
-
-	_forwards_velocity_smoothing.updateDurations(max_velocity);
-	_forwards_velocity_smoothing.updateTraj(dt);
-
 	if (_current_waypoint != current_waypoint) {
 		_currentState = GuidanceState::TURNING;
 	}
@@ -93,10 +88,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 
 		break;
 
-	case GuidanceState::DRIVING:
+	case GuidanceState::DRIVING: {
-		desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
-				_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
+						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
-		break;
+			_forwards_velocity_smoothing.updateDurations(max_velocity);
+			_forwards_velocity_smoothing.updateTraj(dt);
+			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+					_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
+			break;
+		}
 
 	case GuidanceState::GOAL_REACHED:
 		// temporary till I find a better way to stop the vehicle
@@ -116,6 +116,20 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 	output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
 	output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
 	output.timestamp = hrt_absolute_time();
+
+	// Implement a 1-second initialization period for the EKF to prevent the rover from starting too quickly, which can disrupt calibration
+	if (_let_ekf_initialize && current_waypoint.norm() > DBL_EPSILON) {
+		_start_time = hrt_absolute_time();
+		_let_ekf_initialize = false;  // Set _let_ekf_initialize to false so this block only runs once
+	}
+
+	// Check if less than 1 second has passed since the start of the guidance controller
+	if ((hrt_absolute_time() - _start_time) < 1_s) {
+
+		output.speed = 0;
+		output.yaw_rate = 0;
+	}
+
 	_differential_drive_setpoint_pub.publish(output);
 
 	_current_waypoint = current_waypoint;

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp

@@ -40,6 +40,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <math.h>
+#include <chrono>
 // #include <matrix/matrix/Matrix.hpp>
 
 #include <lib/motion_planning/PositionSmoothing.hpp>
@@ -132,6 +133,10 @@ private:
 
 	PID_t _heading_p_controller; ///< The PID controller for yaw rate.
 
+	bool _let_ekf_initialize{true};
+
+	hrt_abstime _start_time{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,