ackermann: restructure module

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

@@ -16,15 +16,17 @@ param set-default NAV_ACC_RAD 0.5
 param set-default RA_ACC_RAD_GAIN 2
 param set-default RA_ACC_RAD_MAX 3
 param set-default RA_MAX_ACCEL 1.5
+param set-default RA_MAX_DECEL 10
 param set-default RA_MAX_JERK 15
-param set-default RA_MAX_SPEED 3
+param set-default RA_MAX_SPEED 5
+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_VEL_DEF 3
-param set-default RA_MISS_VEL_GAIN 5
-param set-default RA_MISS_VEL_MIN 1
+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 2
+param set-default RA_SPEED_P 0.1
 param set-default RA_WHEEL_BASE 0.321
 
 # Pure Pursuit parameters

msg/CMakeLists.txt

@@ -185,6 +185,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
+	RoverAckermannSetpoint.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg

msg/RoverAckermannGuidanceStatus.msg

@@ -1,8 +1,6 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 desired_speed 		# [m/s] Rover desired ground speed
 float32 lookahead_distance 	# [m] Lookahead distance of pure the pursuit controller
 float32 heading_error 		# [deg] Heading error of the pure pursuit controller
-float32 pid_throttle_integral 	# [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
 
 # TOPICS rover_ackermann_guidance_status

msg/RoverAckermannSetpoint.msg

@@ -0,0 +1,8 @@
+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
+
+# TOPICS rover_ackermann_setpoint

msg/RoverAckermannStatus.msg

@@ -1,7 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 throttle_setpoint 	# [-1, 1] Normalized throttle setpoint
-float32 steering_setpoint 	# [-1, 1] Normalized steering setpoint
-float32 actual_speed       	# [m/s] Rover ground speed
+float32 measured_forward_speed       	      # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
+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 pid_throttle_integral                 # Integral of the PID for the closed loop speed controller
 
 # TOPICS rover_ackermann_status

src/modules/logger/logged_topics.cpp

@@ -104,6 +104,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_optional_topic("rover_ackermann_guidance_status", 100);
+	add_optional_topic("rover_ackermann_setpoint", 100);
 	add_optional_topic("rover_ackermann_status", 100);
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_setpoint", 100);

src/modules/rover_ackermann/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(RoverAckermannGuidance)
+add_subdirectory(RoverAckermannControl)
 
 px4_add_module(
 	MODULE modules__rover_ackermann
@@ -41,6 +42,7 @@ px4_add_module(
 		RoverAckermann.hpp
 	DEPENDS
 		RoverAckermannGuidance
+		RoverAckermannControl
 		px4_work_queue
 		SlewRate
 		pure_pursuit

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -33,14 +33,11 @@
 
 #include "RoverAckermann.hpp"
 
-using namespace time_literals;
-using namespace matrix;
-
 RoverAckermann::RoverAckermann() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_rover_ackermann_status_pub.advertise();
+	_rover_ackermann_setpoint_pub.advertise();
 	updateParams();
 }
 
@@ -53,16 +50,6 @@ bool RoverAckermann::init()
 void RoverAckermann::updateParams()
 {
 	ModuleParams::updateParams();
-
-	// Update slew rates
-	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
-		_throttle_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get());
-	}
-
-	if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
-		_steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) /
-						      _param_ra_max_steer_angle.get());
-	}
 }
 
 void RoverAckermann::Run()
@@ -75,45 +62,47 @@ void RoverAckermann::Run()
 
 	updateSubscriptions();
 
-	// Timestamps
-	hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	// Generate motor setpoints
-	if (_armed) {
-		switch (_nav_state) {
-		case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
-				manual_control_setpoint_s manual_control_setpoint{};
-
-				if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-					_motor_setpoint.steering = manual_control_setpoint.roll;
-					_motor_setpoint.throttle =  manual_control_setpoint.throttle;
-				}
-
-			} break;
-
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-			_motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
-			break;
-
-		default: // Unimplemented nav states will stop the rover
-			_motor_setpoint.steering = 0.f;
-			_motor_setpoint.throttle =  0.f;
-			_throttle_with_accel_limit.setForcedValue(0.f);
-			_steering_with_rate_limit.setForcedValue(0.f);
-			break;
-		}
+	// Generate and publish speed and steering setpoints
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	switch (_nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+			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 = manual_control_setpoint.roll;
+				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		_ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed);
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		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 =  0.f;
+		rover_ackermann_setpoint.steering_setpoint = NAN;
+		rover_ackermann_setpoint.steering_setpoint_normalized = 0.f;
+		_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+		break;
+	}
 
-	} else { // Reset on disarm
-		_motor_setpoint.steering = 0.f;
-		_motor_setpoint.throttle =  0.f;
-		_throttle_with_accel_limit.setForcedValue(0.f);
-		_steering_with_rate_limit.setForcedValue(0.f);
+	if (!_armed) {
+		_ackermann_control.resetControllers();
 	}
 
-	publishMotorSetpoints(applySlewRates(_motor_setpoint, dt));
+	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
+
 }
 
 void RoverAckermann::updateSubscriptions()
@@ -129,69 +118,20 @@ void RoverAckermann::updateSubscriptions()
 		_armed = vehicle_status.arming_state == 2;
 	}
 
-	if (_local_position_sub.updated()) {
-		vehicle_local_position_s local_position{};
-		_local_position_sub.copy(&local_position);
-		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
-		_actual_speed = rover_velocity.norm();
-	}
-}
-motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt)
-{
-	// Sanitize actuator commands
-	if (!PX4_ISFINITE(motor_setpoint.steering)) {
-		motor_setpoint.steering = 0.f;
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}
 
-	if (!PX4_ISFINITE(motor_setpoint.throttle)) {
-		motor_setpoint.throttle = 0.f;
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		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);
 	}
-
-	// Acceleration slew rate
-	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
-	    && fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
-		_throttle_with_accel_limit.update(motor_setpoint.throttle, dt);
-
-	} else {
-		_throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle);
-	}
-
-	// Steering slew rate
-	if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
-		_steering_with_rate_limit.update(motor_setpoint.steering, dt);
-
-	} else {
-		_steering_with_rate_limit.setForcedValue(motor_setpoint.steering);
-	}
-
-	motor_setpoint_struct motor_setpoint_temp{};
-	motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
-	motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
-	return motor_setpoint_temp;
-}
-
-void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates)
-{
-	// Publish rover Ackermann status (logging)
-	rover_ackermann_status_s rover_ackermann_status{};
-	rover_ackermann_status.timestamp = _timestamp;
-	rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
-	rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
-	rover_ackermann_status.actual_speed = _actual_speed;
-	_rover_ackermann_status_pub.publish(rover_ackermann_status);
-
-	// Publish to motor
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle;
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	// Publish to servo
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering;
-	actuator_servos.timestamp = _timestamp;
-	_actuator_servos_pub.publish(actuator_servos);
 }
 
 int RoverAckermann::task_spawn(int argc, char *argv[])

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -39,27 +39,25 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
 
 // uORB includes
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/rover_ackermann_status.h>
+#include <uORB/topics/rover_ackermann_setpoint.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 
 // Standard library includes
 #include <math.h>
+#include <matrix/matrix/math.hpp>
 
 // Local includes
 #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
-using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint;
+#include "RoverAckermannControl/RoverAckermannControl.hpp"
 
 using namespace time_literals;
 
@@ -96,49 +94,25 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	 */
 	void updateSubscriptions();
 
-	/**
-	 * @brief Apply slew rates to motor setpoints.
-	 * @param motor_setpoint Normalized steering and throttle setpoints.
-	 * @param dt Time since last update [s].
-	 * @return Motor setpoint with applied slew rates.
-	 */
-	motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt);
-
-	/**
-	 * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus.
-	 * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates.
-	 */
-	void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates);
-
 	// uORB subscriptions
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 
 	// uORB publications
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
-	uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};
+	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
 
 	// Class instances
 	RoverAckermannGuidance _ackermann_guidance{this};
+	RoverAckermannControl _ackermann_control{this};
 
 	// Variables
+	matrix::Quatf _vehicle_attitude_quaternion{};
 	int _nav_state{0};
-	motor_setpoint_struct _motor_setpoint;
-	hrt_abstime _timestamp{0};
-	float _actual_speed{0.f};
-	SlewRate<float> _steering_with_rate_limit{0.f};
-	SlewRate<float> _throttle_with_accel_limit{0.f};
+	float _vehicle_forward_speed{0.f};
+	float _vehicle_yaw{0.f};
 	bool _armed{false};
 
-	// Parameters
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
-		(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_ACCEL>) _param_ra_max_accel,
-		(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate
-	)
 };