diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 100643cfc8..e26f6137b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -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 diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 04e712596e..7d6518a772 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -35,6 +35,7 @@ #include "PX4Accelerometer.hpp" #include +#include 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() diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index d63b010ab3..c81fd5b6ca 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -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 #include #include -#include #include #include #include -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) _param_imu_gyro_rate_max - ) }; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 8ecfa4624b..19a775df48 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -35,6 +35,7 @@ #include "PX4Gyroscope.hpp" #include +#include 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() diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 963f56fd2b..dc83791498 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -35,20 +35,19 @@ #include #include -#include #include #include #include -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) _param_imu_gyro_rate_max - ) }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0879e16c02..455816959a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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"); diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index a0a40143d1..aab36a7c6a 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -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 diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 930f7c290a..2efbc41107 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -60,7 +60,6 @@ #include #include #include -#include #include #include #include @@ -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}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index b8fe710e7e..89dd27f8aa 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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(vehicle_command.param1 + 0.5f); const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + const int instance = static_cast(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) { diff --git a/src/systemcmds/failure/failure.cpp b/src/systemcmds/failure/failure.cpp index 9f65aa805b..6c8a6d86d0 100644 --- a/src/systemcmds/failure/failure.cpp +++ b/src/systemcmds/failure/failure.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include +#include +#include #include #include #include @@ -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 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(unit); command.param2 = static_cast(type); + command.param3 = static_cast(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; }