From 88deb2c0d7eeea090dcfa03c9ef37f172b521a74 Mon Sep 17 00:00:00 2001 From: Ilia-Loginov Date: Fri, 5 Apr 2024 18:01:36 +0400 Subject: [PATCH] fix the code style --- .../failure_detector_HITL.cpp | 69 ++++++++++++------- .../failure_detector_HITL.hpp | 49 +++++++------ src/modules/sensors/sensors.cpp | 23 +++---- 3 files changed, 82 insertions(+), 59 deletions(-) diff --git a/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp b/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp index 1420969c43bf..2412e94982b5 100644 --- a/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp +++ b/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp @@ -41,16 +41,18 @@ #include -FailureDetectorHITL::FailureDetectorHITL(bool hil_enabled) { +FailureDetectorHITL::FailureDetectorHITL(bool hil_enabled) +{ - if(!hil_enabled) { + if (!hil_enabled) { _vehicle_command_sub.unsubscribe(); - } - else { + + } else { PX4_INFO("RUN FailureDetectorHITL"); } } -bool FailureDetectorHITL::update() { +bool FailureDetectorHITL::update() +{ vehicle_command_s vehicle_command{}; bool update = false; @@ -66,66 +68,72 @@ bool FailureDetectorHITL::update() { const int failure_type = static_cast(vehicle_command.param2 + 0.5f); PX4_INFO("Failure detector caught new injection"); - switch (failure_unit) - { + + switch (failure_unit) { case vehicle_command_s::FAILURE_UNIT_SENSOR_GPS: #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) handled = true; + if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { PX4_INFO("CMD_INJECT_FAILURE, gps ok"); _gps = FailureStatus::ok; supported = true; - } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { PX4_WARN("CMD_INJECT_FAILURE, gps off"); supported = true; _gps = FailureStatus::off; - } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { PX4_WARN("CMD_INJECT_FAILURE, gps stuck"); _gps = FailureStatus::stuck; supported = true; } + #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION break; case vehicle_command_s::FAILURE_UNIT_SENSOR_BARO: #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) handled = true; + if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { PX4_INFO("CMD_INJECT_FAILURE, baro ok"); _baro = FailureStatus::ok; supported = true; - } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { PX4_WARN("CMD_INJECT_FAILURE, baro off"); _baro = FailureStatus::off; supported = true; - } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { PX4_WARN("CMD_INJECT_FAILURE, baro stuck"); _baro = FailureStatus::stuck; supported = true; } + #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA break; case vehicle_command_s::FAILURE_UNIT_SENSOR_MAG: #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) handled = true; + if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { PX4_INFO("CMD_INJECT_FAILURE, mag ok"); _mag = FailureStatus::ok; supported = true; - } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { PX4_WARN("CMD_INJECT_FAILURE, mag off"); _mag = FailureStatus::off; supported = true; } + #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER break; + default: break; } @@ -135,43 +143,51 @@ bool FailureDetectorHITL::update() { 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; + 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); } + update |= supported; } + return update; } #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) -bool FailureDetectorHITL::isGpsBlocked() const { +bool FailureDetectorHITL::isGpsBlocked() const +{ return FailureStatus::ok != _gps; } -bool FailureDetectorHITL::isGpsStuck() const { +bool FailureDetectorHITL::isGpsStuck() const +{ return FailureStatus::stuck == _gps; } #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) -bool FailureDetectorHITL::isBaroBlocked() const { +bool FailureDetectorHITL::isBaroBlocked() const +{ return FailureStatus::ok != _baro; } -bool FailureDetectorHITL::isBaroStuck() const { +bool FailureDetectorHITL::isBaroStuck() const +{ return FailureStatus::stuck == _baro; } #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) -bool FailureDetectorHITL::isMagBlocked() const { +bool FailureDetectorHITL::isMagBlocked() const +{ return FailureStatus::ok != _mag; } #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER -FakeSensors::FakeSensors(bool hil_enabled) { +FakeSensors::FakeSensors(bool hil_enabled) +{ if (hil_enabled) { #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) _fake_baro_publisher.start(); @@ -183,7 +199,8 @@ FakeSensors::FakeSensors(bool hil_enabled) { } } -void FakeSensors::update(const FailureDetectorHITL& detector) { +void FakeSensors::update(const FailureDetectorHITL &detector) +{ #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) _fake_gps_publisher.setEnabled(detector.isGpsStuck()); #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION diff --git a/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp b/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp index 2302260f4b8d..75edd6d821f8 100644 --- a/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp +++ b/src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp @@ -99,15 +99,15 @@ class FailureDetectorHITL final uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - FailureStatus _gps{FailureStatus::ok}; + FailureStatus _gps {FailureStatus::ok}; #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - FailureStatus _baro{FailureStatus::ok}; + FailureStatus _baro {FailureStatus::ok}; #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - FailureStatus _mag{FailureStatus::ok}; + FailureStatus _mag {FailureStatus::ok}; #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER }; @@ -133,33 +133,39 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _sensor_pub(id), - _sensor_sub(id) { + _sensor_sub(id) + { _sensor_pub.advertise(); } - ~FakeStuckSensor() override { + ~FakeStuckSensor() override + { stop(); perf_free(_cycle_perf); } - bool start() { + bool start() + { ScheduleNow(); return true; } - void stop() { + void stop() + { Deinit(); } - void setEnabled(bool enable) { + void setEnabled(bool enable) + { _enable = enable; } private: - void Run() override { + void Run() override + { perf_begin(_cycle_perf); - while (_sensor_sub.update(&_last_output)){ - _first_init= true; + while (_sensor_sub.update(&_last_output)) { + _first_init = true; } if (_enable && _first_init) { @@ -171,13 +177,13 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem perf_end(_cycle_perf); } - uORB::Publication _sensor_pub{}; - uORB::Subscription _sensor_sub{}; + uORB::Publication _sensor_pub {}; + uORB::Subscription _sensor_sub {}; - bool _enable{}; - bool _first_init{}; /**< Flag indicating whether the sensor has been initialized for the first time. */ - sensorsData _last_output{}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + bool _enable {}; + bool _first_init {}; /**< Flag indicating whether the sensor has been initialized for the first time. */ + sensorsData _last_output {}; + perf_counter_t _cycle_perf {perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; }; } @@ -188,21 +194,22 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem * This class represents a collection of fake sensors used for simulation purposes. * It is expected to work only in HITL mode. */ -class FakeSensors final { +class FakeSensors final +{ public: FakeSensors(bool hil_enabled); /** * @brief Updates states of the fake sensors with data from the failure detector. */ - void update(const FailureDetectorHITL& detector); + void update(const FailureDetectorHITL &detector); private: #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - sensors::FakeStuckSensor _fake_baro_publisher{ORB_ID::vehicle_air_data}; + sensors::FakeStuckSensor _fake_baro_publisher {ORB_ID::vehicle_air_data}; #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - sensors::FakeStuckSensor _fake_gps_publisher{ORB_ID::vehicle_gps_position}; + sensors::FakeStuckSensor _fake_gps_publisher {ORB_ID::vehicle_gps_position}; #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION }; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8ab62640d305..7e73bc588473 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -161,19 +161,18 @@ bool Sensors::init() int Sensors::parameters_update() { - if (_hil_enabled) - { - #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - InitializeVehicleGPSPosition(); - #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + if (_hil_enabled) { +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + InitializeVehicleGPSPosition(); +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - InitializeVehicleAirData(); - #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + InitializeVehicleAirData(); +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - InitializeVehicleMagnetometer(); - #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + InitializeVehicleMagnetometer(); +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER _fakeSensors.update(_failureDetector); } @@ -650,7 +649,7 @@ void Sensors::Run() } } - if(_hil_enabled && _failureDetector.update()) { + if (_hil_enabled && _failureDetector.update()) { parameters_update(); }