fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(simulation): add ranging beacon simulation in SIH

Browse Source
This commit is contained in:
Marco Hauswirth 2026-03-31 20:43:22 +02:00 committed by Marco Hauswirth
parent f4c820c7e1
commit 65c96fb2bf
3 changed files with 98 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/modules/simulation/simulator_sih/sih.cpp
View File

@ -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)
{
{

32
src/modules/simulation/simulator_sih/sih.hpp
View File

@ -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 @@ private:
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 @@ 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<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
)
};

10
src/modules/simulation/simulator_sih/sih_params.yaml
View File

@ -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