RoverPosControl: Using torque setpoint directly as steering input

- Although the discussion is still ongoing: https://github.com/PX4/PX4-Autopilot/pull/20082#discussion_r962169411
- The original code would cause quick saturation in the steering input,
as it doesn't even have a conecpt of time
- So using direct mapping for now, but we can revert this back and find
the correct solution

ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover

@@ -25,7 +25,6 @@ param set-default NAV_LOITER_RAD 2
 param set-default CBRK_AIRSPD_CHK 162128
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover

@@ -25,7 +25,6 @@ param set-default NAV_LOITER_RAD 2
 param set-default CBRK_AIRSPD_CHK 162128
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 6
 

ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1

@@ -32,7 +32,6 @@ param set-default NAV_LOITER_RAD 2
 param set-default CBRK_AIRSPD_CHK 162128
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 3.0
 
 param set-default CA_AIRFRAME 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat

@@ -13,19 +13,23 @@ param set-default GND_SPEED_I 8
 param set-default GND_SPEED_IMAX 0.125
 param set-default GND_SPEED_P 2
 param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 1
+param set-default GND_SPEED_TRIM 3
 param set-default GND_THR_CRUISE 0.85
 param set-default GND_THR_MAX 1
 param set-default GND_THR_MIN 0
+param set-default GND_RATE_P 0.08
+param set-default GND_RATE_D 0.02
+param set-default GND_RATE_FF 0.0
+param set-default GND_RATE_I 0.0
+param set-default GND_RATE_MAX 0.5
 
 param set-default MIS_TAKEOFF_ALT 0.01
-param set-default NAV_ACC_RAD 0.5
+param set-default NAV_ACC_RAD 5
 param set-default NAV_LOITER_RAD 2
 
 param set-default CBRK_AIRSPD_CHK 162128
 
 param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
 
 param set-default CA_AIRFRAME 9
 

ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx

@@ -34,7 +34,6 @@ param set-default GND_THR_MAX 0.5
 
 # Differential drive acts like ackermann steering with a maximum turn angle of 60 degrees, or pi/3 radians
 param set-default GND_MAX_ANG 1.042
-param set-default GND_WHEEL_BASE 0.17
 
 # TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
 # to support negative throttle.

src/modules/rover_pos_control/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
 px4_add_module(
 	MODULE modules__rover_pos_control
 	MAIN rover_pos_control
@@ -37,6 +38,7 @@ px4_add_module(
 		RoverPositionControl.cpp
 		RoverPositionControl.hpp
 	DEPENDS
+		RateControl
 		l1
 		pid
 	)

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -101,7 +101,11 @@ void RoverPositionControl::parameters_update(bool force)
 				   _param_speed_i.get(),
 				   _param_speed_d.get(),
 				   _param_speed_imax.get(),
-				   _param_gndspeed_max.get());
+				   _param_gndspeed_pid_out_max.get());
+		_rate_control.setGains(matrix::Vector3f(0.0, 0.0, _param_rate_p.get()), matrix::Vector3f(0.0, 0.0, _param_rate_i.get()),
+				       matrix::Vector3f(0.0, 0.0, _param_rate_d.get()));
+		_rate_control.setFeedForwardGain(matrix::Vector3f(0.0, 0.0, _param_rate_ff.get()));
+		_rate_control.setIntegratorLimit(matrix::Vector3f(0.0, 0.0, _param_rate_imax.get()));
 	}
 }
 
@@ -135,6 +139,8 @@ RoverPositionControl::manual_control_setpoint_poll()
 						_reset_yaw_sp = false;
 
 					} else {
+						// TODO: Clarify relationship between RATE_MAX and MAN_Y_MAX.
+						// Currently, in Acro mode we use RATE_MAX as max rate setpoint, and in manual (attitude mode == stabilized), MAN_Y_MAX.
 						const float yaw_rate = math::radians(_param_gnd_man_y_max.get());
 						_att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate;
 						_manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
@@ -143,17 +149,29 @@ RoverPositionControl::manual_control_setpoint_poll()
 					_att_sp.yaw_body = _manual_yaw_sp;
 					_att_sp.thrust_body[0] = _manual_control_setpoint.throttle;
 
+					// Question: What's the point of having yaw_body, but then putting that into Quaternion again?
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
 
 					_att_sp.timestamp = hrt_absolute_time();
 
-
 					_attitude_sp_pub.publish(_att_sp);
 
+				} else if (_control_mode.flag_control_rates_enabled) {
+					// ACRO mode generate the rate setpoint from manual user inputs
+					_rates_sp.roll = 0.0;
+					_rates_sp.pitch = 0.0;
+					_rates_sp.yaw = _manual_control_setpoint.y * _param_rate_max.get();
+					_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
+
+					_rates_sp.timestamp = hrt_absolute_time();
+
+					_rates_sp_pub.publish(_rates_sp);
+
 				} else {
-					_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll;
-					_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch;
+					// MANUAL mode, directly map actuator outputs from Manual control
+					_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
+					_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
 					// Set heading from the manual roll input channel
 					_act_controls.control[actuator_controls_s::INDEX_YAW] =
 						_manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
@@ -187,6 +205,14 @@ RoverPositionControl::attitude_setpoint_poll()
 	}
 }
 
+void
+RoverPositionControl::rates_setpoint_poll()
+{
+	if (_rates_sp_sub.updated()) {
+		_rates_sp_sub.copy(&_rates_sp);
+	}
+}
+
 void
 RoverPositionControl::vehicle_attitude_poll()
 {
@@ -195,6 +221,14 @@ RoverPositionControl::vehicle_attitude_poll()
 	}
 }
 
+void
+RoverPositionControl::vehicle_angular_acceleration_poll()
+{
+	if (_vehicle_angular_acceleration_sub.updated()) {
+		_vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration);
+	}
+}
+
 bool
 RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				       const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -232,6 +266,7 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 		matrix::Vector2f ground_speed_2d(ground_speed);
 
 		float mission_throttle = _param_throttle_cruise.get();
+		float max_yaw_rate = _param_rate_max.get();
 
 		/* Just control the throttle */
 		if (_param_speed_control_mode.get() == 1) {
@@ -244,6 +279,12 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 				mission_target_speed = _pos_sp_triplet.current.cruising_speed;
 			}
 
+			// Log target speed in global NED frame (since we don't always enforce the heading, it is in the direction we are headed)
+			const Vector3f target_vel_vec = mission_target_speed * ground_speed.unit();
+			_local_pos_setpoint.vx = target_vel_vec(0);
+			_local_pos_setpoint.vy = target_vel_vec(1);
+			_local_pos_setpoint.vz = target_vel_vec(2);
+
 			// Velocity in body frame
 			const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
@@ -287,21 +328,29 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
 								 prev_wp(1));
 					_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
 
-					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
-
-					float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
-					float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
-					float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
-								       _gnd_control.nav_lateral_acceleration_demand());
-					control_effort = math::constrain(control_effort, -1.0f, 1.0f);
-					_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+					///WIP: Convert Lateral acceleration demand to rate control reference
+					float min_speed = _param_gndspeed_min.get();
+					matrix::Vector2f saturated_speed_2d = (ground_speed_2d.norm() < min_speed) ? ground_speed_2d :
+									      ground_speed_2d.normalized() * min_speed;
+					///TODO: Max yaw rate
+					float desired_yaw_rate = _gnd_control.nav_lateral_acceleration_demand() / saturated_speed_2d.norm();
+					_rates_sp.roll = 0.0;
+					_rates_sp.pitch = 0.0;
+					_rates_sp.yaw = math::constrain(desired_yaw_rate, -max_yaw_rate, max_yaw_rate);
+					_rates_sp.thrust_body[0] = math::constrain(mission_throttle, 0.0f, 1.0f);
+					_rates_sp.timestamp = hrt_absolute_time();
+					_rates_sp_pub.publish(_rates_sp);
 				}
 			}
 			break;
 
 		case STOPPING: {
-				_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
-				_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
+				_rates_sp.roll = 0.0;
+				_rates_sp.pitch = 0.0;
+				_rates_sp.yaw = 0.0;
+				_rates_sp.thrust_body[0] = 0.0;
+				_rates_sp.timestamp = hrt_absolute_time();
+				_rates_sp_pub.publish(_rates_sp);
 				// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
 				float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
 							       (double)curr_wp(0), (double)curr_wp(1));
@@ -379,32 +428,76 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
 {
 	// quaternion attitude control law, qe is rotation from q to qd
 	const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
+
+	// Attitude error in Euler angles [rad]
 	const Eulerf euler_sp = qe;
+	// PX4_INFO("Yaw error: %f [rad]", double(euler_sp(2)));
 
-	float control_effort = euler_sp(2) / _param_max_turn_angle.get();
-	control_effort = math::constrain(control_effort, -1.0f, 1.0f);
+	_rates_sp.roll = 0.0;
+	_rates_sp.pitch = 0.0;
 
-	_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
+	// Apply Attitude P-gain for Yaw control. Euler angle in [rad], rate setpoint in [rad/s]
+	_rates_sp.yaw = euler_sp(2) * _param_att_p.get();
 
-	const float control_throttle = att_sp.thrust_body[0];
+	// PX4_INFO("Yaw rate setpoint in attitude control: %f [rad/s]", (double)_rates_sp.yaw);
+
+	_rates_sp.thrust_body[0] = math::constrain(att_sp.thrust_body[0], 0.0f, 1.0f);
+	_rates_sp.timestamp = hrt_absolute_time();
+	_rates_sp_pub.publish(_rates_sp);
+
+	// Log yaw setpoint for PID tuning debug
+	_local_pos_setpoint.yaw = att_sp.yaw_body;
+}
+
+void
+RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const  vehicle_angular_acceleration_s &acc,
+				    const vehicle_local_position_s &local_pos,
+				    const vehicle_rates_setpoint_s &rates_sp)
+{
+	float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f;
+	_control_rates_last_called = hrt_absolute_time();
+
+	const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]);
+	const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw);
+
+	// PX4_INFO("Yaw rate setpoint: %f [rad/s]", (double)_rates_sp.yaw);
+
+	const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
+	bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get());
+
+	const matrix::Vector3f angular_acceleration{acc.xyz};
+	const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt,
+					lock_integrator);
+
+	// Direct mapping from torque setpoint to steering
+	_steering_input = math::constrain(torque(2), -1.0f, 1.0f);
+
+	// _steering_input = math::constrain(_steering_input + torque(2), -1.0f, 1.0f);
+
+	_act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input;
+
+	const float control_throttle = rates_sp.thrust_body[0];
 
 	_act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);
 
+	// Log yawrate setpoint for PID tuning
+	_local_pos_setpoint.yawspeed = rates_sp.yaw;
 }
 
+
 void
 RoverPositionControl::Run()
 {
 	parameters_update(true);
 
 	/* run controller on gyro changes */
-	vehicle_angular_velocity_s angular_velocity;
-
-	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
-
+	if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
+		// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
+		vehicle_angular_acceleration_poll();
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 		attitude_setpoint_poll();
+		rates_setpoint_poll();
 		vehicle_attitude_poll();
 		manual_control_setpoint_poll();
 
@@ -413,12 +506,17 @@ RoverPositionControl::Run()
 		/* update parameters from storage */
 		parameters_update();
 
+		if (!PX4_ISFINITE(_steering_input)) {
+			_steering_input = 0.0;
+		}
+
 		/* only run controller if position changed */
 		if (_local_pos_sub.update(&_local_pos)) {
 
 			/* load local copies */
 			_global_pos_sub.update(&_global_pos);
 
+			// Get the latest position setpoint
 			position_setpoint_triplet_poll();
 
 			if (!_global_local_proj_ref.isInitialized()
@@ -430,7 +528,7 @@ RoverPositionControl::Run()
 				_global_local_alt0 = _local_pos.ref_alt;
 			}
 
-			// Convert Local setpoints to global setpoints
+			// Handle Offboard control: override position setpoint triplet item with trajectory_setpoint data
 			if (_control_mode.flag_control_offboard_enabled) {
 				_trajectory_setpoint_sub.update(&_trajectory_setpoint);
 
@@ -476,6 +574,10 @@ RoverPositionControl::Run()
 					pos_ctrl_status.timestamp = hrt_absolute_time();
 
 					_pos_ctrl_status_pub.publish(pos_ctrl_status);
+
+					// Project position setpoint triplet's global position (lat/lon) into local (x/y/z) position
+					_global_local_proj_ref.project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, _local_pos_setpoint.x,
+								       _local_pos_setpoint.y);
 				}
 
 			} else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) {
@@ -492,6 +594,11 @@ RoverPositionControl::Run()
 
 		}
 
+		//Body Rate control
+		if (_control_mode.flag_control_rates_enabled) {
+			control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp);
+		}
+
 		/* Only publish if any of the proper modes are enabled */
 		if (_control_mode.flag_control_velocity_enabled ||
 		    _control_mode.flag_control_attitude_enabled ||
@@ -514,6 +621,20 @@ RoverPositionControl::Run()
 			v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH];
 			v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW];
 			_vehicle_torque_setpoint_pub.publish(v_torque_sp);
+
+			// Normalized (When max thrust, magnitude = 1) thrust in North - East frame
+			// const Vector2f normalized_thrust_vec_2d = _act_controls.control[actuator_controls_s::INDEX_THROTTLE] * Vector2f(_local_pos.vx, _local_pos.vy).unit();
+			// normalized_thrust_vec_2d.copyTo(_local_pos_setpoint.thrust);
+
+			// Hack: For now send the throttle setpoint in Acc X value for debugging
+			_local_pos_setpoint.acceleration[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE];
+
+			// Note: Since we are populating the local position setpoint struct's data in
+			// rate, attitude and position control, it is not guaranteed that the data
+			// is actually valid / up-to-date. Hence, it should only be used for PID tuning
+
+			// Publish local position setpoint topic (for PID tuning UI graph in QGC)
+			_local_pos_setpoint_pub.publish(_local_pos_setpoint);
 		}
 	}
 }

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -41,14 +41,15 @@
  * @author Marco Zorzi <mzorzi@student.ethz.ch>
  */
 
-#include <float.h>
 
+#include <float.h>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/pid/pid.h>
+#include <lib/rate_control/rate_control.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -67,11 +68,15 @@
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
@@ -106,9 +111,11 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Publication<vehicle_rates_setpoint_s>	_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};  /**< navigation capabilities publication */
 	uORB::Publication<actuator_controls_s>		_actuator_controls_pub{ORB_ID(actuator_controls_0)};  /**< actuator controls publication */
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_setpoint_pub{ORB_ID(vehicle_local_position_setpoint)};
 
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
@@ -118,15 +125,31 @@ private:
 	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-
+	uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
 	manual_control_setpoint_s		_manual_control_setpoint{};			    /**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
-	vehicle_attitude_setpoint_s		_att_sp{};			/**< attitude setpoint > */
+
+	// [rad] Attitude setpoint in global frame (North - East - Down)
+	vehicle_attitude_setpoint_s		_att_sp{};
+
+	// [rad/s] Angular rates setpoint in local frame (Front - Right - Down)
+	vehicle_rates_setpoint_s		_rates_sp{};
+
 	vehicle_control_mode_s			_control_mode{};		/**< control mode */
 	vehicle_global_position_s		_global_pos{};			/**< global vehicle position */
 	vehicle_local_position_s		_local_pos{};			/**< global vehicle position */
+
+	// Debug values used for sending velocity & position setpoints to GCS
+	vehicle_local_position_setpoint_s	_local_pos_setpoint{};
+
 	actuator_controls_s				_act_controls{};		/**< direct control of actuators */
 	vehicle_attitude_s				_vehicle_att{};
+	vehicle_angular_acceleration_s		_vehicle_angular_acceleration{};
+
+	// [rad/s] Vehicle's current angular rates in local frame (Front - Right - Down)
+	vehicle_angular_velocity_s _vehicle_rates{};
+
 	trajectory_setpoint_s _trajectory_setpoint{};
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
@@ -136,6 +159,7 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
+	hrt_abstime _control_rates_last_called{0};
 	hrt_abstime _manual_setpoint_last_called{0};
 
 	MapProjection _global_local_proj_ref{};
@@ -149,6 +173,8 @@ private:
 	uint8_t _pos_reset_counter{0};		// captures the number of times the estimator has reset the horizontal position
 
 	ECL_L1_Pos_Controller				_gnd_control;
+	RateControl				_rate_control;
+	float _steering_input{0.0};
 
 	enum UGV_POSCTRL_MODE {
 		UGV_POSCTRL_MODE_AUTO,
@@ -178,7 +204,8 @@ private:
 		(ParamFloat<px4::params::GND_L1_DIST>) _param_l1_distance,
 
 		(ParamFloat<px4::params::GND_SPEED_TRIM>) _param_gndspeed_trim,
-		(ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
+		(ParamFloat<px4::params::GND_SPEED_PID_MX>) _param_gndspeed_pid_out_max,
+		(ParamFloat<px4::params::GND_SPEED_MIN>) _param_gndspeed_min,
 
 		(ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
 		(ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
@@ -187,12 +214,20 @@ private:
 		(ParamFloat<px4::params::GND_SPEED_IMAX>) _param_speed_imax,
 		(ParamFloat<px4::params::GND_SPEED_THR_SC>) _param_throttle_speed_scaler,
 
+		(ParamFloat<px4::params::GND_RATE_P>) _param_rate_p,
+		(ParamFloat<px4::params::GND_RATE_I>) _param_rate_i,
+		(ParamFloat<px4::params::GND_RATE_D>) _param_rate_d,
+		(ParamFloat<px4::params::GND_RATE_FF>) _param_rate_ff,
+		(ParamFloat<px4::params::GND_RATE_IMAX>) _param_rate_imax,
+		(ParamFloat<px4::params::GND_RATE_MAX>) _param_rate_max,
+		(ParamFloat<px4::params::GND_RATE_IMINSPD>) _param_rate_i_minspeed,
+
 		(ParamFloat<px4::params::GND_THR_MIN>) _param_throttle_min,
 		(ParamFloat<px4::params::GND_THR_MAX>) _param_throttle_max,
 		(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
 
-		(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
 		(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
+		(ParamFloat<px4::params::GND_ATT_P>) _param_att_p,
 		(ParamFloat<px4::params::GND_MAN_Y_MAX>) _param_gnd_man_y_max,
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad	/**< loiter radius for Rover */
 	)
@@ -204,16 +239,54 @@ private:
 
 	void		position_setpoint_triplet_poll();
 	void		attitude_setpoint_poll();
+	void		rates_setpoint_poll();
 	void		vehicle_control_mode_poll();
 	void 		vehicle_attitude_poll();
+	void		vehicle_angular_acceleration_poll();
 	void		manual_control_setpoint_poll();
 
 	/**
-	 * Control position.
+	 * @brief Generate rate setpoint based on desired position setpoint using L1 guidance
+	 *
+	 * If the speed control mode (`GND_SP_CTRL_MODE`) is set to 1, PID velocity controller is used for
+	 * closed-loop control. Otherwise, throttle is fixed at position setpoint trpilet's cruising throttle
+	 * setting (open-loop control).
+	 *
+	 * @param global_pos [deg] Lattitude & Longitude of the vehicle currently
+	 * @param ground_speed [m/s] Global velocity in North - East - Down frame
+	 * @param _pos_sp_triplet Modified version of position setpoint triplet generated by the Navigator, indicating desired position
 	 */
 	bool		control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed,
 					 const position_setpoint_triplet_s &_pos_sp_triplet);
+
+	/**
+	 * @brief Generate actuator control setpoint based on desired trajectory setpoint
+	 *
+	 * Trajectory setpoint is received through offboard control, or a flight task.
+	 * This function is currently NOT used for position control.
+	 *
+	 * @param current_velocity [m/s] Current vehicle's velocity derived from local position uORB message in North - East - Down frame
+	 */
 	void		control_velocity(const matrix::Vector3f &current_velocity);
+
+	/**
+	 * @brief Generate rate setpoint based on desired attitude
+	 *
+	 * @param att Current vehicle's attitude (rotation from FRD body frame to NED earth frame)
+	 * @param att_sp Attitude setpoint containing body angle in North - East - Down frame
+	 */
 	void		control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
 
+	/**
+	 * @brief Generate actuator output for achieving desired rate
+	 *
+	 * @param rates [rad/s] Current vehicle's angular rates
+	 * @param acc [rad/s^2] Current vehicle's angular accelerations
+	 * @param local_pos Current vehicle's local position data (used for extracting vehicle velocity)
+	 * @param rates_sp [rad/s] Angular rate setpoint
+	 */
+	void		control_rates(const vehicle_angular_velocity_s &rates, const  vehicle_angular_acceleration_s &acc,
+				      const vehicle_local_position_s &local_pos,
+				      const vehicle_rates_setpoint_s &rates_sp);
+
 };

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -48,19 +48,6 @@
  * Controller parameters, accessible via MAVLink
  */
 
-/**
- * Distance from front axle to rear axle
- *
- * A value of 0.31 is typical for 1/10 RC cars.
- *
- * @unit m
- * @min 0.0
- * @decimal 3
- * @increment 0.01
- * @group Rover Position Control
- */
-PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
-
 /**
  * L1 distance
  *
@@ -156,10 +143,11 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
  * Control mode for speed
  *
  * This allows the user to choose between closed loop gps speed or open loop cruise throttle speed
+ *
  * @min 0
  * @max 1
- * @value 0 open loop control
- * @value 1 close the loop with gps speed
+ * @value 0 Open loop control using cruise throttle
+ * @value 1 Closed loop control using GPS Velocity
  * @group Rover Position Control
  */
 PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
@@ -223,11 +211,10 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
 /**
  * Speed to throttle scaler
  *
- * This is a gain to map the speed control output to the throttle linearly.
+ * This is a gain to map the speed control output (in somewhat [m/s]) to the throttle
  *
- * @unit %m/s
  * @min 0.005
- * @max 50.0
+ * @max 2.0
  * @decimal 3
  * @increment 0.005
  * @group Rover Position Control
@@ -248,8 +235,9 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
 PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
 
 /**
- * Maximum ground speed
+ * Minimum ground speed
  *
+ * Used for calculating saturated speed lower bound for waypoint navigation (position mode)
  *
  * @unit m/s
  * @min 0.0
@@ -258,7 +246,29 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
  * @increment 0.5
  * @group Rover Position Control
  */
-PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f);
+
+/**
+ * Maximum output of velocity PID controller
+ *
+ * Used for limiting output of the speed PID controller (which is somewhat in m/s unit)
+ *
+ * Note: We need to figure out what the unit dimension is for the output of the Velocity PID controller,
+ * since we are mapping it directly to throttle, and it will be then *sqrt of thrust (for RPM controlled ESCs),
+ * but we are calculating based off of velocity error.
+ *
+ * Note: What is the role of the I-term? E.g. in Multicopter, the I-term of the Velocity controller accounts for
+ * constant offset force (acceleration) from external source. E.g. Wind.
+ *
+ * What is it for the boat?
+ *
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_SPEED_PID_MX, 10.0f);
 
 /**
  * Maximum turn angle for Ackerman steering.
@@ -275,6 +285,17 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
  */
 PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
 
+/**
+ * Attitude control P gain
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_ATT_P, 1.0f);
+
 /**
  * Max manual yaw rate
  *
@@ -285,3 +306,87 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
  * @group Rover Position Control
  */
 PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);
+
+/**
+ * Rover Rate Proportional Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_P, 5.0f);
+
+/**
+ * Rover Rate Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_I, 0.5f);
+
+/**
+ * Rover Rate Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_D, 0.0f);
+
+/**
+ * Rover Rate Proportional Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_FF, 1.6f);
+
+/**
+ * Rover Rate Maximum Integral Gain
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 50.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_IMAX, 1.0f);
+
+/**
+ * Rover Rate Maximum Rate
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_MAX, 0.5f);
+
+/**
+ * Rover Rate Integral Minimum speed
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 50.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_RATE_IMINSPD, 1.0f);