rover: seperate speed control

Author: chfriedrich98

msg/CMakeLists.txt

@@ -181,10 +181,10 @@ set(msg_files
 	RoverPositionSetpoint.msg
 	RoverRateSetpoint.msg
 	RoverRateStatus.msg
+	RoverSpeedSetpoint.msg
+	RoverSpeedStatus.msg
 	RoverSteeringSetpoint.msg
 	RoverThrottleSetpoint.msg
-	RoverVelocitySetpoint.msg
-	RoverVelocityStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverSpeedSetpoint.msg

@@ -0,0 +1,5 @@
+# Rover Speed Setpoint
+
+uint64 timestamp # [us] Time since system start
+float32 speed_body_x # [m/s] [@range -inf (Backwards), inf (Forwards)] [@frame Body] Speed setpoint in body x direction
+float32 speed_body_y # [m/s] [@range -inf (Left), inf (Right)] [@frame Body] [@invalid NaN If not mecanum] Mecanum only: Speed setpoint in body y direction

msg/RoverVelocityStatus.msg renamed to msg/RoverSpeedStatus.msg

@@ -111,10 +111,10 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_position_setpoint", 100);
 	add_optional_topic("rover_rate_setpoint", 100);
 	add_optional_topic("rover_rate_status", 100);
+	add_optional_topic("rover_speed_setpoint", 100);
+	add_optional_topic("rover_speed_status", 100);
 	add_optional_topic("rover_steering_setpoint", 100);
 	add_optional_topic("rover_throttle_setpoint", 100);
-	add_optional_topic("rover_velocity_setpoint", 100);
-	add_optional_topic("rover_velocity_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

msg/RoverVelocitySetpoint.msg

@@ -42,7 +42,7 @@ AckermannManualMode::AckermannManualMode(ModuleParams *parent) : ModuleParams(pa
 	_rover_steering_setpoint_pub.advertise();
 	_rover_rate_setpoint_pub.advertise();
 	_rover_attitude_setpoint_pub.advertise();
-	_rover_velocity_setpoint_pub.advertise();
+	_rover_speed_setpoint_pub.advertise();
 	_rover_position_setpoint_pub.advertise();
 }
 
@@ -164,12 +164,10 @@ void AckermannManualMode::position()
 		_pos_ctl_course_direction = Vector2f(NAN, NAN);
 
 		// Speed control
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = hrt_absolute_time();
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = NAN;
-		rover_velocity_setpoint.yaw = NAN;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+		rover_speed_setpoint_s rover_speed_setpoint{};
+		rover_speed_setpoint.timestamp = hrt_absolute_time();
+		rover_speed_setpoint.speed_body_x = speed_setpoint;
+		_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
 
 		// Rate control
 		rover_rate_setpoint_s rover_rate_setpoint{};

src/modules/logger/logged_topics.cpp

@@ -51,7 +51,7 @@
 #include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/rover_rate_setpoint.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_speed_setpoint.h>
 #include <uORB/topics/rover_position_setpoint.h>
 
 /**
@@ -83,7 +83,7 @@ class AckermannManualMode : public ModuleParams
 	void stab();
 
 	/**
-	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
+	 * @brief Generate and publish roverSpeedSetpoint/roverRateSetpoint or roverPositionSetpoint from manualControlSetpoint.
 	 */
 	void position();
 
@@ -109,7 +109,7 @@ class AckermannManualMode : public ModuleParams
 	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
 	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
 	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_speed_setpoint_s>    _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
 	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
 
 	// Variables

src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp

@@ -38,8 +38,9 @@ using namespace time_literals;
 AckermannOffboardMode::AckermannOffboardMode(ModuleParams *parent) : ModuleParams(parent)
 {
 	updateParams();
-	_rover_velocity_setpoint_pub.advertise();
+	_rover_speed_setpoint_pub.advertise();
 	_rover_position_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
 }
 
 void AckermannOffboardMode::updateParams()
@@ -69,11 +70,13 @@ void AckermannOffboardMode::offboardControl()
 
 	} else if (offboard_control_mode.velocity) {
 		const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = hrt_absolute_time();
-		rover_velocity_setpoint.speed = velocity_ned.norm();
-		rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
+		rover_speed_setpoint_s rover_speed_setpoint{};
+		rover_speed_setpoint.timestamp = hrt_absolute_time();
+		rover_speed_setpoint.speed_body_x = velocity_ned.norm();
+		_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 	}
 }

src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp

@@ -43,7 +43,8 @@
 // uORB includes
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_speed_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
 #include <uORB/topics/rover_position_setpoint.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/trajectory_setpoint.h>
@@ -80,6 +81,7 @@ class AckermannOffboardMode : public ModuleParams
 	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 
 	// uORB publications
-	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_speed_setpoint_s>    _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)};
 	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
 };

src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp

@@ -38,7 +38,8 @@ using namespace time_literals;
 AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent)
 {
 	_pure_pursuit_status_pub.advertise();
-	_rover_velocity_setpoint_pub.advertise();
+	_rover_speed_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
 
 	updateParams();
 }
@@ -47,6 +48,7 @@ void AckermannPosControl::updateParams()
 {
 	ModuleParams::updateParams();
 	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+	_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
 
 }
 
@@ -82,20 +84,36 @@ void AckermannPosControl::updatePosControl()
 			const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
 						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
 						       _curr_pos_ned, fabsf(speed_setpoint));
+
+			if (_param_ro_speed_red.get() > FLT_EPSILON) {
+				const float course_error = fabsf(matrix::wrap_pi(bearing_setpoint - _vehicle_yaw));
+				const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
+							      0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
+				const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed,
+									_param_ro_max_thr_speed.get());
+				speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed);
+			}
+
 			_pure_pursuit_status_pub.publish(pure_pursuit_status);
-			rover_velocity_setpoint_s rover_velocity_setpoint{};
-			rover_velocity_setpoint.timestamp = timestamp;
-			rover_velocity_setpoint.speed = speed_setpoint;
-			rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
+			rover_speed_setpoint_s rover_speed_setpoint{};
+			rover_speed_setpoint.timestamp = timestamp;
+			rover_speed_setpoint.speed_body_x = speed_setpoint;
+			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
+			rover_attitude_setpoint_s rover_attitude_setpoint{};
+			rover_attitude_setpoint.timestamp = timestamp;
+			rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
 					bearing_setpoint + M_PI_F);
-			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
 		} else {
-			rover_velocity_setpoint_s rover_velocity_setpoint{};
-			rover_velocity_setpoint.timestamp = timestamp;
-			rover_velocity_setpoint.speed = 0.f;
-			rover_velocity_setpoint.bearing = _vehicle_yaw;
-			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+			rover_speed_setpoint_s rover_speed_setpoint{};
+			rover_speed_setpoint.timestamp = timestamp;
+			rover_speed_setpoint.speed_body_x = 0.f;
+			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
+			rover_attitude_setpoint_s rover_attitude_setpoint{};
+			rover_attitude_setpoint.timestamp = timestamp;
+			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
+			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 		}
 	}
 }