differential: Add stabilized and position mode (#23669)

* differential: add position and stabilized mode

* differential: add hardcoded stick input deadzones

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -21,6 +21,7 @@ param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 5
 param set-default RD_MAX_THR_SPD 7
 param set-default RD_SPEED_P 1
 param set-default RD_SPEED_I 0

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -31,6 +31,7 @@ param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 1.8
 param set-default RD_MAX_THR_SPD 2
 param set-default RD_SPEED_P 0.5
 param set-default RD_SPEED_I 0.1

src/lib/pure_pursuit/PurePursuit.cpp

@@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2
 
 	const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
 	const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
-	const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
-			prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
-	const Vector2f curr_pos_to_path = distance_on_line_segment -
-					  prev_wp_to_curr_pos; // Shortest vector from the current position to the path
+	_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
+	_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;
 
-	if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
-		return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
+	if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
+		return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0));
 	}
 
-	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
-					  2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
-	const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
+	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(),
+					  2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point
+	const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension *
 			prev_wp_to_curr_wp_u;
 	const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
 	return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));

src/lib/pure_pursuit/PurePursuit.hpp

@@ -103,6 +103,8 @@ class PurePursuit : public ModuleParams
 				 float vehicle_speed);
 
 	float getLookaheadDistance() {return _lookahead_distance;};
+	float getCrosstrackError() {return _curr_pos_to_path.norm();};
+	float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};
 
 protected:
 	/**
@@ -122,5 +124,7 @@ class PurePursuit : public ModuleParams
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	float _lookahead_distance{0.f};
+	float _lookahead_distance{0.f}; // Radius of the circle around the vehicle
+	Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
+	Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
 };

src/modules/rover_differential/RoverDifferential.cpp

@@ -99,12 +99,94 @@ void RoverDifferential::Run()
 
 		} break;
 
+	case vehicle_status_s::NAVIGATION_STATE_STAB: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
+					if (!_yaw_ctl) {
+						_stab_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
+				rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
+					if (!_yaw_ctl) {
+						_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
+						_pos_ctl_start_position_ned = _curr_pos_ned;
+						_yaw_ctl = true;
+					}
+
+					// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+					const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
+										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
+					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
+							rover_differential_setpoint.forward_speed_setpoint) *
+									     vector_scaling * _pos_ctl_course_direction;
+					// Calculate yaw setpoint
+					const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
+								   _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
+					rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
+							yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 		_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
 		break;
 
 	default: // Unimplemented nav states will stop the rover
+		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
 		rover_differential_setpoint_s rover_differential_setpoint{};
 		rover_differential_setpoint.forward_speed_setpoint = NAN;
 		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
@@ -115,8 +197,9 @@ void RoverDifferential::Run()
 		break;
 	}
 
-	if (!_armed) { // Reset on disarm
+	if (!_armed) { // Reset when disarmed
 		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
 	}
 
 	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
@@ -135,6 +218,12 @@ void RoverDifferential::updateSubscriptions()
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status{};
 		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
+			_rover_differential_control.resetControllers();
+			_yaw_ctl = false;
+		}
+
 		_nav_state = vehicle_status.nav_state;
 		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 	}
@@ -155,6 +244,7 @@ void RoverDifferential::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
 		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
 		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;

src/modules/rover_differential/RoverDifferential.hpp

@@ -39,6 +39,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/pure_pursuit/PurePursuit.hpp>
 
 // uORB includes
 #include <uORB/Publication.hpp>
@@ -103,24 +104,36 @@ class RoverDifferential : public ModuleBase<RoverDifferential>, public ModulePar
 	// Instances
 	RoverDifferentialGuidance _rover_differential_guidance{this};
 	RoverDifferentialControl _rover_differential_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
 
 	// Variables
+	Vector2f _curr_pos_ned{};
 	matrix::Quatf _vehicle_attitude_quaternion{};
 	float _vehicle_yaw_rate{0.f};
 	float _vehicle_forward_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	int _nav_state{0};
 	bool _armed{false};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
+	float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
+	Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
+	Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
 
 	// Thresholds to avoid moving at rest due to measurement noise
 	static constexpr float YAW_RATE_THRESHOLD =
 		0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
 	static constexpr float SPEED_THRESHOLD =
 		0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
 
+	// Stick input deadzone
+	static constexpr float STICK_DEADZONE =
+		0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
-		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate
+		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
+		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
 	)
 };

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp

@@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints()
 
 	// Waypoint cruising speed
 	if (position_setpoint_triplet.current.cruising_speed > 0.f) {
-		_max_forward_speed = position_setpoint_triplet.current.cruising_speed;
+		_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
 
 	} else {
 		_max_forward_speed = _param_rd_miss_spd_def.get();

src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp

@@ -143,6 +143,7 @@ class RoverDifferentialGuidance : public ModuleParams
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
 		(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
 		(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
+		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
 		(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
 		(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
 		(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn

src/modules/rover_differential/module.yaml

@@ -113,6 +113,19 @@ parameters:
         decimal: 2
         default: 0.5
 
+      RD_MAX_SPEED:
+        description:
+          short: Maximum speed setpoint
+          long: |
+            This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
       RD_MAX_THR_SPD:
         description:
           short: Speed the rover drives at maximum throttle
@@ -133,7 +146,8 @@ parameters:
         description:
           short: Maximum allowed yaw rate for the rover
           long: |
-            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
+            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates
+            in Acro,Stabilized and Position mode.
         type: float
         unit: deg/s
         min: 0.01