mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 18:13:34 +08:00
* add failure injection with first simple usecase for sitl
* add failure type 'drift' and failure-unit 'gps_alt'
This commit is contained in:
Marco Hauswirth
@@ -140,6 +140,7 @@ uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5
|
||||
uint8 FAILURE_UNIT_SENSOR_VIO = 6
|
||||
uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7
|
||||
uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8
|
||||
uint8 FAILURE_UNIT_SENSOR_GPS_ALT = 9
|
||||
uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100
|
||||
uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101
|
||||
uint8 FAILURE_UNIT_SYSTEM_SERVO = 102
|
||||
@@ -155,6 +156,7 @@ uint8 FAILURE_TYPE_WRONG = 4
|
||||
uint8 FAILURE_TYPE_SLOW = 5
|
||||
uint8 FAILURE_TYPE_DELAYED = 6
|
||||
uint8 FAILURE_TYPE_INTERMITTENT = 7
|
||||
uint8 FAILURE_TYPE_DRIFT = 8
|
||||
|
||||
# Used as param1 in DO_CHANGE_SPEED command.
|
||||
uint8 SPEED_TYPE_AIRSPEED = 0
|
||||
|
||||
@@ -0,0 +1,439 @@
|
||||
|
||||
#include <simulation/failure_injection/failureInjection.hpp>
|
||||
|
||||
void FailureInjection::test()
|
||||
{
|
||||
printf("test\n");
|
||||
}
|
||||
|
||||
bool FailureInjection::handle_gps_failure(sensor_gps_s &gps)
|
||||
{
|
||||
if (_gps_blocked) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_gps_stuck) {
|
||||
|
||||
if (_gps_wrong) {
|
||||
gps.latitude_deg += 1.0;
|
||||
gps.longitude_deg += 1.0;
|
||||
gps.altitude_msl_m += 100.0;
|
||||
gps.altitude_ellipsoid_m += 100.0;
|
||||
|
||||
gps.vel_m_s -= 1.f;
|
||||
gps.vel_n_m_s += 5.f;
|
||||
gps.vel_e_m_s -= 8.f;
|
||||
gps.vel_d_m_s += 2.f;
|
||||
}
|
||||
|
||||
|
||||
if (_gps_drift) {
|
||||
if (!_has_drift_ref) {
|
||||
|
||||
_mp.initReference(gps.latitude_deg, gps.longitude_deg);
|
||||
// Project current LL to local meters to anchor the reference point
|
||||
_mp.project(gps.latitude_deg, gps.longitude_deg, _gps_drift_pos(0), _gps_drift_pos(1));
|
||||
_has_drift_ref = true;
|
||||
}
|
||||
|
||||
matrix::Vector2f vel_gps(gps.vel_n_m_s, gps.vel_e_m_s);
|
||||
const float vel_norm = vel_gps.norm();
|
||||
|
||||
// directional drift by 5%
|
||||
if (vel_norm > 1.f) {
|
||||
vel_gps *= 1.f + RELATIVE_GPS_DRIFT;
|
||||
|
||||
} else if (vel_norm > 0.01f) {
|
||||
vel_gps *= 0.5f / vel_norm;
|
||||
|
||||
} else {
|
||||
vel_gps(0) = 0.5f;
|
||||
}
|
||||
|
||||
// perpendicular drift by 5%
|
||||
matrix::Vector2f vel_normal = vel_gps.normalized();
|
||||
vel_gps(0) += vel_norm * RELATIVE_GPS_DRIFT * vel_normal(1);
|
||||
vel_gps(1) -= vel_norm * RELATIVE_GPS_DRIFT * vel_normal(0);
|
||||
|
||||
gps.vel_n_m_s = vel_gps(0);
|
||||
gps.vel_e_m_s = vel_gps(1);
|
||||
|
||||
if (_last_gps_timestamp > 0) {
|
||||
float dt = 1e-6f * (gps.timestamp - _last_gps_timestamp);
|
||||
_gps_drift_pos += vel_gps * dt;
|
||||
_mp.reproject(_gps_drift_pos(0), _gps_drift_pos(1), gps.latitude_deg, gps.longitude_deg);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
} else {
|
||||
gps = _gps_prev;
|
||||
}
|
||||
|
||||
_last_gps_timestamp = gps.timestamp;
|
||||
_gps_prev = gps;
|
||||
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps)
|
||||
{
|
||||
if (_gps_alt_blocked) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_gps_alt_stuck) {
|
||||
|
||||
if (_gps_alt_wrong) {
|
||||
gps.altitude_msl_m += 100.0;
|
||||
gps.altitude_ellipsoid_m += 100.0;
|
||||
|
||||
}
|
||||
|
||||
if (_gps_alt_drift) {
|
||||
if (_t0 == 0) {
|
||||
_t0 = gps.timestamp;
|
||||
}
|
||||
|
||||
if (_gps_alt_offset < 0.01) { _gps_alt_offset = gps.altitude_msl_m; }
|
||||
|
||||
gps.vel_d_m_s = 5. * sin(2 * M_PI / 20. * 1e-6 * (gps.timestamp - _t0));
|
||||
|
||||
if (_last_gps_timestamp > 0) {
|
||||
_gps_alt_offset += -(double)(gps.vel_d_m_s * (gps.timestamp - _last_gps_timestamp) / 1.e6f);
|
||||
}
|
||||
|
||||
gps.altitude_msl_m = _gps_alt_offset;
|
||||
|
||||
}
|
||||
|
||||
|
||||
} else {
|
||||
gps = _gps_prev;
|
||||
}
|
||||
|
||||
_last_gps_timestamp = gps.timestamp;
|
||||
_gps_prev = gps;
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void FailureInjection::check_failure_injections()
|
||||
{
|
||||
vehicle_command_s vehicle_command;
|
||||
|
||||
while (_vehicle_command_sub.update(&vehicle_command)) {
|
||||
if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool handled = false;
|
||||
bool supported = false;
|
||||
|
||||
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;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, GPS off");
|
||||
supported = true;
|
||||
_gps_blocked = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
|
||||
supported = true;
|
||||
_gps_blocked = false;
|
||||
_gps_stuck = false;
|
||||
_gps_wrong = false;
|
||||
_gps_drift = false;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
|
||||
supported = true;
|
||||
_gps_stuck = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
|
||||
supported = true;
|
||||
_gps_wrong = true;
|
||||
// }
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, GPsS drift");
|
||||
supported = true;
|
||||
_gps_drift = true;
|
||||
|
||||
}
|
||||
|
||||
} if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT) {
|
||||
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, GPS off");
|
||||
supported = true;
|
||||
_gps_blocked = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
|
||||
supported = true;
|
||||
_gps_blocked = false;
|
||||
_gps_stuck = false;
|
||||
_gps_wrong = false;
|
||||
_gps_drift = false;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
|
||||
supported = true;
|
||||
_gps_stuck = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
|
||||
supported = true;
|
||||
_gps_wrong = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, GPS drift");
|
||||
supported = true;
|
||||
_gps_drift = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i);
|
||||
_accel_blocked[i] = true;
|
||||
_accel_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1);
|
||||
_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++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i);
|
||||
_accel_blocked[i] = false;
|
||||
_accel_stuck[i] = true;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1);
|
||||
_accel_blocked[instance - 1] = false;
|
||||
_accel_stuck[instance - 1] = true;
|
||||
}
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i);
|
||||
_accel_blocked[i] = false;
|
||||
_accel_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1);
|
||||
_accel_blocked[instance - 1] = false;
|
||||
_accel_stuck[instance - 1] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
|
||||
// handled = true;
|
||||
|
||||
// if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
// supported = true;
|
||||
|
||||
// // 0 to signal all
|
||||
// if (instance == 0) {
|
||||
// for (int i = 0; i < GYRO_COUNT_MAX; i++) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i);
|
||||
// _gyro_blocked[i] = true;
|
||||
// _gyro_stuck[i] = false;
|
||||
// }
|
||||
|
||||
// } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1);
|
||||
// _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++) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i);
|
||||
// _gyro_blocked[i] = false;
|
||||
// _gyro_stuck[i] = true;
|
||||
// }
|
||||
|
||||
// } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1);
|
||||
// _gyro_blocked[instance - 1] = false;
|
||||
// _gyro_stuck[instance - 1] = true;
|
||||
// }
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
// supported = true;
|
||||
|
||||
// // 0 to signal all
|
||||
// if (instance == 0) {
|
||||
// for (int i = 0; i < GYRO_COUNT_MAX; i++) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i);
|
||||
// _gyro_blocked[i] = false;
|
||||
// _gyro_stuck[i] = false;
|
||||
// }
|
||||
|
||||
// } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1);
|
||||
// _gyro_blocked[instance - 1] = false;
|
||||
// _gyro_stuck[instance - 1] = false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {
|
||||
// handled = true;
|
||||
|
||||
// if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
// supported = 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) {
|
||||
// supported = true;
|
||||
|
||||
// // 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) {
|
||||
// supported = true;
|
||||
|
||||
// // 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) {
|
||||
// handled = true;
|
||||
|
||||
// if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, baro off");
|
||||
// supported = true;
|
||||
// _baro_blocked = true;
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, baro stuck");
|
||||
// supported = true;
|
||||
// _baro_stuck = true;
|
||||
// _baro_blocked = false;
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, baro ok");
|
||||
// supported = true;
|
||||
// _baro_blocked = false;
|
||||
// }
|
||||
|
||||
// } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) {
|
||||
// handled = true;
|
||||
|
||||
// if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, airspeed off");
|
||||
// supported = true;
|
||||
// _airspeed_disconnected = true;
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)");
|
||||
// supported = true;
|
||||
// _airspeed_blocked_timestamp = hrt_absolute_time();
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, airspeed ok");
|
||||
// supported = true;
|
||||
// _airspeed_disconnected = false;
|
||||
// _airspeed_blocked_timestamp = 0;
|
||||
// }
|
||||
|
||||
// } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) {
|
||||
// handled = true;
|
||||
|
||||
// if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
// PX4_WARN("CMD_INJECT_FAILURE, vio off");
|
||||
// supported = true;
|
||||
// _vio_blocked = true;
|
||||
|
||||
// } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
// PX4_INFO("CMD_INJECT_FAILURE, vio ok");
|
||||
// supported = true;
|
||||
// _vio_blocked = false;
|
||||
// }
|
||||
// }
|
||||
|
||||
if (handled) {
|
||||
vehicle_command_ack_s ack{};
|
||||
ack.command = vehicle_command.command;
|
||||
ack.from_external = false;
|
||||
ack.result = supported ?
|
||||
vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
|
||||
vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
ack.timestamp = hrt_absolute_time();
|
||||
_command_ack_pub.publish(ack);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
|
||||
|
||||
// #include <drivers/drv_hrt.h>
|
||||
// #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
// #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
// #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
// #include <lib/geo/geo.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/bitmask.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
|
||||
static constexpr uint8_t ACCEL_COUNT_MAX = 4;
|
||||
static constexpr uint8_t GYRO_COUNT_MAX = 4;
|
||||
static constexpr uint8_t MAG_COUNT_MAX = 4;
|
||||
static constexpr uint8_t BARO_COUNT_MAX = 4;
|
||||
|
||||
|
||||
static constexpr float RELATIVE_GPS_DRIFT = 0.1f;
|
||||
|
||||
class FailureInjection
|
||||
{
|
||||
|
||||
public:
|
||||
FailureInjection() {};
|
||||
void test();
|
||||
|
||||
|
||||
void check_failure_injections();
|
||||
bool handle_gps_failure(sensor_gps_s &gps);
|
||||
bool handle_gps_alt_failure(sensor_gps_s &gps);
|
||||
|
||||
bool is_accel_blocked(const int instance=0) { return _accel_blocked[instance]; }
|
||||
bool is_accel_stuck(const int instance=0) { return _accel_stuck[instance]; }
|
||||
|
||||
private:
|
||||
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
|
||||
|
||||
// GPS
|
||||
bool _gps_drift{false};
|
||||
bool _gps_alt_drift{false};
|
||||
MapProjection _mp;
|
||||
bool _has_drift_ref{false};
|
||||
matrix::Vector2f _gps_drift_pos;
|
||||
double _gps_alt_offset{0};
|
||||
hrt_abstime _last_gps_timestamp{0};
|
||||
hrt_abstime _t0{0};
|
||||
|
||||
bool _gps_blocked{false};
|
||||
bool _gps_alt_blocked{false};
|
||||
bool _gps_stuck{false};
|
||||
bool _gps_alt_stuck{false};
|
||||
bool _gps_wrong{false};
|
||||
bool _gps_alt_wrong{false};
|
||||
sensor_gps_s _gps_prev{};
|
||||
|
||||
|
||||
// accel
|
||||
bool _accel_blocked[ACCEL_COUNT_MAX] {};
|
||||
bool _accel_stuck[ACCEL_COUNT_MAX] {};
|
||||
|
||||
};
|
||||
@@ -41,9 +41,11 @@ px4_add_module(
|
||||
INCLUDES
|
||||
${CMAKE_BINARY_DIR}/mavlink
|
||||
${CMAKE_BINARY_DIR}/mavlink/${CONFIG_MAVLINK_DIALECT}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection
|
||||
SRCS
|
||||
SimulatorMavlink.cpp
|
||||
SimulatorMavlink.hpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection/failureInjection.cpp
|
||||
DEPENDS
|
||||
mavlink_c_generate
|
||||
conversion
|
||||
|
||||
@@ -410,70 +410,49 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
|
||||
mavlink_hil_gps_t hil_gps;
|
||||
mavlink_msg_hil_gps_decode(msg, &hil_gps);
|
||||
|
||||
if (!_gps_blocked) {
|
||||
sensor_gps_s gps{};
|
||||
sensor_gps_s gps{};
|
||||
|
||||
if (!_gps_stuck) {
|
||||
if (!_gps_wrong) {
|
||||
gps.latitude_deg = hil_gps.lat / 1e7;
|
||||
gps.longitude_deg = hil_gps.lon / 1e7;
|
||||
gps.altitude_msl_m = hil_gps.alt / 1e3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
|
||||
gps.timestamp = hrt_absolute_time();
|
||||
gps.latitude_deg = hil_gps.lat / 1e7;
|
||||
gps.longitude_deg = hil_gps.lon / 1e7;
|
||||
gps.altitude_msl_m = hil_gps.alt / 1e3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
|
||||
|
||||
} else {
|
||||
gps.latitude_deg = hil_gps.lat / 1e7 + 1.0;
|
||||
gps.longitude_deg = hil_gps.lon / 1e7 + 1.0;
|
||||
gps.altitude_msl_m = hil_gps.alt / 1e3 + 100.0;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3 - 100.0;
|
||||
}
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
gps.hdop = 0; // TODO
|
||||
gps.vdop = 0; // TODO
|
||||
|
||||
gps.hdop = 0; // TODO
|
||||
gps.vdop = 0; // TODO
|
||||
gps.noise_per_ms = 0;
|
||||
gps.automatic_gain_control = 0;
|
||||
gps.jamming_indicator = 0;
|
||||
gps.jamming_state = 0;
|
||||
gps.spoofing_state = 0;
|
||||
|
||||
gps.noise_per_ms = 0;
|
||||
gps.automatic_gain_control = 0;
|
||||
gps.jamming_indicator = 0;
|
||||
gps.jamming_state = 0;
|
||||
gps.spoofing_state = 0;
|
||||
gps.vel_m_s = (float)(hil_gps.vel) / 100.0f;
|
||||
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f;
|
||||
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f;
|
||||
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f;
|
||||
|
||||
if (!_gps_wrong) {
|
||||
gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
|
||||
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
|
||||
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
|
||||
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
|
||||
|
||||
} else {
|
||||
gps.vel_m_s = (float)(hil_gps.vel) / 100.0f - 1.f; // cm/s -> m/s
|
||||
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f + 5.f; // cm/s -> m/s
|
||||
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f - 8.f; // cm/s -> m/s
|
||||
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f + 2.f; // cm/s -> m/s
|
||||
}
|
||||
gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
|
||||
gps.vel_ned_valid = true;
|
||||
|
||||
gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
|
||||
gps.vel_ned_valid = true;
|
||||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
|
||||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
||||
_gps_prev = gps;
|
||||
|
||||
} else {
|
||||
gps = _gps_prev;
|
||||
}
|
||||
|
||||
gps.timestamp = hrt_absolute_time();
|
||||
if (_failure_injection.handle_gps_failure(gps) && _failure_injection.handle_gps_alt_failure(gps)) {
|
||||
|
||||
// New publishers will be created based on the HIL_GPS ID's being different or not
|
||||
for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
|
||||
@@ -1056,7 +1035,7 @@ void SimulatorMavlink::send()
|
||||
if (fds_actuator_outputs[0].revents & POLLIN) {
|
||||
// Got new data to read, update all topics.
|
||||
parameters_update(false);
|
||||
check_failure_injections();
|
||||
_failure_injection.check_failure_injections();
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
_battery_status_sub.update(&_battery_status);
|
||||
|
||||
@@ -1244,279 +1223,6 @@ void SimulatorMavlink::run()
|
||||
}
|
||||
}
|
||||
|
||||
void SimulatorMavlink::check_failure_injections()
|
||||
{
|
||||
vehicle_command_s vehicle_command;
|
||||
|
||||
while (_vehicle_command_sub.update(&vehicle_command)) {
|
||||
if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool handled = false;
|
||||
bool supported = false;
|
||||
|
||||
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;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, GPS off");
|
||||
supported = true;
|
||||
_gps_blocked = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
|
||||
supported = true;
|
||||
_gps_blocked = false;
|
||||
_gps_stuck = false;
|
||||
_gps_wrong = false;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
|
||||
supported = true;
|
||||
_gps_stuck = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
|
||||
supported = true;
|
||||
_gps_wrong = true;
|
||||
}
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i);
|
||||
_accel_blocked[i] = true;
|
||||
_accel_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1);
|
||||
_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++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i);
|
||||
_accel_blocked[i] = false;
|
||||
_accel_stuck[i] = true;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1);
|
||||
_accel_blocked[instance - 1] = false;
|
||||
_accel_stuck[instance - 1] = true;
|
||||
}
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i);
|
||||
_accel_blocked[i] = false;
|
||||
_accel_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1);
|
||||
_accel_blocked[instance - 1] = false;
|
||||
_accel_stuck[instance - 1] = false;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i);
|
||||
_gyro_blocked[i] = true;
|
||||
_gyro_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1);
|
||||
_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++) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i);
|
||||
_gyro_blocked[i] = false;
|
||||
_gyro_stuck[i] = true;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1);
|
||||
_gyro_blocked[instance - 1] = false;
|
||||
_gyro_stuck[instance - 1] = true;
|
||||
}
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
supported = true;
|
||||
|
||||
// 0 to signal all
|
||||
if (instance == 0) {
|
||||
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i);
|
||||
_gyro_blocked[i] = false;
|
||||
_gyro_stuck[i] = false;
|
||||
}
|
||||
|
||||
} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1);
|
||||
_gyro_blocked[instance - 1] = false;
|
||||
_gyro_stuck[instance - 1] = false;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
supported = 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) {
|
||||
supported = true;
|
||||
|
||||
// 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) {
|
||||
supported = true;
|
||||
|
||||
// 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) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, baro off");
|
||||
supported = true;
|
||||
_baro_blocked = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, baro stuck");
|
||||
supported = true;
|
||||
_baro_stuck = true;
|
||||
_baro_blocked = false;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, baro ok");
|
||||
supported = true;
|
||||
_baro_blocked = false;
|
||||
}
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, airspeed off");
|
||||
supported = true;
|
||||
_airspeed_disconnected = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)");
|
||||
supported = true;
|
||||
_airspeed_blocked_timestamp = hrt_absolute_time();
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, airspeed ok");
|
||||
supported = true;
|
||||
_airspeed_disconnected = false;
|
||||
_airspeed_blocked_timestamp = 0;
|
||||
}
|
||||
|
||||
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) {
|
||||
handled = true;
|
||||
|
||||
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
|
||||
PX4_WARN("CMD_INJECT_FAILURE, vio off");
|
||||
supported = true;
|
||||
_vio_blocked = true;
|
||||
|
||||
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
|
||||
PX4_INFO("CMD_INJECT_FAILURE, vio ok");
|
||||
supported = true;
|
||||
_vio_blocked = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (handled) {
|
||||
vehicle_command_ack_s ack{};
|
||||
ack.command = vehicle_command.command;
|
||||
ack.from_external = false;
|
||||
ack.result = supported ?
|
||||
vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
|
||||
vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
ack.timestamp = hrt_absolute_time();
|
||||
_command_ack_pub.publish(ack);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
|
||||
{
|
||||
distance_sensor_s dist{};
|
||||
|
||||
@@ -77,6 +77,7 @@
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/rpm.h>
|
||||
#include <simulation/failure_injection/failureInjection.hpp>
|
||||
|
||||
#include <random>
|
||||
|
||||
@@ -293,10 +294,6 @@ private:
|
||||
bool _mag_blocked[MAG_COUNT_MAX] {};
|
||||
bool _mag_stuck[MAG_COUNT_MAX] {};
|
||||
|
||||
bool _gps_blocked{false};
|
||||
bool _gps_stuck{false};
|
||||
bool _gps_wrong{false};
|
||||
sensor_gps_s _gps_prev{};
|
||||
bool _airspeed_disconnected{false};
|
||||
hrt_abstime _airspeed_blocked_timestamp{0};
|
||||
bool _vio_blocked{false};
|
||||
@@ -308,6 +305,8 @@ private:
|
||||
float _last_baro_pressure{0.0f};
|
||||
float _last_baro_temperature{0.0f};
|
||||
|
||||
FailureInjection _failure_injection{};
|
||||
|
||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
@@ -57,6 +57,7 @@ static constexpr FailureUnit failure_units[] = {
|
||||
{ "mag", vehicle_command_s::FAILURE_UNIT_SENSOR_MAG},
|
||||
{ "baro", vehicle_command_s::FAILURE_UNIT_SENSOR_BARO},
|
||||
{ "gps", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS},
|
||||
{ "gps_alt", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT},
|
||||
{ "optical_flow", vehicle_command_s::FAILURE_UNIT_SENSOR_OPTICAL_FLOW},
|
||||
{ "vio", vehicle_command_s::FAILURE_UNIT_SENSOR_VIO},
|
||||
{ "distance_sensor", vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR},
|
||||
@@ -83,6 +84,7 @@ static constexpr FailureType failure_types[] = {
|
||||
{ "slow", vehicle_command_s::FAILURE_TYPE_SLOW},
|
||||
{ "delayed", vehicle_command_s::FAILURE_TYPE_DELAYED},
|
||||
{ "intermittent", vehicle_command_s::FAILURE_TYPE_INTERMITTENT},
|
||||
{ "drift", vehicle_command_s::FAILURE_TYPE_DRIFT},
|
||||
};
|
||||
|
||||
static void print_usage()
|
||||
|
||||
Reference in New Issue
Block a user