diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d6fd3c6b90..69b4108062 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 77fda598d1..804fe05407 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -72,6 +72,8 @@ #include #include #include +#include +#include #include @@ -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 _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationMulti *_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 _has_initialized {false}; #endif DEFINE_PARAMETERS( - (ParamBool) _param_sim_gps_block, - (ParamBool) _param_sim_accel_block, - (ParamBool) _param_sim_gyro_block, - (ParamBool) _param_sim_baro_block, - (ParamBool) _param_sim_mag_block, - (ParamBool) _param_sim_dpres_block, (ParamInt) _param_mav_type, (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 316f7a6c93..7d959bd974 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(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 = {}; diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c index a4474db109..4a6c211b06 100644 --- a/src/modules/simulator/simulator_params.c +++ b/src/modules/simulator/simulator_params.c @@ -38,63 +38,3 @@ * * @author Mohamed Abdelkader */ - -/** - * 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);