From 65c96fb2bf566db9306ac688090bd9c580d10c77 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 31 Mar 2026 20:43:22 +0200 Subject: [PATCH] feat(simulation): add ranging beacon simulation in SIH --- src/modules/simulation/simulator_sih/sih.cpp | 76 ++++++++++++++----- src/modules/simulation/simulator_sih/sih.hpp | 32 +++++++- .../simulation/simulator_sih/sih_params.yaml | 10 +++ 3 files changed, 98 insertions(+), 20 deletions(-) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index f1e826ffaf..466cb24ae9 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/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(constrain(_sih_vtype.get(), static_cast(VehicleType::First), static_cast(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(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(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) { { diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 84efce4c35..a3acdcfa0d 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -79,6 +79,7 @@ #include #include #include +#include #if defined(ENABLE_LOCKSTEP_SCHEDULER) #include @@ -128,6 +129,7 @@ private: PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION uORB::Publication _distance_snsr_pub{ORB_ID(distance_sensor)}; uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _ranging_beacon_pub{ORB_ID(ranging_beacon)}; // groundtruth uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; @@ -140,6 +142,29 @@ private: // 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 @@ private: 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 @@ private: 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 @@ private: 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 @@ private: (ParamFloat) _sih_thrust_tau, (ParamInt) _sih_vtype, (ParamFloat) _sih_wind_n, - (ParamFloat) _sih_wind_e + (ParamFloat) _sih_wind_e, + (ParamFloat) _sih_ranging_beacon_noise ) }; diff --git a/src/modules/simulation/simulator_sih/sih_params.yaml b/src/modules/simulation/simulator_sih/sih_params.yaml index 475e03e94b..6bd1db0d8b 100644 --- a/src/modules/simulation/simulator_sih/sih_params.yaml +++ b/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