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

* add missing failure units and types

Browse Source 
* move sensor simulation from sih to SensorSimManager
* fix failureInjection usage
This commit is contained in:
Marco Hauswirth
2025-10-03 10:41:12 +02:00
parent 8bf68e91e2
commit bfee522856
9 changed files with 497 additions and 430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/failure_injection/failureInjection.cpp
+177 -177
View File
@@ -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;
src/modules/simulation/failure_injection/failureInjection.hpp
+26 -9
View File
@@ -1,10 +1,6 @@
// #include <drivers/drv_hrt.h>
// #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
// #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
// #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
// #include <lib/geo/geo.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/bitmask.h>
@@ -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};
};
src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp
+236 -35
View File
@@ -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<hrt_abstime>(_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<hrt_abstime>(_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();
src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp
+33 -5
View File
@@ -50,10 +50,15 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <simulation/failure_injection/failureInjection.hpp>
#include <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
@@ -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_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<vehicle_global_position_s> _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<px4::params::SIM_AGP_FAIL>) _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
};
src/modules/simulation/sensor_sim_manager/parameters.c
+4 -4
View File
@@ -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
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+20 -20
View File
@@ -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);
}
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+1 -15
View File
@@ -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};
src/modules/simulation/simulator_sih/sih.cpp
-136
View File
@@ -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<VehicleType>(constrain(_sih_vtype.get(),
static_cast<int32_t>(VehicleType::First),
static_cast<int32_t>(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<typeof _I>(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()
{
src/modules/simulation/simulator_sih/sih.hpp
-29
View File
@@ -63,8 +63,6 @@
#include <conversion/rotation.h> // math::radians,
#include <lib/atmosphere/atmosphere.h> // to get the physical constants
#include <drivers/drv_hrt.h> // to get the real time
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/geo/geo.h>
#include <lib/lat_lon_alt/lat_lon_alt.hpp>
#include <lib/perf/perf_counter.h>
@@ -72,9 +70,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -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_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
// groundtruth
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _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<px4::params::SIH_LOC_LAT0>) _sih_lat0,
(ParamFloat<px4::params::SIH_LOC_LON0>) _sih_lon0,
(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
(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
)