Add boat SIH

ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_boat

@@ -0,0 +1,81 @@
+#!/bin/sh
+# @name Boat (Ackermann style)
+# @type Boat
+# @class Boat
+
+. ${R}etc/init.d/rc.boat_defaults
+
+set VEHICLE_TYPE boat
+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 flying
+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:=boat}
+
+param set-default SIH_VEHICLE_TYPE 4 	# sih as boat
+
+#param set-default SIH_MASS 3.0
+#param set-default SIH_IXX 0.00402
+#param set-default SIH_IYY 0.0144
+#param set-default SIH_IZZ 0.0177
+#param set-default SIH_IXZ 0.00046
+#param set-default SIH_KDV 0.2
+
+param set-default PWM_MAIN_FUNC1 201 # Steering
+param set-default PWM_MAIN_FUNC2 101 # Throttle
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+#param set-default SENS_EN_ARSPDSIM 1
+
+param set-default NAV_ACC_RAD 0.5
+
+# 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
+
+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 3
+param set-default RO_DECEL_LIM 6
+param set-default RO_JERK_LIM 15
+param set-default RO_MAX_THR_SPEED 3.1
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.1
+param set-default RO_YAW_RATE_P 1
+param set-default RO_YAW_RATE_LIM 180
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 3
+
+# Rover Position Control Parameters
+param set-default RO_SPEED_LIM 3
+param set-default RO_SPEED_I 0.1
+param set-default RO_SPEED_P 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
+
+# Wheels
+param set-default SIM_GZ_WH_FUNC1 101
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+# Steering
+param set-default SIM_GZ_SV_MAXA1 30
+param set-default SIM_GZ_SV_MINA1 -30
+param set-default CA_SV_CS_COUNT 1
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_REV 1

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

@@ -109,6 +109,7 @@ px4_add_romfs_files(
 	10041_sihsim_airplane
 	10042_sihsim_xvert
 	10043_sihsim_standard_vtol
+	10044_sihsim_boat
 
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -73,8 +73,13 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		rc.rover_apps
 		rc.rover_defaults
+	)
+endif()
 
-		rc.boat_defaults # hack
+if(CONFIG_SIH_SIM_BOAT)
+	px4_add_romfs_files(
+		rc.boat_apps
+		rc.boat_defaults
 	)
 endif()
 

ROMFS/px4fmu_common/init.d/rc.boat_apps

@@ -0,0 +1,8 @@
+#!/bin/sh
+# Standard apps for an boat.
+
+# Start rover ackermann module.
+rover_ackermann start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -49,6 +49,14 @@ then
 	# Start ackermann drive rover apps.
 	. ${R}etc/init.d/rc.rover_ackermann_apps
 fi
+#
+# Boat setup.
+#
+if [ $VEHICLE_TYPE = boat ]
+then
+	# Start boat apps.
+	. ${R}etc/init.d/rc.boat_apps
+fi
 
 #
 # Mecanum Rover setup.

boards/px4/sitl/default.px4board

@@ -49,6 +49,8 @@ CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_SIH_SIM_BOAT=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y
 CONFIG_MODULES_SIMULATION_GZ_MSGS=y

docs/en/sim_sih/index.md

@@ -9,7 +9,7 @@ It may or may not work with current versions of PX4 (known to work in PX4 v1.14)
 See [Toolchain Installation](../dev_setup/dev_env.md) for information about the environments and tools supported by the core development team.
 :::
 
-Simulation-In-Hardware (SIH) is an alternative to [Hardware In The Loop simulation (HITL)](../simulation/hitl.md) for quadrotors, fixed-wing vehicles (airplane), and VTOL tailsitters.
+Simulation-In-Hardware (SIH) is an alternative to [Hardware In The Loop simulation (HITL)](../simulation/hitl.md) for quadrotors, fixed-wing vehicles (airplane), VTOL tailsitters and boats.
 
 SIH can be used by new PX4 users to get familiar with PX4 and the different modes and features, and of course to learn to fly a vehicle using an RC controller in simulation, which is not possible using SITL.
 
@@ -27,6 +27,7 @@ The Desktop computer is only used to display the virtual vehicle.
 - SIH for fixed-wing (airplane) and VTOL tailsitter are supported from PX4 v1.13.
 - SIH as SITL (without hardware) from PX4 v1.14.
 - SIH for Standard VTOL from PX4 v1.16.
+- SIH for boats from PX4 v1.16.
 
 ### Benefits
 
@@ -61,6 +62,7 @@ To set up/start SIH:
    - [SIH plane AERT](../airframes/airframe_reference.md#plane_simulation_sih_plane_aert)
    - [SIH Tailsitter Duo](../airframes/airframe_reference.md#vtol_simulation_sih_tailsitter_duo)
    - [SIH Standard VTOL QuadPlane](../airframes/airframe_reference.md#vtol_simulation_sih_standard_vtol_quadplane)
+   - Boat uses [Rover Ackermann](../airframes/airframe_reference.md#rover_rover_generic_rover_ackermann) as CA model
 
 The autopilot will then reboot.
 The `sih` module is started on reboot, and the vehicle should be displayed on the ground control station map.
@@ -132,12 +134,18 @@ To run SIH as SITL:
      make px4_sitl sihsim_xvert
      ```
 
-   - Standard VTOL: 
+   - Standard VTOL:
 
      ```sh
      make px4_sitl sihsim_standard_vtol
      ```
 
+   - Boat:
+
+     ```sh
+     make px4_sitl sihsim_boat
+     ```
+
 ### Change Simulation Speed
 
 SITL allows the simulation to be run faster than real time.

src/modules/simulation/simulator_sih/CMakeLists.txt

@@ -54,6 +54,7 @@ if(PX4_PLATFORM MATCHES "posix")
 		quadx
 		xvert
 		standard_vtol
+		boat
 	)
 
 	# find corresponding airframes

src/modules/simulation/simulator_sih/Kconfig

@@ -8,6 +8,15 @@ menuconfig MODULES_SIMULATION_SIMULATOR_SIH
 	---help---
 		Enable support for simulator_sih
 
+if MODULES_SIMULATION_SIMULATOR_SIH
+    config SIH_SIM_BOAT
+        bool "Enable Boat Simulation features (SIH)"
+        default n
+	depends on MODULES_SIMULATION_SIMULATOR_SIH
+        ---help---
+            Enable boat support for SIH simulator.
+endif # MODULES_SIMULATION_SIMULATOR_SIH
+
 menuconfig USER_SIH
 	bool "simulator_sih running as userspace module"
 	default y

src/modules/simulation/simulator_sih/sih.cpp

@@ -76,7 +76,7 @@ void Sih::run()
 	_airspeed_time = task_start;
 	_dist_snsr_time = task_start;
 	_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
-					  static_cast<typeof _sih_vtype.get()>(3));
+					  static_cast<typeof _sih_vtype.get()>(5));
 
 	_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
 
@@ -351,9 +351,66 @@ 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::Boat) {
+		// _T_B = Vector3f(_T_MAX * _u[1], 0.0f, 0.0f); 	// forward thruster
+		// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); 	// thruster torque
+		// _Mt_B = Vector3f();
+		// _Mt_B = Vector3f(0.f, 0.f, _Q_MAX * _u[3] * (+_u[0]));
+		generate_boat_dynamics(_u[1], _u[0]);
+
+		_Fa_E = -_KDV * _v_E;   // first order drag to slow down the vehicle
+		_Ma_B = -_KDW * _w_B;   // first order angular damper
+
 	}
 }
 
+void Sih::generate_boat_dynamics(const float throttle_cmd, const float steering_cmd)
+{
+	const float MAX_FORCE = 100.0f;                   // [N]
+	const float MAX_STEER_ANGLE = 1.2f;              // [rad] (=68.7°)
+	const float MOMENT_EFFECTIVENESS = 0.08f;         // unitless (0..1)
+	const float ENGINE_TO_COG = 1.0f;                // [m]
+
+	const float DAMPING_X = 2.0f;                    // forward drag [N·s/m]
+	const float DAMPING_Y = 8.0f;                    // lateral drag [N·s/m]
+	const float YAW_DAMPING = 1.0f;                  // yaw damping [Nm·s/rad]
+	const float LATERAL_RESISTANCE = 20.0f;          // keel-like resistance [N·s/m]
+
+	// --- Convert nav velocity to body frame ---
+	const matrix::Dcmf R_nb(_q);                     // body to nav frame
+	const matrix::Vector3f v_B = R_nb.T() * _v_N;    // nav -> body
+
+	// --- Compute input-based thrust ---
+	const float steering_angle = MAX_STEER_ANGLE * steering_cmd;
+	const float force = MAX_FORCE * throttle_cmd;
+
+	_T_B = matrix::Vector3f(
+		       std::cos(steering_angle) * force,
+		       std::sin(steering_angle) * force,
+		       0.0f
+	       );
+
+	// --- Compute yaw moment from steering (scaled by speed) ---
+	const float speed = v_B.norm();
+	const float steer_eff = math::constrain(speed / 10.0f, 0.0f, 1.0f);  // more effective at higher speeds
+
+	_Mt_B = matrix::Vector3f(
+			0.0f,
+			0.0f,
+			std::sin(steering_angle) * ENGINE_TO_COG * force * MOMENT_EFFECTIVENESS * steer_eff
+		);
+
+	// --- Apply drag damping ---
+	_T_B(0) -= DAMPING_X * v_B(0);           // forward resistance
+	_T_B(1) -= DAMPING_Y * v_B(1);           // lateral resistance
+	_T_B(1) -= LATERAL_RESISTANCE * v_B(1);  // extra "keel" effect
+
+	// --- Angular damping (if you have _w_B) ---
+	_Mt_B(2) -= YAW_DAMPING * _w_B(2);   // oppose yaw rotation
+}
+
+
 void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd,
 				   const float throttle_cmd)
 {
@@ -452,6 +509,18 @@ void Sih::equations_of_motion(const float dt)
 				ground_force_E += down_u * (-_v_N(2) / dt) * _MASS;
 			}
 
+			_grounded = true;
+
+		} else if (_vehicle == VehicleType::Boat) {
+			Vector3f down_u = _R_N2E.col(2);
+			ground_force_E = -down_u * sum_of_forces_E * down_u;
+
+			if (!_grounded) {
+				// if we just hit the floor
+				// compute the force that will stop the vehicle in one time step
+				ground_force_E += down_u * (-_v_N(2) / dt) * _MASS;
+			}
+
 			_grounded = true;
 		}
 
@@ -729,6 +798,10 @@ int Sih::print_status()
 
 	} else if (_vehicle == VehicleType::StandardVTOL) {
 		PX4_INFO("Running Standard VTOL");
+
+	} else if (_vehicle == VehicleType::Boat) {
+		PX4_INFO("Running Boat");
+
 	}
 
 	PX4_INFO("vehicle landed: %d", _grounded);

src/modules/simulation/simulator_sih/sih.hpp

@@ -161,6 +161,7 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	void send_airspeed(const hrt_abstime &time_now_us);
 	void send_dist_snsr(const hrt_abstime &time_now_us);
 	void publish_ground_truth(const hrt_abstime &time_now_us);
+	void generate_boat_dynamics(const float roll_cmd, const float throttle_cmd);
 	void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust);
 	void generate_ts_aerodynamics();
 	void sensor_step();
@@ -220,7 +221,7 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 
 	float       _u[NUM_ACTUATORS_MAX] {};         // thruster signals
 
-	enum class VehicleType {Multicopter, FixedWing, TailsitterVTOL, StandardVTOL};
+	enum class VehicleType {Multicopter, FixedWing, TailsitterVTOL, StandardVTOL, Boat};
 
 	VehicleType _vehicle = VehicleType::Multicopter;
 

src/modules/simulation/simulator_sih/sih_params.c

@@ -333,6 +333,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
  * @value 1 Fixed-Wing
  * @value 2 Tailsitter
  * @value 3 Standard VTOL
+ * @value 4 Boat
  * @reboot_required true
  * @group Simulation In Hardware
  */