SIH Simulator: Add wind (#26467)

* SIH: explicitly use local velocity for all aerodynamic calculations

no functional change

* SIH: add param & vars for wind and apparent vel

no functional change

* SIH: replace all relevant vels with apparent vel

Only places where _v_E is remaining:
 - ecefToNed to calculate _v_N
 - generate_rover_ackermann_dynamics
 - equations_of_motion
 - parameters_updated, init_variables
and _v_N:
 - ecefToNed
 - print_status
 - publish_ground_truth
 - generate_rover_ackermann_dynamics
 - equations_of_motion
 - parameters_updated, init_variables

which are all not relevant for aerodynamics.

* sih: wind review suggestions

* sih wind: switch direction to global wind source direction convention

* SIH: clean up variable declarations

* SIH: rename variables for consistency

* docs: SIH: document new wind parameters

* Release notes: note for SIH wind settings

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>

docs/en/releases/main.md

@@ -59,7 +59,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 ### Simulation
 
-- TBD
+- SIH: Add option to set wind velocity ([PX4-Autopilot#26467](https://github.com/PX4-Autopilot/pull/26467))
 
 <!-- MOVED THIS TO v1.17
 

docs/en/sim_sih/index.md

@@ -149,6 +149,12 @@ The airplane needs to takeoff in manual mode at full throttle.
 Also, if the airplane crashes the state estimator might lose its fix.
 :::
 
+## Simulation Configuration
+
+### Wind
+
+SIH supports setting a wind velocity with the PX4 parameters [`SIH_WIND_N`](../advanced_config/parameter_reference.md#SIH_WIND_E) and [`SIH_WIND_E`](../advanced_config/parameter_reference.md#SIH_WIND_E) [m/s]. The parameters can also be changed during flight to simulate changing wind.
+
 ## Display/Visualisation (optional)
 
 The SIH-simulated vehicle can be displayed using [jMAVSim](../sim_jmavsim/index.md) as a visualiser.

src/modules/simulation/simulator_sih/sih.cpp

@@ -282,6 +282,8 @@ void Sih::parameters_updated()
 	_distance_snsr_override = _sih_distance_snsr_override.get();
 
 	_T_TAU = _sih_thrust_tau.get();
+
+	_v_wind_N = Vector3f(_sih_wind_n.get(), _sih_wind_e.get(), 0.f);
 }
 
 void Sih::read_motors(const float dt)
@@ -310,8 +312,9 @@ void Sih::generate_force_and_torques(const float dt)
 		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
 				 _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
 				 _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
-		_Fa_E = -_KDV * _v_E;   // first order drag to slow down the aircraft
-		_Ma_B = -_KDW * _w_B;   // first order angular damper
+
+		_Fa_E = -_KDV * _R_N2E * _v_apparent_N; // first order drag to slow down the aircraft
+		_Ma_B = -_KDW * _w_B; // first order angular damper
 
 	} else if (_vehicle == VehicleType::Hexacopter) {
 		/*     m5    m0      ┬
@@ -325,7 +328,8 @@ void Sih::generate_force_and_torques(const float dt)
 		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-.5f * _u[0] - _u[1] - .5f * _u[2] + .5f * _u[3] + _u[4] + .5f * _u[5]),
 				 _L_PITCH * _T_MAX * (M_SQRT3_F / 2.f) * (+_u[0] - _u[2] - _u[3] + _u[5]),
 				 _Q_MAX * (+_u[0] - _u[1] + _u[2] - _u[3] + _u[4] - _u[5]));
-		_Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft
+
+		_Fa_E = -_KDV * _R_N2E * _v_apparent_N; // first order drag to slow down the aircraft
 		_Ma_B = -_KDW * _w_B; // first order angular damper
 
 	} else if (_vehicle == VehicleType::FixedWing) {
@@ -339,8 +343,8 @@ void Sih::generate_force_and_torques(const float dt)
 		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0]));
 		generate_ts_aerodynamics();
 
-		// _Fa_E = -_KDV * _v_E;   // first order drag to slow down the aircraft
-		// _Ma_B = -_KDW * _w_B;   // first order angular damper
+		// _Fa_E = -_KDV * _R_N2E * _v_apparent_N; // first order drag to slow down the aircraft
+		// _Ma_B = -_KDW * _w_B; // first order angular damper
 
 	} else if (_vehicle == VehicleType::StandardVTOL) {
 
@@ -361,7 +365,7 @@ void Sih::generate_force_and_torques(const float dt)
 void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd,
 				   const float throttle_cmd)
 {
-	const Vector3f v_B = _q_E.rotateVectorInverse(_v_E);
+	const Vector3f v_B = _q.rotateVectorInverse(_v_apparent_N);
 	const float &alt = _lla.altitude();
 
 	_wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX);
@@ -383,7 +387,7 @@ void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd,
 void Sih::generate_ts_aerodynamics()
 {
 	// velocity in body frame [m/s]
-	const Vector3f v_B = _q_E.rotateVectorInverse(_v_E);
+	const Vector3f v_B = _q.rotateVectorInverse(_v_apparent_N);
 
 	// the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly)
 	Vector3f v_ts = _R_S2B.transpose() * v_B;
@@ -572,6 +576,8 @@ void Sih::ecefToNed()
 
 	// Transform velocity to NED frame
 	_v_N = C_SE * _v_E;
+	_v_apparent_N = _v_N + _v_wind_N;
+
 	_q = Quatf(C_SE) * _q_E;
 	_q.normalize();
 }
@@ -630,8 +636,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
 
-	// regardless of vehicle type, body frame, etc this holds as long as wind=0
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f);
+	// Assume the pitot tube always points against the wind to not have tailsitter edge cases
+	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_apparent_N.norm() + generate_wgn() * 0.2f);
 	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
 	airspeed.confidence = 0.7f;
 	airspeed.timestamp = hrt_absolute_time();

src/modules/simulation/simulator_sih/sih.hpp

@@ -200,27 +200,34 @@ private:
 	hrt_abstime _airspeed_time{0};
 	hrt_abstime _dist_snsr_time{0};
 
-	bool        _grounded{true};// whether the vehicle is on the ground
+	bool _grounded{true}; // whether the vehicle is on the ground
 
-	matrix::Vector3f    _T_B{};           // thrust force in body frame [N]
-	matrix::Vector3f    _Mt_B{};          // thruster moments in the body frame [Nm]
-	matrix::Vector3f    _Ma_B{};          // aerodynamic moments in the body frame [Nm]
-	matrix::Vector3f    _lpos{};          // position in a local tangent-plane frame [m]
-	matrix::Vector3f    _v_N{};           // velocity in local navigation frame (NED, body-fixed) [m/s]
-	matrix::Vector3f    _v_N_dot{};       // time derivative of velocity in local navigation frame [m/s2]
-	matrix::Quatf       _q{};             // quaternion attitude in local navigation frame
-	matrix::Vector3f    _w_B{};           // body rates in body frame [rad/s]
+	// Quantities in body frame (FRD)
+	matrix::Vector3f _T_B{};  // thrust force [N]
+	matrix::Vector3f _Mt_B{}; // thruster moments [Nm]
+	matrix::Vector3f _Ma_B{}; // aerodynamic moments [Nm]
+	matrix::Vector3f _w_B{};  // body rates in body frame [rad/s]
 
-	LatLonAlt _lla{};
+	// Quantities in local navigation frame (NED, body-fixed)
+	matrix::Vector3f _v_N{};          // velocity [m/s]
+	matrix::Vector3f _v_N_dot{};      // time derivative of velocity [m/s^2]
+	matrix::Vector3f _v_wind_N{};     // wind velocity [m/s]
+	matrix::Vector3f _v_apparent_N{}; // vehicle velocity relative to the air [m/s]
 
 	// Quantities in Earth-centered-Earth-fixed coordinates
-	matrix::Vector3f    _Fa_E{};          // aerodynamic force in ECEF frame [N]
-	matrix::Vector3f    _specific_force_E{};
-	matrix::Quatf       _q_E{matrix::Eulerf(0.f, -M_PI_2_F, 0.f)};
-	matrix::Vector3d    _p_E{Wgs84::equatorial_radius, 0.0, 0.0};
-	matrix::Vector3f    _v_E{};
-	matrix::Vector3f    _v_E_dot{};
-	matrix::Dcmf        _R_N2E;           // local navigation to ECEF frame rotation matrix
+	matrix::Vector3f _Fa_E{};             // aerodynamic force [N]
+	matrix::Vector3d _p_E{};              // position [m]
+	matrix::Vector3f _v_E{};              // velocity [m/s]
+	matrix::Vector3f _v_E_dot{};          // time derivative of velocity [m/s^2]
+	matrix::Vector3f _specific_force_E{}; // acceleration except gravity (for IMU) [m/s^2]
+
+	// Frame conversion
+	matrix::Quatf _q_E{}; // Attitude quaternion (rotation from body to ECEF frame)
+	matrix::Quatf _q{};   // Attitude quaternion (rotation from body to local navigation frame)
+	matrix::Dcmf _R_N2E;  // Rotation matrix from local navigation to ECEF frame
+
+	LatLonAlt _lla{};
+	matrix::Vector3f _lpos{};  // position in a local tangent-plane frame [m]
 
 	float _u[NUM_ACTUATORS_MAX] {}; // thruster signals
 
@@ -292,6 +299,8 @@ private:
 		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
 		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
 		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
-		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
+		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
+		(ParamFloat<px4::params::SIH_WIND_N>) _sih_wind_n,
+		(ParamFloat<px4::params::SIH_WIND_E>) _sih_wind_e
 	)
 };

src/modules/simulation/simulator_sih/sih_params.c

@@ -339,3 +339,19 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);
+
+/**
+ * Wind velocity from north direction.
+ *
+ * @unit m/s
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_WIND_N, 0.0f);
+
+/**
+ * Wind velocity from east direction.
+ *
+ * @unit m/s
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_WIND_E, 0.0f);