feat(simulation): add ranging beacon simulation in SIH

Author: haumarco

src/modules/simulation/simulator_sih/sih.cpp

@@ -78,6 +78,7 @@ void Sih::run()
 	_last_run = task_start;
 	_airspeed_time = task_start;
 	_dist_snsr_time = task_start;
+	_ranging_beacon_time = task_start;
 	_vehicle = static_cast<VehicleType>(constrain(_sih_vtype.get(),
 					    static_cast<int32_t>(VehicleType::First),
 					    static_cast<int32_t>(VehicleType::Last)));
@@ -232,6 +233,12 @@ void Sih::sensor_step()
 		send_dist_snsr(now);
 	}
 
+	// ranging beacon published at 2 Hz (each beacon at 0.5 Hz)
+	if (now - _ranging_beacon_time >= 500_ms) {
+		_ranging_beacon_time = now;
+		send_ranging_beacon(now);
+	}
+
 	publish_ground_truth(now);
 
 	perf_end(_loop_perf);
@@ -259,7 +266,7 @@ void Sih::parameters_updated()
 		_lla.setAltitude(_lpos_ref_alt);
 		_p_E = _lla.toEcef();
 
-		const Dcmf R_E2N = computeRotEcefToNed(_lla);
+		const Dcmf R_E2N = _lla.computeRotEcefToNed();
 		_R_N2E = R_E2N.transpose();
 		_v_E = _R_N2E * _v_N;
 
@@ -571,7 +578,7 @@ void Sih::ecefToNed()
 {
 	_lla = LatLonAlt::fromEcef(_p_E);
 
-	const Dcmf C_SE = computeRotEcefToNed(_lla);
+	const Dcmf C_SE = _lla.computeRotEcefToNed();
 	_R_N2E = C_SE.transpose();
 
 	// Transform velocity to NED frame
@@ -582,22 +589,6 @@ void Sih::ecefToNed()
 	_q.normalize();
 }
 
-Dcmf Sih::computeRotEcefToNed(const LatLonAlt &lla)
-{
-	// Calculate the ECEF to NED coordinate transformation matrix
-	const double cos_lat = cos(lla.latitude_rad());
-	const double sin_lat = sin(lla.latitude_rad());
-	const double cos_lon = cos(lla.longitude_rad());
-	const double sin_lon = sin(lla.longitude_rad());
-
-	const float val[] = {(float)(-sin_lat * cos_lon), (float)(-sin_lat * sin_lon), (float)cos_lat,
-			     (float) - sin_lon, (float)cos_lon, 0.f,
-			     (float)(-cos_lat * cos_lon), (float)(-cos_lat * sin_lon), (float) - sin_lat
-			    };
-
-	return Dcmf(val);
-}
-
 void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us)
 {
 	// The sensor signals reconstruction and noise levels are from [1]
@@ -678,6 +669,55 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
 	_distance_snsr_pub.publish(distance_sensor);
 }
 
+void Sih::send_ranging_beacon(const hrt_abstime &time_now_us)
+{
+	if (_lpos_ref.isInitialized()) {
+
+		if (!_beacons_configured) {
+			_beacons_configured = true;
+
+			for (uint8_t i = 0; i < NUM_RANGING_BEACONS; i++) {
+				_lpos_ref.reproject(RANGING_BEACON_OFFSETS[i].north_m, RANGING_BEACON_OFFSETS[i].east_m,
+						    _ranging_beacons[i].lat_deg, _ranging_beacons[i].lon_deg);
+				_ranging_beacons[i].alt_m = _sih_h0.get() + RANGING_BEACON_OFFSETS[i].alt_offset_m;
+			}
+		}
+
+		const RangingBeaconConfig &beacon = _ranging_beacons[_ranging_beacon_idx];
+		const LatLonAlt beacon_lla(beacon.lat_deg, beacon.lon_deg, beacon.alt_m);
+		const matrix::Vector3d beacon_ecef = beacon_lla.toEcef();
+
+		// Compute true range in ECEF
+		const matrix::Vector3d delta_ecef = beacon_ecef - _p_E;
+		const double true_range_m = delta_ecef.norm();
+
+		const float noise_std = _sih_ranging_beacon_noise.get();
+		const float noise_m = (noise_std > 0.f) ? generate_wgn() * noise_std : 0.f;
+		const double measured_range_m = math::max(0.0, true_range_m + static_cast<double>(noise_m));
+
+		ranging_beacon_s msg{};
+		msg.timestamp = hrt_absolute_time();
+		msg.timestamp_sample = time_now_us;
+		msg.beacon_id = _ranging_beacon_idx;
+		msg.range = static_cast<float>(measured_range_m);
+		msg.lat = beacon.lat_deg;
+		msg.lon = beacon.lon_deg;
+		msg.alt = beacon.alt_m;
+		msg.alt_type = 0; // WGS84
+		msg.hacc = 1.0f;
+		msg.vacc = 1.0f;
+		msg.range_accuracy = noise_std;
+		msg.sequence_nr = 0;
+		msg.status = 0;
+		msg.carrier_freq = 0;
+
+		_ranging_beacon_pub.publish(msg);
+
+		// cycle through the beacons
+		_ranging_beacon_idx = (_ranging_beacon_idx + 1) % NUM_RANGING_BEACONS;
+	}
+}
+
 void Sih::publish_ground_truth(const hrt_abstime &time_now_us)
 {
 	{

src/modules/simulation/simulator_sih/sih.hpp

@@ -79,6 +79,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/ranging_beacon.h>
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <sys/time.h>
@@ -128,6 +129,7 @@ class Sih : public ModuleBase, public ModuleParams
 	PX4Gyroscope     _px4_gyro{1310988};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 	uORB::Publication<distance_sensor_s>  _distance_snsr_pub{ORB_ID(distance_sensor)};
 	uORB::Publication<airspeed_s>         _airspeed_pub{ORB_ID(airspeed)};
+	uORB::Publication<ranging_beacon_s>   _ranging_beacon_pub{ORB_ID(ranging_beacon)};
 
 	// groundtruth
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
@@ -140,6 +142,29 @@ class Sih : public ModuleBase, public ModuleParams
 
 	// hard constants
 	static constexpr uint16_t NUM_ACTUATORS_MAX = 9;
+
+	// Ranging beacon simulation constants
+	static constexpr uint8_t NUM_RANGING_BEACONS = 4;
+	bool _beacons_configured{false};
+	struct RangingBeaconConfig {
+		double lat_deg;
+		double lon_deg;
+		float alt_m;
+	};
+	struct RangingBeaconOffset {
+		float north_m;
+		float east_m;
+		float alt_offset_m;
+	};
+	// NED offsets from SIH_LOC_LAT0/LON0/H0, resolved once in init_variables()
+	static constexpr RangingBeaconOffset RANGING_BEACON_OFFSETS[NUM_RANGING_BEACONS] = {
+		{  5000.f,     0.f,  30.f},  // ~5 km North
+		{     0.f, 10000.f,   0.f},  // ~10 km East
+		{-20000.f, -15000.f, 110.f}, // ~20 km South-West
+		{ 35000.f,  45000.f, 310.f}  // ~50 km North-East
+	};
+	RangingBeaconConfig _ranging_beacons[NUM_RANGING_BEACONS] {};
+
 	static constexpr float T1_C = 15.0f;                        // ground temperature in Celsius
 	static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius;   // ground temperature in Kelvin
 	static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
@@ -163,6 +188,7 @@ class Sih : public ModuleBase, public ModuleParams
 	void reconstruct_sensors_signals(const hrt_abstime &time_now_us);
 	void send_airspeed(const hrt_abstime &time_now_us);
 	void send_dist_snsr(const hrt_abstime &time_now_us);
+	void send_ranging_beacon(const hrt_abstime &time_now_us);
 	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();
@@ -171,7 +197,6 @@ class Sih : public ModuleBase, public ModuleParams
 	static float computeGravity(double lat);
 
 	void ecefToNed();
-	static matrix::Dcmf computeRotEcefToNed(const LatLonAlt &lla);
 
 	struct Wgs84 {
 		static constexpr double equatorial_radius = 6378137.0;
@@ -199,6 +224,8 @@ class Sih : public ModuleBase, public ModuleParams
 	hrt_abstime _last_actuator_output_time{0};
 	hrt_abstime _airspeed_time{0};
 	hrt_abstime _dist_snsr_time{0};
+	hrt_abstime _ranging_beacon_time{0};
+	uint8_t _ranging_beacon_idx{0};
 
 	bool _grounded{true}; // whether the vehicle is on the ground
 
@@ -301,6 +328,7 @@ class Sih : public ModuleBase, public ModuleParams
 		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
 		(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
+		(ParamFloat<px4::params::SIH_WIND_E>) _sih_wind_e,
+		(ParamFloat<px4::params::SIH_RNGBC_NOISE>) _sih_ranging_beacon_noise
 	)
 };

src/modules/simulation/simulator_sih/sih_params.yaml

@@ -271,3 +271,13 @@ parameters:
       type: float
       default: 0.0
       unit: m/s
+    SIH_RNGBC_NOISE:
+      description:
+        short: Ranging beacon measurement noise standard deviation
+        long: Gaussian noise added to simulated ranging beacon measurements. Set to 0 to disable noise.
+      type: float
+      default: 30.0
+      unit: m
+      min: 0.0
+      max: 100.0
+      decimal: 1