fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 08:10:36 +08: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
src/modules/mavlink/mavlink_receiver.cpp
+8 -8
View File
@@ -2195,8 +2195,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
// gyro
if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
if (_px4_gyro == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1311244);
// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1310988);
}
if (_px4_gyro != nullptr) {
@@ -2211,8 +2211,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
// accelerometer
if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
if (_px4_accel == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1310988);
}
if (_px4_accel != nullptr) {
@@ -2634,8 +2634,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* accelerometer */
{
if (_px4_accel == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1310988);
if (_px4_accel == nullptr) {
PX4_ERR("PX4Accelerometer alloc failed");
@@ -2652,8 +2652,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* gyroscope */
{
if (_px4_gyro == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1311244);
// 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1310988);
if (_px4_gyro == nullptr) {
PX4_ERR("PX4Gyroscope alloc failed");
src/modules/sih/sih.hpp
+2 -2
View File
@@ -98,8 +98,8 @@ private:
void parameters_updated();
// simulated sensor instances
PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
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
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
src/modules/simulator/simulator.h
+22 -7
View File
@@ -60,7 +60,6 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -157,11 +156,19 @@ private:
static Simulator *_instance;
// simulated sensor instances
PX4Accelerometer _px4_accel_0{1311244, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Accelerometer _px4_accel_1{1311500, ROTATION_NONE}; // 1311500: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] {
{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
};
PX4Gyroscope _px4_gyro_0{1311244, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro_1{1311500, ROTATION_NONE}; // 1311500: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
static constexpr uint8_t GYRO_COUNT_MAX = 3;
PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] {
{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
{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
@@ -256,12 +263,20 @@ private:
vehicle_status_s _vehicle_status{};
bool _accel_blocked{false};
bool _gyro_blocked{false};
bool _accel_blocked[ACCEL_COUNT_MAX] {};
bool _accel_stuck[ACCEL_COUNT_MAX] {};
matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
bool _gyro_blocked[GYRO_COUNT_MAX] {};
bool _gyro_stuck[GYRO_COUNT_MAX] {};
matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
bool _baro_blocked{false};
bool _baro_stuck{false};
bool _mag_blocked{false};
bool _mag_stuck{false};
bool _gps_blocked{false};
bool _airspeed_blocked{false};
src/modules/simulator/simulator_mavlink.cpp
+101 -17
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) {