Skip to content

Commit

Permalink
fix the code style
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ilia-Loginov
Ilia-Loginov committed Apr 5, 2024
1 parent 420b31a commit 88deb2c
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 59 deletions.
69 changes: 43 additions & 26 deletions src/modules/sensors/failure_detector_HITL/failure_detector_HITL.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,16 +41,18 @@

#include <px4_platform_common/log.h>

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;

Expand All @@ -66,66 +68,72 @@ bool FailureDetectorHITL::update() {
const int failure_type = static_cast<int>(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;
}
Expand All @@ -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();
Expand All @@ -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
Expand Down
49 changes: 28 additions & 21 deletions src/modules/sensors/failure_detector_HITL/failure_detector_HITL.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,15 +99,15 @@ class FailureDetectorHITL final
uORB::Publication<vehicle_command_ack_s> _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
};

Expand All @@ -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) {
Expand All @@ -171,13 +177,13 @@ class FakeStuckSensor final: public ModuleParams, public px4::ScheduledWorkItem
perf_end(_cycle_perf);
}

uORB::Publication<sensorsData> _sensor_pub{};
uORB::Subscription _sensor_sub{};
uORB::Publication<sensorsData> _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")};
};

}
Expand All @@ -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<vehicle_air_data_s> _fake_baro_publisher{ORB_ID::vehicle_air_data};
sensors::FakeStuckSensor<vehicle_air_data_s> _fake_baro_publisher {ORB_ID::vehicle_air_data};
#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA

#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
sensors::FakeStuckSensor<sensor_gps_s> _fake_gps_publisher{ORB_ID::vehicle_gps_position};
sensors::FakeStuckSensor<sensor_gps_s> _fake_gps_publisher {ORB_ID::vehicle_gps_position};
#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
};
23 changes: 11 additions & 12 deletions src/modules/sensors/sensors.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -650,7 +649,7 @@ void Sensors::Run()
}
}

if(_hil_enabled && _failureDetector.update()) {
if (_hil_enabled && _failureDetector.update()) {
parameters_update();
}

Expand Down

0 comments on commit 88deb2c

Please sign in to comment.