Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

simulator: use failure injection to block GPS

Browse Source 
This uses the new failure injection system using vehicle_commands
instead of specific parameters. For now it implements GPS only.
This commit is contained in:
Julian Oes
2020-06-09 14:20:08 +02:00
committed by Daniel Agar
parent 53b14233a2
commit a9c4f269bd
4 changed files with 122 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+1
View File
@@ -1115,6 +1115,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW:
case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE:
/* ignore commands that are handled by other parts of the system */
break;
src/modules/simulator/simulator.h
+14 -8
View File
@@ -72,6 +72,8 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <random>
@@ -145,7 +147,8 @@ private:
_instance = nullptr;
}
// class methods
void check_failure_injections();
int publish_flow_topic(const mavlink_hil_optical_flow_t *flow);
int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
@@ -176,6 +179,8 @@ private:
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::PublicationQueued<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::PublicationMulti<distance_sensor_s> *_dist_pubs[RANGE_FINDER_MAX_SENSORS] {};
uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {};
@@ -235,6 +240,7 @@ private:
actuator_outputs_s _actuator_outputs{};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
// hil map_ref data
struct map_projection_reference_s _hil_local_proj_ref {};
@@ -246,20 +252,20 @@ private:
float _hil_ref_alt{0.0f};
uint64_t _hil_ref_timestamp{0};
// uORB data containers
vehicle_status_s _vehicle_status{};
bool _accel_blocked{false};
bool _gyro_blocked{false};
bool _baro_blocked{false};
bool _mag_blocked{false};
bool _gps_blocked{false};
bool _airspeed_blocked{false};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
#endif
DEFINE_PARAMETERS(
(ParamBool<px4::params::SIM_GPS_BLOCK>) _param_sim_gps_block,
(ParamBool<px4::params::SIM_ACCEL_BLOCK>) _param_sim_accel_block,
(ParamBool<px4::params::SIM_GYRO_BLOCK>) _param_sim_gyro_block,
(ParamBool<px4::params::SIM_BARO_BLOCK>) _param_sim_baro_block,
(ParamBool<px4::params::SIM_MAG_BLOCK>) _param_sim_mag_block,
(ParamBool<px4::params::SIM_DPRES_BLOCK>) _param_sim_dpres_block,
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id
src/modules/simulator/simulator_mavlink.cpp
+107 -6
View File
@@ -234,31 +234,31 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
}
// accel
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
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);
}
// gyro
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
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);
}
// magnetometer
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {
_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
}
// baro
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) {
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
_px4_baro_0.update(time, sensors.abs_pressure);
_px4_baro_1.update(time, sensors.abs_pressure);
}
// differential pressure
if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_param_sim_dpres_block.get()) {
if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
differential_pressure_s report{};
report.timestamp = time;
report.temperature = temperature;
@@ -322,7 +322,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
mavlink_hil_gps_t hil_gps;
mavlink_msg_hil_gps_decode(msg, &hil_gps);
if (!_param_sim_gps_block.get()) {
if (!_gps_blocked) {
vehicle_gps_position_s gps{};
gps.timestamp = hrt_absolute_time();
@@ -618,6 +618,7 @@ void Simulator::send()
if (fds_actuator_outputs[0].revents & POLLIN) {
// Got new data to read, update all topics.
parameters_update(false);
check_failure_injections();
_vehicle_status_sub.update(&_vehicle_status);
send_controls();
// Wait for other modules, such as logger or ekf2
@@ -924,6 +925,106 @@ int openUart(const char *uart_name, int baud)
}
#endif
void Simulator::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);
if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) {
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
supported = true;
_gps_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_gps_blocked = false;
}
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
supported = true;
_accel_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_accel_blocked = 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;
_gyro_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_gyro_blocked = 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;
_mag_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_mag_blocked = false;
}
} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) {
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
supported = true;
_baro_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_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) {
supported = true;
_airspeed_blocked = true;
} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
supported = true;
_airspeed_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_RESULT_ACCEPTED :
vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
_command_ack_pub.publish(ack);
}
}
}
int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
{
optical_flow_s flow = {};
src/modules/simulator/simulator_params.c
-60
View File
@@ -38,63 +38,3 @@
*
* @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
*/
/**
* Simulator block GPS data.
*
* Enable to block the publication of any incoming simulation GPS data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_GPS_BLOCK, 0);
/**
* Simulator block accelerometer data.
*
* Enable to block the publication of any incoming simulation accelerometer data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_ACCEL_BLOCK, 0);
/**
* Simulator block gyroscope data.
*
* Enable to block the publication of any incoming simulation gyroscope data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_GYRO_BLOCK, 0);
/**
* Simulator block magnetometer data.
*
* Enable to block the publication of any incoming simulation magnetometer data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_MAG_BLOCK, 0);
/**
* Simulator block barometer data.
*
* Enable to block the publication of any incoming simulation barometer data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_BARO_BLOCK, 0);
/**
* Simulator block differential pressure data.
*
* Enable to block the publication of any incoming simulation differential pressure data.
*
* @boolean
* @group SITL
*/
PARAM_DEFINE_INT32(SIM_DPRES_BLOCK, 0);