yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

simulator: support accel/gyro instances and stuck failure

Browse Source 
- expand simulator to 3 accels and gyros
 - PX4Accelerometer/PX4Gyroscope switch to old param usage due to copy constructor issues with ModuleParams
This commit is contained in:
Daniel Agar
2020-10-07 13:20:13 -04:00
committed by GitHub
parent c01fabaf11
commit 378cb155d6
10 changed files with 201 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -215,13 +215,6 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// temperature only updated with baro
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
if (PX4_ISFINITE(sensors.temperature)) {
_px4_accel_0.set_temperature(sensors.temperature);
_px4_accel_1.set_temperature(sensors.temperature);
_px4_gyro_0.set_temperature(sensors.temperature);
_px4_gyro_1.set_temperature(sensors.temperature);
_px4_mag_0.set_temperature(sensors.temperature);
_px4_mag_1.set_temperature(sensors.temperature);
@@ -230,15 +223,31 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
}
// accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_accel_blocked) {
_px4_accel_0.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
_px4_accel_1.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
if (_accel_stuck[i]) {
_px4_accel[i].update(time, _last_accel[i](0), _last_accel[i](1), _last_accel[i](2));
} 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};
}
}
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_gyro_blocked) {
_px4_gyro_0.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
_px4_gyro_1.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
if (_gyro_stuck[i]) {
_px4_gyro[i].update(time, _last_gyro[i](0), _last_gyro[i](1), _last_gyro[i](2));
} 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};
}
}
}
// magnetometer
@@ -954,6 +963,7 @@ void Simulator::check_failure_injections()
const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) {
handled = true;
@@ -972,11 +982,48 @@ void Simulator::check_failure_injections()
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
supported = true;
_accel_blocked = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
_accel_blocked[i] = true;
_accel_stuck[i] = false;
}
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
_accel_blocked[instance - 1] = true;
_accel_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 < ACCEL_COUNT_MAX; i++) {
_accel_blocked[i] = false;
_accel_stuck[i] = true;
}
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
_accel_blocked[instance - 1] = false;
_accel_stuck[instance - 1] = true;
}
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_accel_blocked = false;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
_accel_blocked[i] = false;
_accel_stuck[i] = false;
}
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
_accel_blocked[instance - 1] = false;
_accel_stuck[instance - 1] = false;
}
}
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
@@ -984,11 +1031,48 @@ void Simulator::check_failure_injections()
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
supported = true;
_gyro_blocked = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
_gyro_blocked[i] = true;
_gyro_stuck[i] = false;
}
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
_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++) {
_gyro_blocked[i] = false;
_gyro_stuck[i] = true;
}
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
_gyro_blocked[instance - 1] = false;
_gyro_stuck[instance - 1] = true;
}
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_gyro_blocked = false;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
_gyro_blocked[i] = false;
_gyro_stuck[i] = false;
}
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
_gyro_blocked[instance - 1] = false;
_gyro_stuck[instance - 1] = false;
}
}
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {