Added spoolup and removed temporary timeout for EKF

src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp

@@ -34,6 +34,7 @@
 #include "DifferentialDriveControl.hpp"
 
 using namespace matrix;
+using namespace time_literals;
 
 DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
 {
@@ -89,6 +90,16 @@ void DifferentialDriveControl::control(float dt)
 		}
 	}
 
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
+		}
+	}
+
 	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
 
 	// PID to reach setpoint using control_output
@@ -104,6 +115,12 @@ void DifferentialDriveControl::control(float dt)
 			pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
 	}
 
+
+	if (!_spooled_up) {
+		differential_drive_control_output.speed = 0.0f;
+		differential_drive_control_output.yaw_rate = 0.0f;
+	}
+
 	differential_drive_control_output.timestamp = hrt_absolute_time();
 	_differential_drive_control_output_pub.publish(differential_drive_control_output);
 }

src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp

@@ -48,6 +48,7 @@
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
 
 class DifferentialDriveControl : public ModuleParams
 {
@@ -67,6 +68,7 @@ private:
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
 
@@ -82,10 +84,14 @@ private:
 	PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
 	PID_t _pid_speed; ///< The PID controller for velocity.
 
+	bool _spooled_up{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
 		(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
 		(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
-		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
+		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity,
+
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
 	)
 };

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -36,7 +36,6 @@
 #include <mathlib/math/Limits.hpp>
 
 using namespace matrix;
-using namespace time_literals;
 
 DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
 {
@@ -117,19 +116,6 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 	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

@@ -131,10 +131,6 @@ 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,