From bfee522856507b745d26b6648a7a666a0a8e90f6 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 18 Sep 2025 03:37:23 +0200 Subject: [PATCH] * add missing failure units and types * move sensor simulation from sih to SensorSimManager * fix failureInjection usage --- .../failure_injection/failureInjection.cpp | 354 +++++++++--------- .../failure_injection/failureInjection.hpp | 35 +- .../sensor_sim_manager/SensorSimManager.cpp | 271 ++++++++++++-- .../sensor_sim_manager/SensorSimManager.hpp | 38 +- .../sensor_sim_manager/parameters.c | 8 +- .../simulator_mavlink/SimulatorMavlink.cpp | 40 +- .../simulator_mavlink/SimulatorMavlink.hpp | 16 +- src/modules/simulation/simulator_sih/sih.cpp | 136 ------- src/modules/simulation/simulator_sih/sih.hpp | 29 -- 9 files changed, 497 insertions(+), 430 deletions(-) diff --git a/src/modules/simulation/failure_injection/failureInjection.cpp b/src/modules/simulation/failure_injection/failureInjection.cpp index 1c15ef2c31..4923fbe71b 100644 --- a/src/modules/simulation/failure_injection/failureInjection.cpp +++ b/src/modules/simulation/failure_injection/failureInjection.cpp @@ -171,35 +171,35 @@ void FailureInjection::check_failure_injections() } - } if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT) { + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT) { handled = true; if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, GPS off"); + PX4_WARN("CMD_INJECT_FAILURE, GPS altitude off"); supported = true; - _gps_blocked = true; + _gps_alt_blocked = true; } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); + PX4_INFO("CMD_INJECT_FAILURE, GPS altitude ok"); supported = true; - _gps_blocked = false; - _gps_stuck = false; - _gps_wrong = false; - _gps_drift = false; + _gps_alt_blocked = false; + _gps_alt_stuck = false; + _gps_alt_wrong = false; + _gps_alt_drift = false; } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { supported = true; - _gps_stuck = true; + _gps_alt_stuck = true; } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { supported = true; - _gps_wrong = true; + _gps_alt_wrong = true; } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { - PX4_INFO("CMD_INJECT_FAILURE, GPS drift"); + PX4_INFO("CMD_INJECT_FAILURE, GPS altitude drift"); supported = true; - _gps_drift = true; + _gps_alt_drift = true; } @@ -258,173 +258,173 @@ void FailureInjection::check_failure_injections() _accel_stuck[instance - 1] = false; } } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); + _gyro_blocked[i] = true; + _gyro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); + _gyro_blocked[instance - 1] = true; + _gyro_stuck[instance - 1] = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILUREe, gyro %d stuck", i); + _gyro_blocked[i] = false; + _gyro_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); + _gyro_blocked[instance - 1] = false; + _gyro_stuck[instance - 1] = true; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); + _gyro_blocked[i] = false; + _gyro_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); + _gyro_blocked[instance - 1] = false; + _gyro_stuck[instance - 1] = false; + } + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < MAG_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); + _mag_blocked[i] = true; + _mag_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); + _mag_blocked[instance - 1] = true; + _mag_stuck[instance - 1] = false; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < MAG_COUNT_MAX; i++) { + PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); + _mag_blocked[i] = false; + _mag_stuck[i] = true; + } + + } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); + _mag_blocked[instance - 1] = false; + _mag_stuck[instance - 1] = true; + } + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + supported = true; + + // 0 to signal all + if (instance == 0) { + for (int i = 0; i < MAG_COUNT_MAX; i++) { + PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", i); + _mag_blocked[i] = false; + _mag_stuck[i] = false; + } + + } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", instance - 1); + _mag_blocked[instance - 1] = false; + _mag_stuck[instance - 1] = false; + } + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, baro off"); + supported = true; + _baro_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); + supported = true; + _baro_stuck = true; + _baro_blocked = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, baro ok"); + supported = true; + _baro_blocked = false; + _baro_stuck = false; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); + supported = true; + _airspeed_disconnected = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); + supported = true; + _airspeed_blocked_timestamp = hrt_absolute_time(); + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); + supported = true; + _airspeed_disconnected = false; + _airspeed_blocked_timestamp = 0; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, vio off"); + supported = true; + _vio_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, vio ok"); + supported = true; + _vio_blocked = false; + } } - // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { - // handled = true; - - // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < GYRO_COUNT_MAX; i++) { - // PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); - // _gyro_blocked[i] = true; - // _gyro_stuck[i] = false; - // } - - // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - // PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); - // _gyro_blocked[instance - 1] = true; - // _gyro_stuck[instance - 1] = false; - // } - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < GYRO_COUNT_MAX; i++) { - // PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); - // _gyro_blocked[i] = false; - // _gyro_stuck[i] = true; - // } - - // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - // PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); - // _gyro_blocked[instance - 1] = false; - // _gyro_stuck[instance - 1] = true; - // } - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < GYRO_COUNT_MAX; i++) { - // PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); - // _gyro_blocked[i] = false; - // _gyro_stuck[i] = false; - // } - - // } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - // PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); - // _gyro_blocked[instance - 1] = false; - // _gyro_stuck[instance - 1] = false; - // } - // } - - // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { - // handled = true; - - // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < MAG_COUNT_MAX; i++) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); - // _mag_blocked[i] = true; - // _mag_stuck[i] = false; - // } - - // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); - // _mag_blocked[instance - 1] = true; - // _mag_stuck[instance - 1] = false; - // } - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < MAG_COUNT_MAX; i++) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); - // _mag_blocked[i] = false; - // _mag_stuck[i] = true; - // } - - // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); - // _mag_blocked[instance - 1] = false; - // _mag_stuck[instance - 1] = true; - // } - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - // supported = true; - - // // 0 to signal all - // if (instance == 0) { - // for (int i = 0; i < MAG_COUNT_MAX; i++) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", i); - // _mag_blocked[i] = false; - // _mag_stuck[i] = false; - // } - - // } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - // PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", instance - 1); - // _mag_blocked[instance - 1] = false; - // _mag_stuck[instance - 1] = false; - // } - // } - - // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { - // handled = true; - - // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // PX4_WARN("CMD_INJECT_FAILURE, baro off"); - // supported = true; - // _baro_blocked = true; - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - // PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); - // supported = true; - // _baro_stuck = true; - // _baro_blocked = false; - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - // PX4_INFO("CMD_INJECT_FAILURE, baro ok"); - // supported = true; - // _baro_blocked = false; - // } - - // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { - // handled = true; - - // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); - // supported = true; - // _airspeed_disconnected = true; - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - // PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); - // supported = true; - // _airspeed_blocked_timestamp = hrt_absolute_time(); - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - // PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); - // supported = true; - // _airspeed_disconnected = false; - // _airspeed_blocked_timestamp = 0; - // } - - // } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { - // handled = true; - - // if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // PX4_WARN("CMD_INJECT_FAILURE, vio off"); - // supported = true; - // _vio_blocked = true; - - // } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - // PX4_INFO("CMD_INJECT_FAILURE, vio ok"); - // supported = true; - // _vio_blocked = false; - // } - // } - if (handled) { vehicle_command_ack_s ack{}; ack.command = vehicle_command.command; diff --git a/src/modules/simulation/failure_injection/failureInjection.hpp b/src/modules/simulation/failure_injection/failureInjection.hpp index 9f0db0b6ed..402ecc21e2 100644 --- a/src/modules/simulation/failure_injection/failureInjection.hpp +++ b/src/modules/simulation/failure_injection/failureInjection.hpp @@ -1,10 +1,6 @@ -// #include -// #include -// #include -// #include -// #include +#include #include #include #include @@ -40,8 +36,22 @@ public: bool handle_gps_failure(sensor_gps_s &gps); bool handle_gps_alt_failure(sensor_gps_s &gps); - bool is_accel_blocked(const int instance=0) { return _accel_blocked[instance]; } - bool is_accel_stuck(const int instance=0) { return _accel_stuck[instance]; } + bool is_accel_blocked(const int instance = 0) { return _accel_blocked[instance]; } + bool is_accel_stuck(const int instance = 0) { return _accel_stuck[instance]; } + + bool is_gyro_blocked(const int instance = 0) { return _gyro_blocked[instance]; } + bool is_gyro_stuck(const int instance = 0) { return _gyro_stuck[instance]; } + + bool is_mag_blocked(const int instance = 0) { return _mag_blocked[instance]; } + bool is_mag_stuck(const int instance = 0) { return _mag_stuck[instance]; } + + bool is_baro_blocked() { return _baro_blocked; } + bool is_baro_stuck() { return _baro_stuck; } + + bool is_airspeed_disconnected() { return _airspeed_disconnected; } + hrt_abstime get_airspeed_blocked_timestamp() { return _airspeed_blocked_timestamp; } + + bool is_vio_blocked() { return _vio_blocked; } private: @@ -68,9 +78,16 @@ private: bool _gps_alt_wrong{false}; sensor_gps_s _gps_prev{}; - - // accel bool _accel_blocked[ACCEL_COUNT_MAX] {}; bool _accel_stuck[ACCEL_COUNT_MAX] {}; + bool _gyro_blocked[GYRO_COUNT_MAX] {}; + bool _gyro_stuck[GYRO_COUNT_MAX] {}; + bool _mag_blocked[MAG_COUNT_MAX] {}; + bool _mag_stuck[MAG_COUNT_MAX] {}; + bool _baro_blocked{false}; + bool _baro_stuck{false}; + bool _airspeed_disconnected{false}; + hrt_abstime _airspeed_blocked_timestamp{0}; + bool _vio_blocked{false}; }; diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp index 5e06940ba8..f19951aa19 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp @@ -49,7 +49,9 @@ SensorSimManager::SensorSimManager() : _gen(_rd()), _uniform_dist(0.0f, 1.0f) { - _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); + _px4_accel.set_temperature(T1_C); + _px4_gyro.set_temperature(T1_C); + srand(1234); // Initialize sensor timings with base intervals and random offsets @@ -95,11 +97,29 @@ SensorSimManager::SensorSimManager() : .enabled = false, }; + // IMU: 250 Hz + _imu_timing = { + .interval_us = 4000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 4000), + .enabled = true, // Always enabled + }; + + // Distance sensor: 50 Hz + _distance_sensor_timing = { + .interval_us = 20000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 20000), + .enabled = true, // Always enabled + }; + _gps_timing.next_update_time += _gps_timing.offset_us; _baro_timing.next_update_time += _baro_timing.offset_us; _mag_timing.next_update_time += _mag_timing.offset_us; _airspeed_timing.next_update_time += _airspeed_timing.offset_us; _agp_timing.next_update_time += _agp_timing.offset_us; + _imu_timing.next_update_time += _imu_timing.offset_us; + _distance_sensor_timing.next_update_time += _distance_sensor_timing.offset_us; } @@ -111,11 +131,13 @@ SensorSimManager::~SensorSimManager() perf_free(_mag_perf); perf_free(_airspeed_perf); perf_free(_agp_perf); + perf_free(_imu_perf); + perf_free(_distance_sensor_perf); } bool SensorSimManager::init() { - ScheduleOnInterval(10_ms); + ScheduleOnInterval(2_ms); return true; } @@ -173,11 +195,11 @@ void SensorSimManager::Run() _agp_timing.enabled = (_param_sens_en_agpsim.get() != 0); } - _failure_injection.check_failure_injections(); - const hrt_abstime now = hrt_absolute_time(); if (_gps_timing.enabled && now >= _gps_timing.next_update_time) { + _failure_injection.check_failure_injections(); // TODO, just here for now... + updateGPS(); _gps_timing.next_update_time = now + _gps_timing.interval_us; } @@ -202,6 +224,16 @@ void SensorSimManager::Run() _agp_timing.next_update_time = now + _agp_timing.interval_us; } + if (_imu_timing.enabled && now >= _imu_timing.next_update_time) { + updateIMU(); + _imu_timing.next_update_time = now + _imu_timing.interval_us; + } + + if (_distance_sensor_timing.enabled && now >= _distance_sensor_timing.next_update_time) { + updateDistanceSensor(); + _distance_sensor_timing.next_update_time = now + _distance_sensor_timing.interval_us; + } + perf_end(_loop_perf); } @@ -209,8 +241,6 @@ void SensorSimManager::updateGPS() { perf_begin(_gps_perf); - _failure_injection.check_failure_injections(); - vehicle_local_position_s lpos{}; _vehicle_local_position_sub.copy(&lpos); @@ -342,25 +372,25 @@ void SensorSimManager::updateBarometer() } } - // Apply noise and drift - const float abs_pressure_noise = 1.f * (float)y1; // 1 Pa RMS noise - _baro_drift_pa += _baro_drift_pa_per_sec * dt; - const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa; + if (!_failure_injection.is_baro_stuck()) { + const float abs_pressure_noise = 1.f * (float)y1; // 1 Pa RMS noise + _baro_drift_pa += _baro_drift_pa_per_sec * dt; + const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa; - // convert to hPa - float pressure = absolute_pressure_noisy + _sim_baro_off_p.get(); + _last_baro_pressure = absolute_pressure_noisy + _sim_baro_off_p.get(); - // calculate temperature in Celsius - float temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); + _last_baro_temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); + } - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = gpos.timestamp; - sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = pressure; - sensor_baro.temperature = temperature; - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); + if (!_failure_injection.is_baro_blocked()) { + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = gpos.timestamp; + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = _last_baro_pressure; + sensor_baro.temperature = _last_baro_temperature; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + } _last_baro_update_time = gpos.timestamp; } @@ -395,14 +425,23 @@ void SensorSimManager::updateMagnetometer() vehicle_attitude_s attitude; if (_vehicle_attitude_sub.copy(&attitude)) { - Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; + if (_failure_injection.is_mag_stuck(0)) { + _px4_mag.update(attitude.timestamp, _last_mag(0), _last_mag(1), _last_mag(2)); - expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f); + } else if (!_failure_injection.is_mag_blocked(0)) { + Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; - _px4_mag.update(attitude.timestamp, + expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f); + + Vector3f mag_output{ expected_field(0) + _sim_mag_offset_x.get(), expected_field(1) + _sim_mag_offset_y.get(), - expected_field(2) + _sim_mag_offset_z.get()); + expected_field(2) + _sim_mag_offset_z.get() + }; + + _px4_mag.update(attitude.timestamp, mag_output(0), mag_output(1), mag_output(2)); + _last_mag = mag_output; + } } } @@ -413,6 +452,11 @@ void SensorSimManager::updateAirspeed() { perf_begin(_airspeed_perf); + if (_failure_injection.is_airspeed_disconnected()) { + perf_end(_airspeed_perf); + return; + } + vehicle_local_position_s lpos{}; _vehicle_local_position_sub.copy(&lpos); @@ -428,29 +472,56 @@ void SensorSimManager::updateAirspeed() if (lpos.timestamp > 0 && gpos.timestamp > 0 && attitude.timestamp > 0 && (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000) { - Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; - Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + Vector3f velocity_E{lpos.vx, lpos.vy, lpos.vz}; // Velocity in NED frame (similar to Earth frame) - // device id - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + airspeed_s airspeed{}; + airspeed.timestamp_sample = now; + float true_airspeed = fmaxf(0.1f, velocity_E.norm() + generate_wgn() * 0.2f); + + hrt_abstime blocked_timestamp = _failure_injection.get_airspeed_blocked_timestamp(); + + if (blocked_timestamp > 0) { + // Simulate blocked pitot tube - airspeed decreases exponentially + float time_since_blocked = (now - blocked_timestamp) * 1e-6f; + true_airspeed *= expf(-time_since_blocked / 10.0f); // Decay over ~10 seconds + } + + airspeed.true_airspeed_m_s = true_airspeed; + + // Calculate indicated airspeed using air density ratio + // For now, use standard air density ratio calculation const float alt_amsl = gpos.alt; const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); const float air_density = AIR_DENSITY_MSL / density_ratio; + airspeed.indicated_airspeed_m_s = true_airspeed * sqrtf(air_density / RHO); + airspeed.confidence = 0.7f; + airspeed.timestamp = hrt_absolute_time(); + _airspeed_pub.publish(airspeed); + + // Also publish differential pressure for compatibility + Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; + Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + // calculate differential pressure + noise in hPa const float diff_pressure_noise = (float)generate_wgn() * 0.01f; float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( 0) + diff_pressure_noise; + const float blockage_fraction = 0.7f; // defines max blockage (fully ramped) + const float airspeed_blockage_rampup_time = 1e6f; // 1 second in microseconds + + float airspeed_blockage_scale = 1.f; + if (blocked_timestamp > 0) { + airspeed_blockage_scale = math::constrain(1.f - (now - blocked_timestamp) / + airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); + } + differential_pressure_s differential_pressure{}; differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa; + differential_pressure.differential_pressure_pa = diff_pressure * 100.f * airspeed_blockage_scale; // hPa to Pa with blockage scaling differential_pressure.temperature = temperature_local; differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(differential_pressure); @@ -518,6 +589,136 @@ void SensorSimManager::updateAGP() perf_end(_agp_perf); } +void SensorSimManager::updateIMU() +{ + perf_begin(_imu_perf); + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); + + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); + + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + + // Check if data is recent (within 20ms) + const hrt_abstime now = hrt_absolute_time(); + + if (gpos.timestamp > 0 && lpos.timestamp > 0 && attitude.timestamp > 0 && angular_velocity.timestamp > 0 && + (now - gpos.timestamp) < 20000 && (now - lpos.timestamp) < 20000 && + (now - attitude.timestamp) < 20000 && (now - angular_velocity.timestamp) < 20000) { + + // The sensor signals reconstruction and noise levels are from [1] + // [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." + // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. + + Quatf q_E{attitude.q}; + const Dcmf R_E2B(q_E.inversed()); + + Vector3f acceleration_N{lpos.ax, lpos.ay, lpos.az}; // acceleration in NED frame + + const float gravity = 9.80665f; // Standard gravity + Vector3f gravity_N{0.0f, 0.0f, gravity}; // Gravity in NED (down positive) + Vector3f specific_force_N = acceleration_N - gravity_N; // Subtract gravity from acceleration to get specific force + Vector3f specific_force_B = Dcmf{Quatf{attitude.q}}.transpose() * specific_force_N; + + Vector3f accel_noise; + Vector3f gyro_noise; + + // Use velocity magnitude to determine noise level (higher noise when moving) + Vector3f velocity{lpos.vx, lpos.vy, lpos.vz}; + + if (velocity.norm() > 0.1f) { + accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); + gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); + + } else { + accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f); + gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); + } + + Vector3f accel = specific_force_B + accel_noise; + + // Angular velocity with Earth rotation and noise + Vector3f w_B{angular_velocity.xyz[0], angular_velocity.xyz[1], angular_velocity.xyz[2]}; + const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); + Vector3f gyro = w_B + earth_spin_rate_B + gyro_noise; + + if (_failure_injection.is_accel_stuck()) { + _px4_accel.update(now, _last_accel(0), _last_accel(1), _last_accel(2)); + + } else if (!_failure_injection.is_accel_blocked()) { + _px4_accel.update(now, accel(0), accel(1), accel(2)); + } + + if (_failure_injection.is_gyro_stuck()) { + _px4_gyro.update(now, _last_gyro(0), _last_gyro(1), _last_gyro(2)); + + } else if (!_failure_injection.is_gyro_blocked()) { + _px4_gyro.update(now, gyro(0), gyro(1), gyro(2)); + } + + _last_accel = accel; + _last_gyro = gyro; + } + + perf_end(_imu_perf); +} + +void SensorSimManager::updateDistanceSensor() +{ + perf_begin(_distance_sensor_perf); + + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); + + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); + + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (lpos.timestamp > 0 && attitude.timestamp > 0 && + (now - lpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000 && + fabsf(_distance_snsr_override) < 10000) { + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + + distance_sensor_s distance_sensor{}; + distance_sensor.device_id = device_id.devid; + distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + distance_sensor.min_distance = _distance_snsr_min; + distance_sensor.max_distance = _distance_snsr_max; + distance_sensor.signal_quality = -1; + + if (_distance_snsr_override >= 0.f) { + distance_sensor.current_distance = _distance_snsr_override; + + } else { + Quatf q{attitude.q}; + distance_sensor.current_distance = -lpos.z / q.dcm_z()(2); + + if (distance_sensor.current_distance > _distance_snsr_max) { + distance_sensor.current_distance = UINT16_MAX / 100.f; + } + } + + distance_sensor.timestamp = hrt_absolute_time(); + _distance_sensor_pub.publish(distance_sensor); + } + + perf_end(_distance_sensor_perf); +} + int SensorSimManager::task_spawn(int argc, char *argv[]) { SensorSimManager *instance = new SensorSimManager(); diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp index 6643b83850..ddec12dbd0 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp @@ -50,10 +50,15 @@ #include #include #include +#include +#include +#include #include #include #include #include +#include +#include #include #include #include @@ -88,6 +93,8 @@ private: void updateMagnetometer(); void updateAirspeed(); void updateAGP(); + void updateIMU(); + void updateDistanceSensor(); // Utility methods static float generate_wgn(); @@ -107,24 +114,34 @@ private: SensorTiming _mag_timing; SensorTiming _airspeed_timing; SensorTiming _agp_timing; + SensorTiming _imu_timing; + SensorTiming _distance_sensor_timing; // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity_groundtruth)}; // Publications uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::PublicationMulti _airspeed_pub{ORB_ID(airspeed)}; + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; - // Magnetometer uses PX4Magnetometer publisher - PX4Magnetometer _px4_mag{1, ROTATION_NONE}; + PX4Accelerometer _px4_accel{1310988, ROTATION_NONE}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1310988, ROTATION_NONE}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION FailureInjection _failure_injection{}; + matrix::Vector3f _last_mag{}; + float _last_baro_pressure{0.0f}; + float _last_baro_temperature{0.0f}; + // TODO: needed? perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _gps_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gps")}; @@ -132,6 +149,8 @@ private: perf_counter_t _mag_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": mag")}; perf_counter_t _airspeed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": airspeed")}; perf_counter_t _agp_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": agp")}; + perf_counter_t _imu_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": imu")}; + perf_counter_t _distance_sensor_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": distance")}; // Random number generator for offsets and noise std::random_device _rd; @@ -154,26 +173,35 @@ private: (ParamInt) _sim_agp_fail ) - // Barometer simulation state hrt_abstime _last_baro_update_time{0}; float _baro_drift_pa{0.0f}; float _baro_drift_pa_per_sec{0.1f}; // TODO, was 0 bool _baro_rnd_use_last{false}; double _baro_rnd_y2{0.0}; - // Magnetometer simulation state matrix::Vector3f _mag_earth_pred{}; bool _mag_earth_available{false}; - // AGP simulation state LatLonAlt _agp_measured_lla{}; matrix::Vector3f _agp_position_bias{}; uint64_t _agp_time_last_update{0}; + matrix::Vector3f _last_accel{}; + matrix::Vector3f _last_gyro{}; + matrix::Vector3f _specific_force_E{}; + + float _distance_snsr_min{0.3f}; + float _distance_snsr_max{50.0f}; + float _distance_snsr_override{-1.0f}; + // Air constants static constexpr float TEMPERATURE_MSL = 288.0f; // [K] static constexpr float PRESSURE_MSL = 101325.0f; // [Pa] static constexpr float LAPSE_RATE = 0.0065f; // [K/m] static constexpr float AIR_DENSITY_MSL = 1.225f; // [kg/m^3] + static constexpr float RHO = 1.225f; // Air density at sea level [kg/m^3] + + // IMU constants + static constexpr float T1_C = 15.0f; // Temperature constant }; diff --git a/src/modules/simulation/sensor_sim_manager/parameters.c b/src/modules/simulation/sensor_sim_manager/parameters.c index b29e979c3d..bccceafa8a 100644 --- a/src/modules/simulation/sensor_sim_manager/parameters.c +++ b/src/modules/simulation/sensor_sim_manager/parameters.c @@ -46,7 +46,7 @@ PARAM_DEFINE_INT32(SENS_EN_GPSSIM, 1); * @value 0 Disabled * @value 1 Enabled */ -PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 1); +PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 0); /** * Enable simulated magnetometer sensor @@ -55,7 +55,7 @@ PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 1); * @value 0 Disabled * @value 1 Enabled */ -PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 1); +PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 0); /** * Enable simulated airspeed sensor @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 1); * @value 0 Disabled * @value 1 Enabled */ -PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 1); +PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0); /** * Enable simulated AGP sensor @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 1); * @value 0 Disabled * @value 1 Enabled */ -PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 1); +PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); /** * Number of GPS satellites used in simulation diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index ef3748bacf..fe2656552e 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -210,10 +210,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil _px4_accel[sensors.id].set_scale(ACCEL_FIFO_SCALE); _px4_accel[sensors.id].set_range(ACCEL_FIFO_RANGE); - if (_accel_stuck[sensors.id]) { + if (_failure_injection.is_accel_stuck(sensors.id)) { _px4_accel[sensors.id].updateFIFO(_last_accel_fifo); - } else if (!_accel_blocked[sensors.id]) { + } else if (!_failure_injection.is_accel_blocked(sensors.id)) { _px4_accel[sensors.id].set_temperature(_sensors_temperature); _last_accel_fifo.samples = 1; @@ -227,10 +227,11 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil } } else { - if (_accel_stuck[sensors.id]) { + + if (_failure_injection.is_accel_stuck(sensors.id)) { _px4_accel[sensors.id].update(time, _last_accel[sensors.id](0), _last_accel[sensors.id](1), _last_accel[sensors.id](2)); - } else if (!_accel_blocked[sensors.id]) { + } else if (!_failure_injection.is_accel_blocked(sensors.id)) { _px4_accel[sensors.id].set_temperature(_sensors_temperature); _px4_accel[sensors.id].update(time, sensors.xacc, sensors.yacc, sensors.zacc); _last_accel[sensors.id] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc}; @@ -253,10 +254,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil _px4_gyro[sensors.id].set_scale(GYRO_FIFO_SCALE); _px4_gyro[sensors.id].set_range(GYRO_FIFO_RANGE); - if (_gyro_stuck[sensors.id]) { + if (_failure_injection.is_gyro_stuck(sensors.id)) { _px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo); - } else if (!_gyro_blocked[sensors.id]) { + } else if (!_failure_injection.is_gyro_blocked(sensors.id)) { _px4_gyro[sensors.id].set_temperature(_sensors_temperature); _last_gyro_fifo.samples = 1; @@ -270,10 +271,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil } } else { - if (_gyro_stuck[sensors.id]) { + if (_failure_injection.is_gyro_stuck(sensors.id)) { _px4_gyro[sensors.id].update(time, _last_gyro[sensors.id](0), _last_gyro[sensors.id](1), _last_gyro[sensors.id](2)); - } else if (!_gyro_blocked[sensors.id]) { + } else if (!_failure_injection.is_gyro_blocked(sensors.id)) { _px4_gyro[sensors.id].set_temperature(_sensors_temperature); _px4_gyro[sensors.id].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro); _last_gyro[sensors.id] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro}; @@ -288,22 +289,20 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil return; } - if (_mag_stuck[sensors.id]) { - _px4_mag[sensors.id].update(time, _last_magx[sensors.id], _last_magy[sensors.id], _last_magz[sensors.id]); + if (_failure_injection.is_mag_stuck(sensors.id)) { + _px4_mag[sensors.id].update(time, _last_mag[sensors.id](0), _last_mag[sensors.id](1), _last_mag[sensors.id](2)); - } else if (!_mag_blocked[sensors.id]) { + } else if (!_failure_injection.is_mag_blocked(sensors.id)) { _px4_mag[sensors.id].set_temperature(_sensors_temperature); _px4_mag[sensors.id].update(time, sensors.xmag, sensors.ymag, sensors.zmag); - _last_magx[sensors.id] = sensors.xmag; - _last_magy[sensors.id] = sensors.ymag; - _last_magz[sensors.id] = sensors.zmag; + _last_mag[sensors.id] = matrix::Vector3f{sensors.xmag, sensors.ymag, sensors.zmag}; } } // baro - if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { + if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_failure_injection.is_baro_blocked()) { - if (!_baro_stuck) { + if (!_failure_injection.is_baro_stuck()) { _last_baro_pressure = sensors.abs_pressure * 100.f; // hPa to Pa _last_baro_temperature = sensors.temperature; } @@ -326,15 +325,16 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil } // differential pressure - if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_disconnected) { + if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_failure_injection.is_airspeed_disconnected()) { const float blockage_fraction = 0.7; // defines max blockage (fully ramped) const float airspeed_blockage_rampup_time = 1_s; // time it takes to go max blockage, linear ramp float airspeed_blockage_scale = 1.f; - if (_airspeed_blocked_timestamp > 0) { - airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) / + hrt_abstime blocked_timestamp = _failure_injection.get_airspeed_blocked_timestamp(); + if (blocked_timestamp > 0) { + airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - blocked_timestamp) / airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); } @@ -827,7 +827,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) case MAV_ESTIMATOR_TYPE_NAIVE: case MAV_ESTIMATOR_TYPE_VISION: case MAV_ESTIMATOR_TYPE_VIO: - if (!_vio_blocked) { + if (!_failure_injection.is_vio_blocked()) { odom.timestamp = hrt_absolute_time(); _visual_odometry_pub.publish(odom); } diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 65243d6ce4..48b6011df1 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -158,7 +158,6 @@ private: } - void check_failure_injections(); int publish_distance_topic(const mavlink_distance_sensor_t *dist); @@ -278,29 +277,16 @@ private: vehicle_status_s _vehicle_status{}; battery_status_s _battery_status{}; - bool _accel_blocked[ACCEL_COUNT_MAX] {}; - bool _accel_stuck[ACCEL_COUNT_MAX] {}; sensor_accel_fifo_s _last_accel_fifo{}; matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {}; - bool _gyro_blocked[GYRO_COUNT_MAX] {}; - bool _gyro_stuck[GYRO_COUNT_MAX] {}; sensor_gyro_fifo_s _last_gyro_fifo{}; matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {}; - bool _baro_blocked{false}; - bool _baro_stuck{false}; - bool _mag_blocked[MAG_COUNT_MAX] {}; - bool _mag_stuck[MAG_COUNT_MAX] {}; - bool _airspeed_disconnected{false}; - hrt_abstime _airspeed_blocked_timestamp{0}; - bool _vio_blocked{false}; - float _last_magx[MAG_COUNT_MAX] {}; - float _last_magy[MAG_COUNT_MAX] {}; - float _last_magz[MAG_COUNT_MAX] {}; + matrix::Vector3f _last_mag[MAG_COUNT_MAX] {}; float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f}; diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index d6227ed4fa..05f246648e 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -65,16 +65,11 @@ Sih::~Sih() void Sih::run() { - _px4_accel.set_temperature(T1_C); - _px4_gyro.set_temperature(T1_C); - init_variables(); parameters_updated(); const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; - _airspeed_time = task_start; - _dist_snsr_time = task_start; _vehicle = static_cast(constrain(_sih_vtype.get(), static_cast(VehicleType::First), static_cast(VehicleType::Last))); @@ -215,23 +210,6 @@ void Sih::sensor_step() equations_of_motion(dt); - reconstruct_sensors_signals(now); - - if ((_vehicle == VehicleType::FixedWing - || _vehicle == VehicleType::TailsitterVTOL - || _vehicle == VehicleType::StandardVTOL) - && now - _airspeed_time >= 50_ms) { - _airspeed_time = now; - send_airspeed(now); - } - - // distance sensor published at 50 Hz - if (now - _dist_snsr_time >= 20_ms - && fabs(_distance_snsr_override) < 10000) { - _dist_snsr_time = now; - send_dist_snsr(now); - } - publish_ground_truth(now); perf_end(_loop_perf); @@ -277,10 +255,6 @@ void Sih::parameters_updated() // guards against too small determinants _Im1 = 100.0f * inv(static_cast(100.0f * _I)); - _distance_snsr_min = _sih_distance_snsr_min.get(); - _distance_snsr_max = _sih_distance_snsr_max.get(); - _distance_snsr_override = _sih_distance_snsr_override.get(); - _T_TAU = _sih_thrust_tau.get(); } @@ -608,85 +582,8 @@ Dcmf Sih::computeRotEcefToNed(const LatLonAlt &lla) return Dcmf(val); } -void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) -{ - // The sensor signals reconstruction and noise levels are from [1] - // [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." - // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. - // IMU - const Dcmf R_E2B(_q_E.inversed()); - Vector3f accel_noise; - Vector3f gyro_noise; - if (_T_B.longerThan(FLT_EPSILON)) { - accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); - gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); - - } else { - // Lower noise when not armed - accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f); - gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); - } - - Vector3f specific_force_B = R_E2B * _specific_force_E; - Vector3f accel = specific_force_B + accel_noise; - - const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); - Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise; - - // update IMU every iteration - _px4_accel.update(time_now_us, accel(0), accel(1), accel(2)); - _px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2)); -} - -void Sih::send_airspeed(const hrt_abstime &time_now_us) -{ - // TODO: send differential pressure instead? - 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); - 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(); - _airspeed_pub.publish(airspeed); -} - -void Sih::send_dist_snsr(const hrt_abstime &time_now_us) -{ - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - - distance_sensor_s distance_sensor{}; - // distance_sensor.timestamp_sample = time_now_us; - distance_sensor.device_id = device_id.devid; - distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - distance_sensor.min_distance = _distance_snsr_min; - distance_sensor.max_distance = _distance_snsr_max; - distance_sensor.signal_quality = -1; - - if (_distance_snsr_override >= 0.f) { - distance_sensor.current_distance = _distance_snsr_override; - - } else { - distance_sensor.current_distance = -_lpos(2) / _q.dcm_z()(2); - - if (distance_sensor.current_distance > _distance_snsr_max) { - // this is based on lightware lw20 behaviour - distance_sensor.current_distance = UINT16_MAX / 100.f; - - } - } - - distance_sensor.timestamp = hrt_absolute_time(); - _distance_snsr_pub.publish(distance_sensor); -} void Sih::publish_ground_truth(const hrt_abstime &time_now_us) { @@ -763,39 +660,6 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) } } -float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 -{ - // algorithm 1: - // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); - // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); - // algorithm 2: from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - float U1 = (float)rand() / (float)RAND_MAX; - float U2 = (float)rand() / (float)RAND_MAX; - V1 = 2.0f * U1 - 1.0f; - V2 = 2.0f * U2 - 1.0f; - S = V1 * V1 + V2 * V2; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz) -{ - return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); -} int Sih::print_status() { diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index bda4484884..e9225d09a3 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -63,8 +63,6 @@ #include // math::radians, #include // to get the physical constants #include // to get the real time -#include -#include #include #include #include @@ -72,9 +70,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -110,20 +106,9 @@ public: /** @see ModuleBase::run() */ void run() override; - static float generate_wgn(); // generate white Gaussian noise sample - - // generate white Gaussian noise sample as a 3D vector with specified std - static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz); - private: void parameters_updated(); - // simulated sensors - PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - 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)}; - // groundtruth uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; @@ -135,11 +120,7 @@ private: // hard constants static constexpr uint16_t NUM_ACTUATORS_MAX = 9; - 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 // Aerodynamic coefficients - static constexpr float RHO = 1.225f; // air density at sea level [kg/m^3] static constexpr float SPAN = 0.86f; // wing span [m] static constexpr float MAC = 0.21f; // wing mean aerodynamic chord [m] static constexpr float RP = 0.1f; // radius of the propeller [m] @@ -156,10 +137,6 @@ private: // apply the equations of motion of a rigid body and integrate one step void equations_of_motion(const float dt); - // reconstruct the noisy sensor signals - 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 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(); @@ -194,8 +171,6 @@ private: hrt_abstime _last_run{0}; hrt_abstime _last_actuator_output_time{0}; - hrt_abstime _airspeed_time{0}; - hrt_abstime _dist_snsr_time{0}; bool _grounded{true};// whether the vehicle is on the ground @@ -275,7 +250,6 @@ private: matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix - float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override; // parameters defined in sih_params.c DEFINE_PARAMETERS( @@ -297,9 +271,6 @@ private: (ParamFloat) _sih_lat0, (ParamFloat) _sih_lon0, (ParamFloat) _sih_h0, - (ParamFloat) _sih_distance_snsr_min, - (ParamFloat) _sih_distance_snsr_max, - (ParamFloat) _sih_distance_snsr_override, (ParamFloat) _sih_thrust_tau, (ParamInt) _sih_vtype )