forked from Archive/PX4-Autopilot
simulator: support accel/gyro instances and stuck failure
- 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
GitHub
parent
c01fabaf11
commit
378cb155d6
|
|
@ -123,10 +123,12 @@ then
|
|||
|
||||
param set BAT_N_CELLS 4
|
||||
|
||||
param set CAL_ACC0_ID 1311244
|
||||
param set CAL_ACC1_ID 1311500
|
||||
param set CAL_GYRO0_ID 1311244
|
||||
param set CAL_GYRO1_ID 1311500
|
||||
param set CAL_ACC0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_ACC1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_ACC2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_GYRO1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
||||
param set CAL_MAG0_ID 197388
|
||||
param set CAL_MAG1_ID 197644
|
||||
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@
|
|||
#include "PX4Accelerometer.hpp"
|
||||
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <lib/parameters/param.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
|
@ -64,7 +65,6 @@ static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit,
|
|||
}
|
||||
|
||||
PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) :
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_accel)},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo)},
|
||||
_device_id{device_id},
|
||||
|
|
@ -73,7 +73,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) :
|
|||
// advertise immediately to keep instance numbering in sync
|
||||
_sensor_pub.advertise();
|
||||
|
||||
updateParams();
|
||||
param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max);
|
||||
}
|
||||
|
||||
PX4Accelerometer::~PX4Accelerometer()
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -36,20 +36,19 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_accel_fifo.h>
|
||||
|
||||
class PX4Accelerometer : public ModuleParams
|
||||
class PX4Accelerometer
|
||||
{
|
||||
public:
|
||||
PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Accelerometer() override;
|
||||
~PX4Accelerometer();
|
||||
|
||||
uint32_t get_device_id() const { return _device_id; }
|
||||
|
||||
float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
|
||||
int32_t get_max_rate_hz() const { return _imu_gyro_rate_max; }
|
||||
|
||||
void set_device_id(uint32_t device_id) { _device_id = device_id; }
|
||||
void set_device_type(uint8_t devtype);
|
||||
|
|
@ -73,6 +72,8 @@ private:
|
|||
uint32_t _device_id{0};
|
||||
const enum Rotation _rotation;
|
||||
|
||||
int32_t _imu_gyro_rate_max{0}; // match gyro max rate
|
||||
|
||||
float _range{16 * CONSTANTS_ONE_G};
|
||||
float _scale{1.f};
|
||||
float _temperature{NAN};
|
||||
|
|
@ -82,8 +83,4 @@ private:
|
|||
uint32_t _error_count{0};
|
||||
|
||||
int16_t _last_sample[3] {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
|
||||
)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@
|
|||
#include "PX4Gyroscope.hpp"
|
||||
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <lib/parameters/param.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
|
@ -51,7 +52,6 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
|
|||
}
|
||||
|
||||
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_gyro)},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)},
|
||||
_device_id{device_id},
|
||||
|
|
@ -60,7 +60,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
|
|||
// advertise immediately to keep instance numbering in sync
|
||||
_sensor_pub.advertise();
|
||||
|
||||
updateParams();
|
||||
param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max);
|
||||
}
|
||||
|
||||
PX4Gyroscope::~PX4Gyroscope()
|
||||
|
|
|
|||
|
|
@ -35,20 +35,19 @@
|
|||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_gyro_fifo.h>
|
||||
|
||||
class PX4Gyroscope : public ModuleParams
|
||||
class PX4Gyroscope
|
||||
{
|
||||
public:
|
||||
PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Gyroscope() override;
|
||||
~PX4Gyroscope();
|
||||
|
||||
uint32_t get_device_id() const { return _device_id; }
|
||||
|
||||
float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
|
||||
int32_t get_max_rate_hz() const { return _imu_gyro_rate_max; }
|
||||
|
||||
void set_device_id(uint32_t device_id) { _device_id = device_id; }
|
||||
void set_device_type(uint8_t devtype);
|
||||
|
|
@ -71,6 +70,8 @@ private:
|
|||
uint32_t _device_id{0};
|
||||
const enum Rotation _rotation;
|
||||
|
||||
int32_t _imu_gyro_rate_max{0};
|
||||
|
||||
float _range{math::radians(2000.f)};
|
||||
float _scale{1.f};
|
||||
float _temperature{NAN};
|
||||
|
|
@ -78,8 +79,4 @@ private:
|
|||
uint32_t _error_count{0};
|
||||
|
||||
int16_t _last_sample[3] {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
|
||||
)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@
|
|||
****************************************************************************/
|
||||
|
||||
#include <parameters/param.h>
|
||||
#include <px4_platform_common/cli.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
|
@ -83,7 +85,6 @@ static constexpr FailureType failure_types[] = {
|
|||
{ "intermittent", vehicle_command_s::FAILURE_TYPE_INTERMITTENT},
|
||||
};
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
|
|
@ -104,6 +105,7 @@ failure gps off
|
|||
PRINT_MODULE_USAGE_COMMAND_DESCR("help", "Show this help text");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("gps|...", "Specify component");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("ok|off|...", "Specify failure type");
|
||||
PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "sensor instance (0=all)", true);
|
||||
|
||||
PX4_INFO_RAW("\nComponents:\n");
|
||||
for (const auto &failure_unit : failure_units) {
|
||||
|
|
@ -116,41 +118,58 @@ failure gps off
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
int inject_failure(uint8_t unit, uint8_t type)
|
||||
int inject_failure(uint8_t unit, uint8_t type, uint8_t instance)
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
PX4_WARN("inject failure unit: %s (%d), type: %s (%d), instance: %d", failure_units[unit].key, unit, failure_types[type].key, type, instance);
|
||||
|
||||
uORB::Subscription command_ack_sub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
uORB::PublicationQueued<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
|
||||
vehicle_command_s command{};
|
||||
command.timestamp = now;
|
||||
|
||||
command.command = vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE;
|
||||
command.param1 = static_cast<float>(unit);
|
||||
command.param2 = static_cast<float>(type);
|
||||
command.param3 = static_cast<float>(instance);
|
||||
command.timestamp = hrt_absolute_time();
|
||||
command_pub.publish(command);
|
||||
|
||||
vehicle_command_ack_s ack;
|
||||
while (hrt_elapsed_time(&now) < 1_s) {
|
||||
|
||||
while (hrt_elapsed_time(&command.timestamp) < 1_s) {
|
||||
if (command_ack_sub.update(&ack)) {
|
||||
if (ack.command == command.command) {
|
||||
if (ack.result != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
|
||||
PX4_ERR("Result: %d", ack.result);
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
px4_usleep(10000);
|
||||
}
|
||||
|
||||
PX4_ERR("Timeout waiting for ack");
|
||||
return 1;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int failure_main(int argc, char *argv[])
|
||||
{
|
||||
int32_t param = 0;
|
||||
|
||||
if (PX4_OK != param_get(param_find("SYS_FAILURE_EN"), ¶m)) {
|
||||
PX4_ERR("Could not get param SYS_FAILURE_EN");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (param != 1) {
|
||||
PX4_ERR("Failure injection disabled by SYS_FAILURE_EN param.");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (argc == 2 && strcmp(argv[1], "help") == 0) {
|
||||
print_usage();
|
||||
return 0;
|
||||
|
|
@ -162,37 +181,46 @@ extern "C" __EXPORT int failure_main(int argc, char *argv[])
|
|||
return 1;
|
||||
}
|
||||
|
||||
int32_t param = 0;
|
||||
if (PX4_OK != param_get(param_find("SYS_FAILURE_EN"), ¶m)) {
|
||||
PX4_ERR("Could not get param SYS_FAILURE_EN");
|
||||
return 1;
|
||||
const char *myoptarg = nullptr;
|
||||
int ch = 0;
|
||||
int myoptind = 1;
|
||||
|
||||
uint8_t instance = 0;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'i':
|
||||
instance = (uint8_t)atoi(myoptarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("Unknown option");
|
||||
print_usage();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (param != 1) {
|
||||
PX4_ERR("Failure injection disabled by SYS_FAILURE_EN param.");
|
||||
return 1;
|
||||
}
|
||||
|
||||
const char *requested_failure_unit = argv[1];
|
||||
const char *requested_failure_type = argv[2];
|
||||
|
||||
const char *requested_failure_unit = argv[myoptind];
|
||||
|
||||
for (const auto &failure_unit : failure_units) {
|
||||
if (strncmp(failure_unit.key, requested_failure_unit, sizeof(failure_unit.key)) != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const char *requested_failure_type = argv[myoptind + 1];
|
||||
|
||||
for (const auto &failure_type : failure_types) {
|
||||
if (strncmp(failure_type.key, requested_failure_type, sizeof(failure_type.key)) != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
return inject_failure(failure_unit.value, failure_type.value);
|
||||
return inject_failure(failure_unit.value, failure_type.value, instance);
|
||||
}
|
||||
|
||||
PX4_ERR("Failure type '%s' not found", requested_failure_type);
|
||||
return 1;
|
||||
}
|
||||
|
||||
PX4_ERR("Component '%s' not found", requested_failure_unit);
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue