Enable multiple simulated imu and magnetometers in gazebo

This commit is contained in:
Konrad 2022-08-23 13:59:49 +02:00 committed by JaeyoungLim
parent f9b6edab07
commit 5a7f098b8d
3 changed files with 145 additions and 93 deletions

@ -1 +1 @@
Subproject commit 07552959bc49bb1333e17a0ecd29ddd2ff70829c
Subproject commit b968405a617005ba0a793a8b4703913d4c6eff5f

View File

@ -189,105 +189,112 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
// temperature only updated with baro
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
if (PX4_ISFINITE(sensors.temperature)) {
_px4_mag_0.set_temperature(sensors.temperature);
_px4_mag_1.set_temperature(sensors.temperature);
_sensors_temperature = sensors.temperature;
}
}
// accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
if (i == 0) {
// accel 0 is simulated FIFO
static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f;
static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G;
if (sensors.id >= ACCEL_COUNT_MAX) {
PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
return;
}
_px4_accel[i].set_scale(ACCEL_FIFO_SCALE);
_px4_accel[i].set_range(ACCEL_FIFO_RANGE);
if (sensors.id == 0) {
// accel 0 is simulated FIFO
static constexpr float ACCEL_FIFO_SCALE = CONSTANTS_ONE_G / 2048.f;
static constexpr float ACCEL_FIFO_RANGE = 16.f * CONSTANTS_ONE_G;
if (_accel_stuck[i]) {
_px4_accel[i].updateFIFO(_last_accel_fifo);
_px4_accel[sensors.id].set_scale(ACCEL_FIFO_SCALE);
_px4_accel[sensors.id].set_range(ACCEL_FIFO_RANGE);
} else if (!_accel_blocked[i]) {
_px4_accel[i].set_temperature(_sensors_temperature);
if (_accel_stuck[sensors.id]) {
_px4_accel[sensors.id].updateFIFO(_last_accel_fifo);
_last_accel_fifo.samples = 1;
_last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample;
_last_accel_fifo.timestamp_sample = time;
_last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE;
} else if (!_accel_blocked[sensors.id]) {
_px4_accel[sensors.id].set_temperature(_sensors_temperature);
_px4_accel[i].updateFIFO(_last_accel_fifo);
}
_last_accel_fifo.samples = 1;
_last_accel_fifo.dt = time - _last_accel_fifo.timestamp_sample;
_last_accel_fifo.timestamp_sample = time;
_last_accel_fifo.x[0] = sensors.xacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.y[0] = sensors.yacc / ACCEL_FIFO_SCALE;
_last_accel_fifo.z[0] = sensors.zacc / ACCEL_FIFO_SCALE;
} else {
if (_accel_stuck[i]) {
_px4_accel[i].update(time, _last_accel[i](0), _last_accel[i](1), _last_accel[i](2));
_px4_accel[sensors.id].updateFIFO(_last_accel_fifo);
}
} else if (!_accel_blocked[i]) {
_px4_accel[i].set_temperature(_sensors_temperature);
_px4_accel[i].update(time, sensors.xacc, sensors.yacc, sensors.zacc);
_last_accel[i] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc};
}
} else {
if (_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]) {
_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};
}
}
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
if (i == 0) {
// gyro 0 is simulated FIFO
static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f);
static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f);
if (sensors.id >= GYRO_COUNT_MAX) {
PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
return;
}
_px4_gyro[i].set_scale(GYRO_FIFO_SCALE);
_px4_gyro[i].set_range(GYRO_FIFO_RANGE);
if (sensors.id == 0) {
// gyro 0 is simulated FIFO
static constexpr float GYRO_FIFO_SCALE = math::radians(2000.f / 32768.f);
static constexpr float GYRO_FIFO_RANGE = math::radians(2000.f);
if (_gyro_stuck[i]) {
_px4_gyro[i].updateFIFO(_last_gyro_fifo);
_px4_gyro[sensors.id].set_scale(GYRO_FIFO_SCALE);
_px4_gyro[sensors.id].set_range(GYRO_FIFO_RANGE);
} else if (!_gyro_blocked[i]) {
_px4_gyro[i].set_temperature(_sensors_temperature);
if (_gyro_stuck[sensors.id]) {
_px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo);
_last_gyro_fifo.samples = 1;
_last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample;
_last_gyro_fifo.timestamp_sample = time;
_last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE;
} else if (!_gyro_blocked[sensors.id]) {
_px4_gyro[sensors.id].set_temperature(_sensors_temperature);
_px4_gyro[i].updateFIFO(_last_gyro_fifo);
}
_last_gyro_fifo.samples = 1;
_last_gyro_fifo.dt = time - _last_gyro_fifo.timestamp_sample;
_last_gyro_fifo.timestamp_sample = time;
_last_gyro_fifo.x[0] = sensors.xgyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.y[0] = sensors.ygyro / GYRO_FIFO_SCALE;
_last_gyro_fifo.z[0] = sensors.zgyro / GYRO_FIFO_SCALE;
} else {
if (_gyro_stuck[i]) {
_px4_gyro[i].update(time, _last_gyro[i](0), _last_gyro[i](1), _last_gyro[i](2));
_px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo);
}
} else if (!_gyro_blocked[i]) {
_px4_gyro[i].set_temperature(_sensors_temperature);
_px4_gyro[i].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
_last_gyro[i] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro};
}
} else {
if (_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]) {
_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};
}
}
}
// magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {
if (_mag_stuck) {
_px4_mag_0.update(time, _last_magx, _last_magy, _last_magz);
_px4_mag_1.update(time, _last_magx, _last_magy, _last_magz);
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG) {
if (sensors.id >= MAG_COUNT_MAX) {
PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX);
return;
}
} else {
_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_last_magx = sensors.xmag;
_last_magy = sensors.ymag;
_last_magz = sensors.zmag;
if (_mag_stuck[sensors.id]) {
_px4_mag[sensors.id].update(time, _last_magx[sensors.id], _last_magy[sensors.id], _last_magz[sensors.id]);
} else if (!_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;
}
}
@ -459,16 +466,21 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg)
{
if (_lockstep_component == -1) {
_lockstep_component = px4_lockstep_register_component();
}
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
struct timespec ts;
abstime_to_ts(&ts, imu.time_usec);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
// Assume imu with id 0 is the primary imu an base lockstep based on this.
if (imu.id == 0) {
if (_lockstep_component == -1) {
_lockstep_component = px4_lockstep_register_component();
}
struct timespec ts;
abstime_to_ts(&ts, imu.time_usec);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
}
hrt_abstime now_us = hrt_absolute_time();
@ -487,15 +499,17 @@ void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg)
update_sensors(now_us, imu);
if (imu.id == 0) {
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (!_has_initialized.load()) {
_has_initialized.store(true);
}
if (!_has_initialized.load()) {
_has_initialized.store(true);
}
#endif
px4_lockstep_progress(_lockstep_component);
px4_lockstep_progress(_lockstep_component);
}
}
void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
@ -1341,20 +1355,55 @@ void SimulatorMavlink::check_failure_injections()
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
PX4_WARN("CMD_INJECT_FAILURE, mag off");
supported = true;
_mag_blocked = 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) {
PX4_WARN("CMD_INJECT_FAILURE, mag stuck");
supported = true;
_mag_stuck = true;
_mag_blocked = false;
// 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) {
PX4_INFO("CMD_INJECT_FAILURE, mag ok");
supported = true;
_mag_blocked = false;
// 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) {

View File

@ -178,8 +178,11 @@ private:
{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
};
PX4Magnetometer _px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
static constexpr uint8_t MAG_COUNT_MAX = 2;
PX4Magnetometer _px4_mag[MAG_COUNT_MAX] {
{197388, ROTATION_NONE},
{197644, ROTATION_NONE},
};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pubs[2] {{ORB_ID(sensor_baro)}, {ORB_ID(sensor_baro)}};
@ -287,15 +290,15 @@ private:
bool _baro_blocked{false};
bool _baro_stuck{false};
bool _mag_blocked{false};
bool _mag_stuck{false};
bool _mag_blocked[MAG_COUNT_MAX] {};
bool _mag_stuck[MAG_COUNT_MAX] {};
bool _gps_blocked{false};
bool _airspeed_blocked{false};
float _last_magx{0.0f};
float _last_magy{0.0f};
float _last_magz{0.0f};
float _last_magx[MAG_COUNT_MAX] {};
float _last_magy[MAG_COUNT_MAX] {};
float _last_magz[MAG_COUNT_MAX] {};
float _last_baro_pressure{0.0f};
float _last_baro_temperature{0.0f};