ackermann: add acro mode

chfriedrich98 · Oct 1, 2024 · 0d057e1 · 0d057e1
1 parent c184b38
commit 0d057e1
Show file tree

Hide file tree

Showing 7 changed files with 118 additions and 11 deletions.
diff --git a/msg/RoverAckermannSetpoint.msg b/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
diff --git a/msg/RoverAckermannStatus.msg b/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
diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/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();
 }
 
@@ -81,6 +84,23 @@ void RoverAckermann::Run()
 
 		} 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{};
 
@@ -143,7 +163,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);
 
 }
 
@@ -176,10 +196,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 = 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);
 	}
 }
 

diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp
@@ -55,6 +55,7 @@
 // Standard library includes
 #include <math.h>
 #include <matrix/matrix/math.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 // Local includes
 #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
@@ -126,12 +127,17 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	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
 
 	)

diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/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,7 +74,8 @@ 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;
@@ -90,6 +98,17 @@ 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 steering_setpoint = asinf(math::constrain(_param_ra_wheel_base.get() *
+						_rover_ackermann_setpoint.lateral_acceleration_setpoint / powf(
+							vehicle_forward_speed, 2.f), -1.f, 1.f));
+		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());
+	}
+
 	// Steering control
 	float steering_normalized{0.f};
 
@@ -116,7 +135,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

diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/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
 	)
 };
diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml
@@ -204,3 +204,37 @@ parameters:
         increment: 0.01
         decimal: 2
         default: -1
+
+      RA_MAX_LAT_ACCEL:
+        description:
+          short: Maximum allowed lateral acceleration
+          long: |
+            This parameter is used to cap the lateral acceleration and map controller inputs to desired lateral acceleration
+            in Acro, Stabilized and Position mode.
+        type: float
+        unit: m/s^2
+        min: -1
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: -1
+
+      RA_LAT_ACCEL_P:
+        description:
+          short: Proportional gain for lateral acceleration controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_LAT_ACCEL_I:
+        description:
+          short: Integral gain for lateral acceleration controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 1