Add support for stabilized flight mode for rovers

This commit adds support for stabilized flight mode for rovers, which enables the rover tracking a fixed heading that is set with a manual input

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -116,8 +116,58 @@ RoverPositionControl::vehicle_control_mode_poll()
 void
 RoverPositionControl::manual_control_setpoint_poll()
 {
-	if (_manual_control_setpoint_sub.updated()) {
-		_manual_control_setpoint_sub.copy(&_manual_control_setpoint);
+	if (_control_mode.flag_control_manual_enabled) {
+		if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
+			float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f,  0.0002f, 0.04f);
+
+			if (!_control_mode.flag_control_climb_rate_enabled &&
+			    !_control_mode.flag_control_offboard_enabled) {
+
+				if (_control_mode.flag_control_attitude_enabled) {
+					// STABILIZED mode generate the attitude setpoint from manual user inputs
+					_att_sp.roll_body = 0.0;
+					_att_sp.pitch_body = 0.0;
+
+					/* reset yaw setpoint to current position if needed */
+					if (_reset_yaw_sp) {
+						const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi();
+						_manual_yaw_sp = vehicle_yaw;
+						_reset_yaw_sp = false;
+
+					} else {
+						const float yaw_rate = math::radians(_param_gnd_man_y_max.get());
+						_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
+						_manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
+					}
+
+					_att_sp.yaw_body = _manual_yaw_sp;
+					_att_sp.thrust_body[0] = _manual_control_setpoint.z;
+
+					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 {
+					_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.y; // Nominally yaw: _manual_control_setpoint.r;
+					// Set throttle from the manual throttle channel
+					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
+					_reset_yaw_sp = true;
+				}
+
+			} else {
+				_reset_yaw_sp = true;
+			}
+
+			_manual_setpoint_last_called = hrt_absolute_time();
+		}
 	}
 }
 
@@ -352,8 +402,6 @@ RoverPositionControl::Run()
 		/* update parameters from storage */
 		parameters_update();
 
-		bool manual_mode = _control_mode.flag_control_manual_enabled;
-
 		/* only run controller if position changed */
 		if (_local_pos_sub.update(&_local_pos)) {
 
@@ -382,7 +430,7 @@ RoverPositionControl::Run()
 			matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon);
 			matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
 
-			if (!manual_mode && _control_mode.flag_control_position_enabled) {
+			if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
 
 				if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 
@@ -412,7 +460,7 @@ RoverPositionControl::Run()
 
 				}
 
-			} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) {
+			} else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) {
 
 				control_velocity(current_velocity, _pos_sp_triplet);
 
@@ -420,30 +468,18 @@ RoverPositionControl::Run()
 		}
 
 		// Respond to an attitude update and run the attitude controller if enabled
-		if (!manual_mode && _control_mode.flag_control_attitude_enabled
+		if (_control_mode.flag_control_attitude_enabled
 		    && !_control_mode.flag_control_position_enabled
 		    && !_control_mode.flag_control_velocity_enabled) {
-
 			control_attitude(_vehicle_att, _att_sp);
 
 		}
 
-		// Manual pass-through if no other control mode is enabled
-		if (manual_mode) {
-			_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.y; // Nominally yaw: _manual_control_setpoint.r;
-			// Set throttle from the manual throttle channel
-			_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
-		}
-
-		// publish the controller output
+		/* Only publish if any of the proper modes are enabled */
 		if (_control_mode.flag_control_velocity_enabled ||
 		    _control_mode.flag_control_attitude_enabled ||
 		    _control_mode.flag_control_position_enabled ||
-		    manual_mode) {
+		    _control_mode.flag_control_manual_enabled) {
 			// timestamp and publish controls
 			_act_controls.timestamp = hrt_absolute_time();
 			_actuator_controls_pub.publish(_act_controls);

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -102,6 +102,7 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	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 */
 
@@ -126,6 +127,7 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
+	hrt_abstime _manual_setpoint_last_called{0};
 
 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
@@ -150,6 +152,9 @@ private:
 	/* previous waypoint */
 	matrix::Vector2d _prev_wp{0, 0};
 
+	float _manual_yaw_sp{0.0};
+	bool _reset_yaw_sp{true};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
 		(ParamFloat<px4::params::GND_L1_DAMPING>) _param_l1_damping,
@@ -171,6 +176,7 @@ private:
 
 		(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
 		(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
+		(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 */
 	)
 

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -273,3 +273,14 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
  * @group Rover Position Control
  */
 PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 400
+ * @decimal 1
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);