RoverPosControl: Fix up rate / attitude / velocity /pos control

- There were small points that were missed from the original PR: https://github.com/PX4/PX4-Autopilot/pull/20082
- This was tested in sync with the QGC PR, and was updated accordingly: https://github.com/mavlink/qgroundcontrol/pull/10483
- This modifies some of the parameters unit / range / name /
definitions, as some didn't make sense (e.g. GND_SPEED_MAX being used as
an output limiter for the velocity PID controller? the unit doens't
match)
- Also added clear unit documentation & function documentation to make
sure we don't mess up the radians / degrees, etc.

RoverPosControl: Multiply Roll command via Max Rate Parameter

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -101,7 +101,7 @@ 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()));
@@ -139,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);
@@ -147,19 +149,19 @@ 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) {
-					// STABILIZED mode generate the attitude setpoint from manual user inputs
+					// 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;
+					_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();
@@ -167,8 +169,9 @@ RoverPositionControl::manual_control_setpoint_poll()
 					_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;
@@ -276,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));
@@ -419,15 +428,25 @@ 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", double(euler_sp(2)));
+	// PX4_INFO("Yaw error: %f [rad]", double(euler_sp(2)));
 
 	_rates_sp.roll = 0.0;
 	_rates_sp.pitch = 0.0;
-	_rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get();
+
+	// 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();
+
+	// 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
@@ -441,6 +460,8 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con
 	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());
 
@@ -455,6 +476,9 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con
 	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;
 }
 
 
@@ -489,6 +513,7 @@ RoverPositionControl::Run()
 			/* load local copies */
 			_global_pos_sub.update(&_global_pos);
 
+			// Get the latest position setpoint
 			position_setpoint_triplet_poll();
 
 			if (!_global_local_proj_ref.isInitialized()
@@ -500,7 +525,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);
 
@@ -546,6 +571,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) {
@@ -589,6 +618,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

@@ -76,6 +76,7 @@
 #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>
@@ -114,6 +115,7 @@ private:
 	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)};
@@ -127,14 +129,25 @@ private:
 	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 > */
-	vehicle_rates_setpoint_s		_rates_sp{};			/**< rate 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{};
@@ -191,7 +204,7 @@ 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,
@@ -233,12 +246,45 @@ private:
 	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

@@ -143,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);
@@ -210,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
@@ -237,6 +237,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
 /**
  * Minimum ground speed
  *
+ * Used for calculating saturated speed lower bound for waypoint navigation (position mode)
  *
  * @unit m/s
  * @min 0.0
@@ -248,17 +249,26 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
 PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f);
 
 /**
- * Maximum ground speed
+ * 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?
  *
- * @unit m/s
  * @min 0.0
  * @max 40
  * @decimal 1
  * @increment 0.5
  * @group Rover Position Control
  */
-PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(GND_SPEED_PID_MX, 10.0f);
 
 /**
  * Maximum turn angle for Ackerman steering.
@@ -278,8 +288,6 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
 /**
  * Attitude control P gain
  *
- *
- * @unit rad
  * @min 0.0
  * @max 5.0
  * @decimal 3