ackermann: add SIH (#25194)

ROMFS/px4fmu_common/init.d-posix/airframes/10045_sihsim_rover_ackermann

@@ -0,0 +1,72 @@
+#!/bin/sh
+# @name Rover Ackermann
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults
+
+set VEHICLE_TYPE rover_ackermann
+param set-default CA_AIRFRAME 5         # Rover (Ackermann)
+param set-default CA_R_REV 1            # Motor is assumed to be reversible
+param set-default EKF2_MAG_TYPE 1       # Make sure magnetometer is fused even when not driving
+param set-default NAV_ACC_RAD 0.5       # Waypoint acceptance radius
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann}
+
+param set-default SIH_VEHICLE_TYPE 5 # sih as rover ackermann
+
+param set-default PWM_MAIN_FUNC1 201 # Steering
+param set-default PWM_MAIN_FUNC2 101 # Throttle
+param set-default SIH_MASS 20
+param set-default SIH_IXX 0.4333
+param set-default SIH_IYY 1.6833
+param set-default SIH_IZZ 2.0833
+param set-default SIH_IXZ 0
+param set-default SIH_KDV 50
+param set-default SIH_KDW 10
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
+# Ackermann Parameters
+param set-default RA_WHEEL_BASE 0.321
+param set-default RA_ACC_RAD_GAIN 2
+param set-default RA_ACC_RAD_MAX 3
+param set-default RA_MAX_STR_ANG 0.5236
+param set-default RA_STR_RATE_LIM 360
+
+# Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.01
+param set-default RO_YAW_RATE_P 0.25
+param set-default RO_YAW_RATE_LIM 180
+param set-default RO_YAW_ACCEL_LIM 400
+param set-default RO_YAW_DECEL_LIM 800
+param set-default RO_YAW_RATE_CORR 1
+
+# Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Velocity Control Parameters
+param set-default RO_ACCEL_LIM 3
+param set-default RO_DECEL_LIM 3
+param set-default RO_JERK_LIM 10
+param set-default RO_MAX_THR_SPEED 3.2
+param set-default RO_SPEED_LIM 3
+param set-default RO_SPEED_I 0.001
+param set-default RO_SPEED_P 0.001
+param set-default RO_SPEED_RED 1
+
+# Pure Pursuit parameters
+param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+
+# Pure Pursuit parameters
+param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -108,6 +108,7 @@ px4_add_romfs_files(
 	10042_sihsim_xvert
 	10043_sihsim_standard_vtol
 	10044_sihsim_hex
+	10045_sihsim_rover_ackermann
 
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

src/modules/simulation/simulator_sih/CMakeLists.txt

@@ -55,6 +55,7 @@ if(PX4_PLATFORM MATCHES "posix")
 		xvert
 		standard_vtol
 		hex
+		rover_ackermann
 	)
 
 	# find corresponding airframes

src/modules/simulation/simulator_sih/sih.cpp

@@ -211,7 +211,7 @@ void Sih::sensor_step()
 
 	read_motors(dt);
 
-	generate_force_and_torques();
+	generate_force_and_torques(dt);
 
 	equations_of_motion(dt);
 
@@ -290,6 +290,7 @@ void Sih::init_variables()
 
 	_lpos = Vector3f(0.0f, 0.0f, 0.0f);
 	_v_N = Vector3f(0.0f, 0.0f, 0.0f);
+	_v_N_dot = Vector3f(0.0f, 0.0f, 0.0f);
 	_p_E = Vector3d(Wgs84::equatorial_radius, 0.0, 0.0);
 	_v_E = Vector3f(0.0f, 0.0f, 0.0f);
 	_q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
@@ -318,7 +319,7 @@ void Sih::read_motors(const float dt)
 	}
 }
 
-void Sih::generate_force_and_torques()
+void Sih::generate_force_and_torques(const float dt)
 {
 	if (_vehicle == VehicleType::Quadcopter) {
 		_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
@@ -367,6 +368,9 @@ void Sih::generate_force_and_torques()
 		// thrust 0 because it is already contained in _T_B. in
 		// equations_of_motion they are all summed into sum_of_forces_E
 		generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0);
+
+	} else if (_vehicle == VehicleType::RoverAckermann) {
+		generate_rover_ackermann_dynamics(_u[1], _u[0], dt);
 	}
 }
 
@@ -422,6 +426,73 @@ void Sih::generate_ts_aerodynamics()
 	_Ma_B = _R_S2B * Ma_ts - _KDW * _w_B; 	// aerodynamic moments
 }
 
+void Sih::generate_rover_ackermann_dynamics(const float throttle_cmd, const float steering_cmd, const float dt)
+{
+	// --- Constants ---
+	static constexpr float MAX_THROTTLE_FORCE = 400.0f;     // [N]
+	static constexpr float MAX_STEER_ANGLE = radians(30.f); // [rad]
+	static constexpr float WHEEL_BASE = 0.321f;             // [m] Distance between front and rear axle
+	static constexpr float C = 500.f;                       // [N/rad] Cornering stiffness
+	static constexpr float MU_S = 0.5f;                     // [-] Static Coefficient of friction
+	static constexpr float MU_K = 0.4f;                     // [-] Kinetic Coefficient of friction
+	static constexpr float MU_R = 0.3f;                     // [-] Rolling Coefficient of friction
+	static constexpr float ROLLING_THRESHOLD = 0.05f;       // [m/s] Threshold for rolling resistance
+	static constexpr float STATIC_THRESHOLD = 0.01f;        // [m/s] Threshold for static resistance
+
+	matrix::Vector3f v_B = _q.rotateVectorInverse(_v_N); // Nav -> Body
+
+	// --- Compute inputs ---
+	const float delta = MAX_STEER_ANGLE * steering_cmd; // [rad] Steering angle
+	const float F_x = MAX_THROTTLE_FORCE * throttle_cmd;         // [N] Throttle force
+
+	// --- Compute forces and moments ---
+	float F_y = 0.f; // [N] Lateral force
+	float M_z = 0.f; // [Nm] Yaw moment
+
+	if (fabsf(v_B(0)) > ROLLING_THRESHOLD) {
+		// Equations based on the lateral dynamics of the bicycle model from [1]
+		// [1] Sri Anumakonda, Everything you need to know about Self-Driving Cars in <30 minutes
+		// Link: https://srianumakonda.medium.com/everything-you-need-to-know-about-self-driving-in-30-minutes-b38d68bd3427
+		const float a_y = C * delta / _MASS - fabsf(v_B(0)) * _w_B(2)
+				  - 2 * C * v_B(1) / (_MASS * fabsf(v_B(0))); // [m/s^2] Lateral acceleration
+		const float psi_dot_dot = WHEEL_BASE * C * delta / _sih_izz.get()
+					  - C * WHEEL_BASE * WHEEL_BASE  * _w_B(2) / (_sih_izz.get() * fabsf(v_B(0))); // [rad/s^2] Yaw acceleration
+		F_y = _MASS * a_y; // Lateral force [N]
+		M_z = _sih_izz.get() * psi_dot_dot; // [Nm] Yaw moment
+	}
+
+	_T_B = Vector3f(F_x, F_y, 0.f);
+	_Mt_B = Vector3f(0.f, 0.f, M_z);
+
+	// --- Compute drag/friction forces and moments ---
+	Vector3f F_f = Vector3f(0.f, 0.f, 0.f); // [N] Friction force
+	Vector3f F_a = Vector3f(0.f, 0.f, 0.f); // [N] Aerodynamic force (neglect until rover is rolling)
+
+	if (_v_E.norm() < STATIC_THRESHOLD) { // Static friction
+		Vector3f F_f_B = Vector3f(sign(F_x) * math::min(fabsf(F_x), MU_S * _MASS * 9.81f), sign(F_y) * math::min(fabsf(F_y),
+					  MU_S * _MASS * 9.81f), 0.f);
+		F_f = _q_E.rotateVector(F_f_B);
+
+	} else if (_v_E.norm() < ROLLING_THRESHOLD) { // Kinetic friction
+		if (_T_B.norm() > FLT_EPSILON) {
+			F_f = _v_E.unit_or_zero() * MU_K * _MASS * 9.81f;
+
+		} else {
+			F_f = _v_E * _MASS / dt; // Stop the vehicle
+		}
+
+	} else { // Rolling friction
+		F_f = _v_E.unit_or_zero() * MU_R * _MASS * 9.81f;
+		Vector3f v_E_squared = Vector3f(sign(_v_E(0)) * _v_E(0) * _v_E(0), sign(_v_E(1)) * _v_E(1) * _v_E(1),
+						sign(_v_E(2)) * _v_E(2) * _v_E(2));
+		F_a = _KDV * v_E_squared; // [N] Second order drag
+	}
+
+	_Fa_E = -F_a - F_f;   // [N] Second order drag and friction
+	_Ma_B = -_KDW * _w_B; // [Nm] First order angular damper
+
+}
+
 float Sih::computeGravity(const double lat)
 {
 	// Somigliana formula for gravitational acceleration
@@ -459,7 +530,8 @@ void Sih::equations_of_motion(const float dt)
 
 			_grounded = true;
 
-		} else if (_vehicle == VehicleType::FixedWing) {
+		} else if (_vehicle == VehicleType::FixedWing
+			   || _vehicle == VehicleType::RoverAckermann) {
 			Vector3f down_u = _R_N2E.col(2);
 			ground_force_E = -down_u * sum_of_forces_E * down_u;
 
@@ -749,6 +821,9 @@ int Sih::print_status()
 
 	} else if (_vehicle == VehicleType::StandardVTOL) {
 		PX4_INFO("Standard VTOL");
+
+	} else if (_vehicle == VehicleType::RoverAckermann) {
+		PX4_INFO("Rover Ackermann");
 	}
 
 	PX4_INFO("vehicle landed: %d", _grounded);

src/modules/simulation/simulator_sih/sih.hpp

@@ -151,7 +151,7 @@ private:
 	void read_motors(const float dt);
 
 	// generate the motors thrust and torque in the body frame
-	void generate_force_and_torques();
+	void generate_force_and_torques(const float dt);
 
 	// apply the equations of motion of a rigid body and integrate one step
 	void equations_of_motion(const float dt);
@@ -163,6 +163,7 @@ private:
 	void publish_ground_truth(const hrt_abstime &time_now_us);
 	void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust);
 	void generate_ts_aerodynamics();
+	void generate_rover_ackermann_dynamics(const float throttle_cmd, const float steering_cmd, const float dt);
 	void sensor_step();
 	static float computeGravity(double lat);
 
@@ -220,7 +221,7 @@ private:
 
 	float _u[NUM_ACTUATORS_MAX] {}; // thruster signals
 
-	enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, First = Quadcopter, Last = Hexacopter}; // numbering dependent on parameter SIH_VEHICLE_TYPE
+	enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, RoverAckermann, First = Quadcopter, Last = RoverAckermann}; // numbering dependent on parameter SIH_VEHICLE_TYPE
 	VehicleType _vehicle = VehicleType::Quadcopter;
 
 	// aerodynamic segments for the fixedwing

src/modules/simulation/simulator_sih/sih_params.c

@@ -334,6 +334,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
  * @value 2 Tailsitter
  * @value 3 Standard VTOL
  * @value 4 Hexacopter
+ * @value 5 Rover Ackermann
  * @reboot_required true
  * @group Simulation In Hardware
  */