simulation: gz bridge refactor

rover pos control: velocity control refactor

ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x

@@ -12,6 +12,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=holybro-x500}
 
 param set-default SIM_GZ_EN 1
+param set-default SIM_GZ_RUN_GZSIM 0
 
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
@@ -36,20 +37,20 @@ param set-default CA_ROTOR3_PX -0.175
 param set-default CA_ROTOR3_PY 0.175
 param set-default CA_ROTOR3_KM -0.05
 
-param set-default SIM_GZ_FUNC1 101
-param set-default SIM_GZ_FUNC2 102
-param set-default SIM_GZ_FUNC3 103
-param set-default SIM_GZ_FUNC4 104
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
 
-param set-default SIM_GZ_MIN1 150
-param set-default SIM_GZ_MIN2 150
-param set-default SIM_GZ_MIN3 150
-param set-default SIM_GZ_MIN4 150
+param set-default SIM_GZ_EC_MIN1 150
+param set-default SIM_GZ_EC_MIN2 150
+param set-default SIM_GZ_EC_MIN3 150
+param set-default SIM_GZ_EC_MIN4 150
 
-param set-default SIM_GZ_MAX1 1000
-param set-default SIM_GZ_MAX2 1000
-param set-default SIM_GZ_MAX3 1000
-param set-default SIM_GZ_MAX4 1000
+param set-default SIM_GZ_EC_MAX1 1000
+param set-default SIM_GZ_EC_MAX2 1000
+param set-default SIM_GZ_EC_MAX3 1000
+param set-default SIM_GZ_EC_MAX4 1000
 
 param set-default MPC_THR_HOVER 0.60
 

ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_striver_mini

@@ -13,6 +13,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=striver_mini}
 
 param set-default SIM_GZ_EN 1
+param set-default SIM_GZ_RUN_GZSIM 0
 
 # Control allocator parameters
 param set-default CA_AIRFRAME 2

ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8

@@ -13,6 +13,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=skywalker_x8}
 
 param set-default SIM_GZ_EN 1
+param set-default SIM_GZ_RUN_GZSIM 0
 
 # Control allocator parameters
 param set-default CA_AIRFRAME 1

ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover

@@ -15,6 +15,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=scout_mini}
 
 param set-default SIM_GZ_EN 1
+param set-default SIM_GZ_RUN_GZSIM 0
 
 param set-default CA_AIRFRAME 6
 
@@ -67,6 +68,10 @@ param set-default GND_THR_MIN 0
 param set-default GND_SPEED_P 0.4
 param set-default GND_SPEED_I 1
 param set-default GND_SPEED_D 0.001
-param set-default GND_SPEED_IMAX 1
-param set-default GND_SPEED_MAX 1
-param set-default GND_SPEED_THR_SC 1
+param set-default GND_SPEED_MAX 3.0
+param set-default GND_SPEED_TRIM 3.0
+param set-default GND_SPEED_THR_SC 1.0
+param set-default GND_VEL_CTRL 1
+param set-default GND_ANG_VEL_CTRL 1
+param set-default GND_ACC_LIMIT 10
+param set-default GND_DEC_LIMIT 100

packaging/entrypoint_sitl_gzsim.sh

@@ -6,18 +6,19 @@ export PATH=/px4_sitl/bin:$PATH
 case $PX4_VEHICLE_TYPE in
   mc)
     export PX4_SYS_AUTOSTART=4401
+    export PX4_GZ_MODEL=holybro-x500
     ;;
   rover)
-    export PX4_SYS_AUTOSTART=50006
+    export PX4_SYS_AUTOSTART=50005
+    export PX4_GZ_MODEL=scout_mini
     ;;
   vtol)
     export PX4_SYS_AUTOSTART=4430
+    export PX4_GZ_MODEL=striver_mini
     ;;
   fw)
     export PX4_SYS_AUTOSTART=4440
-    ;;
-  uuv)
-    export PX4_SYS_AUTOSTART=4403
+    export PX4_GZ_MODEL=skywalker_x8
     ;;
   *)
     echo "ERROR: unknown vehicle type: $PX4_VEHICLE_TYPE"
@@ -33,4 +34,4 @@ export GZ_IP=$(hostname -i)
 
 source /opt/ros/humble/setup.sh
 
-/px4/bin/px4 -d -s /px4/etc/init.d-posix/rcS -w /px4
+/px4_sitl/bin/px4 -d -s /px4_sitl/etc/init.d-posix/rcS -w /px4_sitl

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -320,20 +320,38 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 	const Vector3f desired_velocity{_trajectory_setpoint.velocity};
 	float dt = 0.01; // Using non zero value to a avoid division by zero
 
+	if (_control_velocity_last_called > 0) {
+		dt = hrt_elapsed_time(&_control_velocity_last_called) * 1e-6f;
+	}
+
+	_control_velocity_last_called = hrt_absolute_time();
+
 	const float mission_throttle = _param_throttle_cruise.get();
 	const float desired_speed = desired_velocity.norm();
+	float desired_angular_vel = PX4_ISFINITE(_trajectory_setpoint.yawspeed) ?  _trajectory_setpoint.yawspeed : desired_velocity(1);
 
-	if (desired_speed > 0.01f) {
+	if (desired_speed > 0.01f || desired_angular_vel > 0.01f) {
 		const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
 		const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));
 
 		const float x_vel = vel(0);
 		const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
-
-		const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
-
-		//Constrain maximum throttle to mission throttle
-		_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
+		float control_throttle = 0.0f;
+		const float speed_error = desired_speed - x_vel;
+
+		if (_param_speed_control_mode.get() == 0) {
+			// Use PID control
+			control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
+			_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
+		} else if (_param_speed_control_mode.get() == 1) {
+			// Use acc limited direct control
+			float max_delta_speed = (speed_error > 0 ? _param_speed_acc_limit.get() : _param_speed_dec_limit.get()) * dt;
+			// Compute the velocity with delta speed and constrain it to GND_SPEED_TRIM
+			float command_velocity = math::constrain(x_vel + math::constrain(speed_error, -max_delta_speed, max_delta_speed),
+				-_param_gndspeed_trim.get(), _param_gndspeed_trim.get());
+			// Compute the desired velocity and divide it by max speed to get the throttle control
+			_throttle_control = command_velocity / _param_gndspeed_max.get();
+		}
 
 		Vector3f desired_body_velocity;
 
@@ -345,11 +363,15 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
 			desired_body_velocity = R_to_body * desired_velocity;
 		}
 
-		const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0));
-		float control_effort = desired_theta / _param_max_turn_angle.get();
-		control_effort = math::constrain(control_effort, -1.0f, 1.0f);
-
-		_yaw_control = control_effort;
+		if (_param_ang_vel_control_mode.get() == 0) {
+			// Determine yaw from XY vector
+			const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0));
+			_yaw_control = math::constrain(desired_theta / _param_max_turn_angle.get(), -1.0f, 1.0f);
+		} else if (_param_ang_vel_control_mode.get() == 1) {
+			// Use direct yaw input from velocity setpoint
+			// Limit it to max anguler velocity
+			_yaw_control = math::constrain(desired_angular_vel, -_param_max_angular_velocity.get(), _param_max_angular_velocity.get());
+		}
 
 	} else {
 

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -133,6 +133,7 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
+	hrt_abstime _control_velocity_last_called{0};	/**<last call of control_velocity  */
 	hrt_abstime _manual_setpoint_last_called{0};
 
 	MapProjection _global_local_proj_ref{};
@@ -167,8 +168,8 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 
 	float _manual_yaw_sp{0.0};
 	bool _reset_yaw_sp{true};
-	float _throttle_control{0.f};
-	float _yaw_control{0.f};
+	float _throttle_control{0.0};
+	float _yaw_control{0.0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
@@ -179,6 +180,8 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 		(ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
 
 		(ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
+		(ParamInt<px4::params::GND_VEL_CTRL>) _param_vel_control_mode,
+		(ParamInt<px4::params::GND_ANG_VEL_CTRL>) _param_ang_vel_control_mode,
 		(ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
 		(ParamFloat<px4::params::GND_SPEED_I>) _param_speed_i,
 		(ParamFloat<px4::params::GND_SPEED_D>) _param_speed_d,
@@ -190,7 +193,10 @@ class RoverPositionControl final : public ModuleBase<RoverPositionControl>, publ
 		(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
 
 		(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
+		(ParamFloat<px4::params::GND_ACC_LIMIT>) _param_speed_acc_limit,
+		(ParamFloat<px4::params::GND_DEC_LIMIT>) _param_speed_dec_limit,
 		(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
+		(ParamFloat<px4::params::GND_MAX_ANG_VEL>) _param_max_angular_velocity,
 		(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

@@ -164,6 +164,32 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
  */
 PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
 
+/**
+ * Control mode for speed
+ *
+ * This allows the user to choose between PID control or direct throttle control to work with
+ * rover_interface
+ * @min 0
+ * @max 1
+ * @value 0 PID control
+ * @value 1 Direct control
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_INT32(GND_VEL_CTRL, 0);
+
+/**
+ * Control mode for angular velocity
+ *
+ * Whether the angular velocity will be done with XY vel command difference, or the angular
+ * velocity commands are given directly either in yawspeed or _trajectory_setpoint.velocity(1)
+ * @min 0
+ * @max 1
+ * @value 0 Difference in XY vel commands
+ * @value 1 Raw input from controller
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_INT32(GND_ANG_VEL_CTRL, 0);
+
 /**
  * Speed proportional gain
  *
@@ -260,6 +286,42 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
 
+/**
+ * Velocity control acceleration limit
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 100.0
+ * @decimal 2
+ * @increment 0.05
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_ACC_LIMIT, 1.0f);
+
+/**
+ * Velocity control deceleration limit
+ *
+ * @unit m/s^2
+ * @min 0.005
+ * @max 100.0
+ * @decimal 2
+ * @increment 0.05
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_DEC_LIMIT, 1.0f);
+
+/**
+ * Limit the given angular velocity
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 3.14159
+ * @decimal 3
+ * @increment 0.01
+ * @group Rover Position Control
+ */
+PARAM_DEFINE_FLOAT(GND_MAX_ANG_VEL, 0.4f);
+
 /**
  * Maximum turn angle for Ackerman steering.
  *