ackermann: add lateral acceleration setpoint, acro/position mode and …

…updates to auto modes

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -23,8 +23,6 @@ param set-default RA_MAX_THR_SPEED 6
 param set-default RA_MAX_STR_ANG 0.5236
 param set-default RA_MAX_STR_RATE 360
 param set-default RA_MISS_SPD_DEF 5
-param set-default RA_MISS_SPD_GAIN 5
-param set-default RA_MISS_SPD_MIN 1
 param set-default RA_SPEED_I 0.01
 param set-default RA_SPEED_P 0.1
 param set-default RA_WHEEL_BASE 0.321

msg/RoverAckermannSetpoint.msg

@@ -1,8 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
-float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed for the rover
-float32 steering_setpoint      		  # [rad/s] Desired steering for the rover
-float32 steering_setpoint_normalized      # [-1, 1] Desired normalized steering for the rover
+float32 forward_speed_setpoint 		  # [m/s] Desired speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 steering_setpoint      		  # [rad] Desired steering angle
+float32 steering_setpoint_normalized      # [-1, 1] Desired normalized steering angle
+float32 lateral_acceleration_setpoint     # [m/s^2] Desired acceleration in body y direction. Positiv: right, Negativ: left.
 
 # TOPICS rover_ackermann_setpoint

msg/RoverAckermannStatus.msg

@@ -4,6 +4,8 @@ float32 measured_forward_speed       	      # [m/s] Measured speed in body x dir
 float32 adjusted_forward_speed_setpoint       # [m/s] Speed setpoint after applying slew rate
 float32 steering_setpoint_normalized          # [-1, 1] Normalized steering setpoint
 float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate
+float32 measured_lateral_acceleration         # [m/s^2] Measured acceleration in body y direction. Positiv: right, Negativ: left.
 float32 pid_throttle_integral                 # Integral of the PID for the closed loop speed controller
+float32 pid_lat_accel_integral                # Integral of the PID for the closed loop lateral acceleration controller
 
 # TOPICS rover_ackermann_status

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -38,6 +38,9 @@ RoverAckermann::RoverAckermann() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	_rover_ackermann_setpoint_pub.advertise();
+	_ax_filter.setAlpha(0.05);
+	_ay_filter.setAlpha(0.05);
+	_az_filter.setAlpha(0.05);
 	updateParams();
 }
 
@@ -76,6 +79,68 @@ void RoverAckermann::Run()
 				rover_ackermann_setpoint.forward_speed_setpoint_normalized =  manual_control_setpoint.throttle;
 				rover_ackermann_setpoint.steering_setpoint = NAN;
 				rover_ackermann_setpoint.steering_setpoint_normalized = manual_control_setpoint.roll;
+				rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN;
+				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
+				rover_ackermann_setpoint.timestamp =  timestamp;
+				rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
+				rover_ackermann_setpoint.forward_speed_setpoint_normalized =  manual_control_setpoint.throttle;
+				rover_ackermann_setpoint.steering_setpoint = NAN;
+				rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
+				rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f,
+						-_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get());
+				_rover_ackermann_setpoint_pub.publish(rover_ackermann_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_ackermann_setpoint_s rover_ackermann_setpoint{};
+				rover_ackermann_setpoint.timestamp = timestamp;
+				rover_ackermann_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_ra_max_speed.get(), _param_ra_max_speed.get());
+				rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_ackermann_setpoint.steering_setpoint = NAN;
+				rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
+				rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get());
+
+				if (fabsf(rover_ackermann_setpoint.lateral_acceleration_setpoint) > FLT_EPSILON
+				    || fabsf(rover_ackermann_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+					_course_control = false;
+
+				} else { // Course control if the steering input is zero (keep driving on a straight line)
+					if (!_course_control) {
+						_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
+						_pos_ctl_start_position_ned = _curr_pos_ned;
+						_course_control = 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_ackermann_setpoint.forward_speed_setpoint) *
+									     vector_scaling * _pos_ctl_course_direction;
+					// Calculate steering setpoint
+					const float steering_setpoint = _ackermann_guidance.calcDesiredSteering(_posctl_pure_pursuit,
+									target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, _param_ra_wheel_base.get(),
+									rover_ackermann_setpoint.forward_speed_setpoint, _vehicle_yaw, _param_ra_max_steer_angle.get(), _armed);
+					rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(_vehicle_forward_speed,
+							2.f) * tanf(steering_setpoint) / _param_ra_wheel_base.get();
+				}
+
 				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
 			}
 
@@ -93,6 +158,7 @@ void RoverAckermann::Run()
 		rover_ackermann_setpoint.forward_speed_setpoint_normalized =  0.f;
 		rover_ackermann_setpoint.steering_setpoint = NAN;
 		rover_ackermann_setpoint.steering_setpoint_normalized = 0.f;
+		rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN;
 		_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
 		break;
 	}
@@ -101,7 +167,7 @@ void RoverAckermann::Run()
 		_ackermann_control.resetControllers();
 	}
 
-	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
+	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw, _vehicle_lateral_acceleration);
 
 }
 
@@ -114,6 +180,12 @@ void RoverAckermann::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
+			_ackermann_control.resetControllers();
+			_course_control = false;
+		}
+
 		_nav_state = vehicle_status.nav_state;
 		_armed = vehicle_status.arming_state == 2;
 	}
@@ -128,9 +200,26 @@ void RoverAckermann::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
+
+		if (PX4_ISFINITE(vehicle_local_position.ax)) {
+			_ax_filter.update(vehicle_local_position.ax);
+		}
+
+		if (PX4_ISFINITE(vehicle_local_position.ay)) {
+			_ay_filter.update(vehicle_local_position.ay);
+		}
+
+		if (PX4_ISFINITE(vehicle_local_position.az)) {
+			_az_filter.update(vehicle_local_position.az);
+		}
+
+		_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 = velocity_in_body_frame(0);
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+		Vector3f acceleration_in_local_frame(_ax_filter.getState(), _ay_filter.getState(), _az_filter.getState());
+		Vector3f acceleration_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(acceleration_in_local_frame);
+		_vehicle_lateral_acceleration = acceleration_in_body_frame(1);
 	}
 }
 

src/modules/rover_ackermann/RoverAckermann.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>
@@ -54,13 +55,20 @@
 // Standard library includes
 #include <math.h>
 #include <matrix/matrix/math.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 // Local includes
 #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
 #include "RoverAckermannControl/RoverAckermannControl.hpp"
 
 using namespace time_literals;
 
+// Constants
+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
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
+
 class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
@@ -107,12 +115,31 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	// Class instances
 	RoverAckermannGuidance _ackermann_guidance{this};
 	RoverAckermannControl _ackermann_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
 
 	// Variables
 	matrix::Quatf _vehicle_attitude_quaternion{};
 	int _nav_state{0};
 	float _vehicle_forward_speed{0.f};
 	float _vehicle_yaw{0.f};
 	bool _armed{false};
+	bool _course_control{false};
+	Vector2f _pos_ctl_course_direction{};
+	Vector2f _pos_ctl_start_position_ned{};
+	Vector2f _curr_pos_ned{};
+	float _vehicle_lateral_acceleration{0.f};
+	AlphaFilter<float> _ax_filter;
+	AlphaFilter<float> _ay_filter;
+	AlphaFilter<float> _az_filter;
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
+		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
+		(ParamFloat<px4::params::RA_MAX_LAT_ACCEL>) _param_ra_max_lat_accel,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+
+	)
 
 };

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp

@@ -50,8 +50,15 @@ void RoverAckermannControl::updateParams()
 	ModuleParams::updateParams();
 
 	pid_set_parameters(&_pid_throttle,
-			   _param_ra_p_speed.get(), // Proportional gain
-			   _param_ra_i_speed.get(), // Integral gain
+			   _param_ra_speed_p.get(), // Proportional gain
+			   _param_ra_speed_i.get(), // Integral gain
+			   0, // Derivative gain
+			   1, // Integral limit
+			   1); // Output limit
+
+	pid_set_parameters(&_pid_lat_accel,
+			   _param_ra_lat_accel_p.get(), // Proportional gain
+			   _param_ra_lat_accel_i.get(), // Integral gain
 			   0, // Derivative gain
 			   1, // Integral limit
 			   1); // Output limit
@@ -67,12 +74,13 @@ void RoverAckermannControl::updateParams()
 	}
 }
 
-void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw)
+void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw,
+		const float vehicle_lateral_acceleration)
 {
 	// Timestamps
 	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5_s) * 1e-6f;
 
 	// Update ackermann setpoint
 	_rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint);
@@ -90,6 +98,37 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 
 	}
 
+	// Closed loop lateral acceleration control (overrides steering setpoint)
+	if (PX4_ISFINITE(_rover_ackermann_setpoint.lateral_acceleration_setpoint)) {
+		float vehicle_forward_speed_temp{0.f};
+
+		if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { // Use valid measurement if available
+			vehicle_forward_speed_temp = vehicle_forward_speed;
+
+		} else if (PX4_ISFINITE(forward_speed_normalized) && _param_ra_max_thr_speed.get() > FLT_EPSILON) {
+			vehicle_forward_speed_temp = math::interpolate<float>(forward_speed_normalized,
+						     -1.f, 1.f, -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get());
+		}
+
+		if (fabsf(vehicle_forward_speed_temp) > FLT_EPSILON) {
+			float steering_setpoint = atanf(_param_ra_wheel_base.get() *
+							_rover_ackermann_setpoint.lateral_acceleration_setpoint / powf(
+								vehicle_forward_speed_temp, 2.f));
+
+			if (sign(vehicle_forward_speed_temp) ==
+			    1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability)
+				steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint,
+								   vehicle_lateral_acceleration, 0, dt);
+			}
+
+			_rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(),
+					_param_ra_max_steer_angle.get());
+
+		} else {
+			_rover_ackermann_setpoint.steering_setpoint = 0.f;
+		}
+	}
+
 	// Steering control
 	float steering_normalized{0.f};
 
@@ -116,7 +155,9 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 	_rover_ackermann_status.measured_forward_speed = vehicle_forward_speed;
 	_rover_ackermann_status.steering_setpoint_normalized = steering_normalized;
 	_rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState();
+	_rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration;
 	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.integral;
+	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral;
 	_rover_ackermann_status_pub.publish(_rover_ackermann_status);
 
 	// Publish to motor
@@ -188,4 +229,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 void RoverAckermannControl::resetControllers()
 {
 	pid_reset_integral(&_pid_throttle);
+	pid_reset_integral(&_pid_lat_accel);
+	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
+	_steering_with_rate_limit.setForcedValue(0.f);
+
 }

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp

@@ -71,8 +71,11 @@ class RoverAckermannControl : public ModuleParams
 
 	/**
 	 * @brief Compute motor commands based on setpoints
+	 * @param vehicle_forward_speed Measured speed in body x direction. Positiv: forwards, Negativ: backwards [m/s]
+	 * @param vehicle_yaw Measured yaw [rad]
+	 * @param vehicle_lateral_acceleration Measured acceleration in body y direction. Positiv: right, Negativ: left [m/s^s]
 	 */
-	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw);
+	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw, float vehicle_lateral_acceleration);
 
 	/**
 	 * @brief Reset PID controllers and slew rates
@@ -112,19 +115,23 @@ class RoverAckermannControl : public ModuleParams
 
 	// Controllers
 	PID_t _pid_throttle; // The PID controller for the closed loop speed control
+	PID_t _pid_lat_accel; // The PID controller for the closed loop speed control
 	SlewRate<float> _steering_with_rate_limit{0.f};
 	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_p_speed,
-		(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
+		(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_speed_p,
+		(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_speed_i,
+		(ParamFloat<px4::params::RA_LAT_ACCEL_P>) _param_ra_lat_accel_p,
+		(ParamFloat<px4::params::RA_LAT_ACCEL_I>) _param_ra_lat_accel_i,
 		(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
 		(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
 		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
 		(ParamFloat<px4::params::RA_MAX_THR_SPEED>) _param_ra_max_thr_speed,
 		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
 		(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate,
+		(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
 		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
 	)
 };