20604212380

Committed 30 Dec 2025 07:24PM UTC coverage: 48.017% (-0.06%) from 48.078%

Build # 20604212380

Build Type

Pull #2746

github

Committed by

web-flow

Commit Message

Merge 5269e5f1e into 5d2947b34

Pull Request Pull Request #2746: Configuration and component cleanup

Run Details

47 of 158 new or added lines in 19 files covered. (29.75%)

21 existing lines in 9 files now uncovered.

17769 of 37006 relevant lines covered (48.02%)

483.39 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/failure/failure_impl.cpp

#include "failure_impl.h"
#include "autopilot.h"

namespace mavsdk {

FailureImpl::FailureImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

FailureImpl::FailureImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

FailureImpl::~FailureImpl()
{
    _system_impl->unregister_plugin(this);
}

void FailureImpl::init() {}

void FailureImpl::deinit() {}

void FailureImpl::enable()
{
    // Only PX4 implements the SYS_FAILURE_EN param.
    if (_system_impl->effective_autopilot() != Autopilot::Px4) {
        _enabled = EnabledState::Unknown;
        return;
    }

    constexpr auto param_name = "SYS_FAILURE_EN";

    _system_impl->get_param_int_async(
        param_name,
        [this](MavlinkParameterClient::Result result, int32_t value) {
            if (result == MavlinkParameterClient::Result::Success) {
                if (value == 1) {
                    _enabled = EnabledState::Enabled;
                } else if (value == 0) {
                    _enabled = EnabledState::Disabled;
                } else {
                    _enabled = EnabledState::Unknown;
                }
            } else {
                _enabled = EnabledState::Unknown;
            }
        },
        this);

    _system_impl->subscribe_param_int(
        param_name,
        [this](int value) {
            if (value == 1) {
                _enabled = EnabledState::Enabled;
            } else if (value == 0) {
                _enabled = EnabledState::Disabled;
            } else {
                _enabled = EnabledState::Unknown;
            }
        },
        this);
}

void FailureImpl::disable()
{
    _enabled = EnabledState::Init;
}

Failure::Result FailureImpl::inject(
    Failure::FailureUnit failure_unit, Failure::FailureType failure_type, int32_t instance)
{
    while (_enabled == EnabledState::Init) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // If the param is unknown we ignore it and try anyway.
    if (_enabled == EnabledState::Disabled) {
        return Failure::Result::Disabled;
    }

    MavlinkCommandSender::CommandLong command{};

    command.command = MAV_CMD_INJECT_FAILURE;
    command.params.maybe_param1 = failure_unit_to_mavlink_enum(failure_unit);
    command.params.maybe_param2 = failure_type_to_mavlink_enum(failure_type);
    command.params.maybe_param3 = static_cast<float>(instance);
    command.target_component_id = _system_impl->get_autopilot_id();

    return failure_result_from_command_result(_system_impl->send_command(command));
}

float FailureImpl::failure_unit_to_mavlink_enum(const Failure::FailureUnit& failure_unit)
{
    switch (failure_unit) {
        case Failure::FailureUnit::SensorGyro:
            return FAILURE_UNIT_SENSOR_GYRO;
        case Failure::FailureUnit::SensorAccel:
            return FAILURE_UNIT_SENSOR_ACCEL;
        case Failure::FailureUnit::SensorMag:
            return FAILURE_UNIT_SENSOR_MAG;
        case Failure::FailureUnit::SensorBaro:
            return FAILURE_UNIT_SENSOR_BARO;
        case Failure::FailureUnit::SensorGps:
            return FAILURE_UNIT_SENSOR_GPS;
        case Failure::FailureUnit::SensorOpticalFlow:
            return FAILURE_UNIT_SENSOR_OPTICAL_FLOW;
        case Failure::FailureUnit::SensorVio:
            return FAILURE_UNIT_SENSOR_VIO;
        case Failure::FailureUnit::SensorDistanceSensor:
            return FAILURE_UNIT_SENSOR_DISTANCE_SENSOR;
        case Failure::FailureUnit::SensorAirspeed:
            return FAILURE_UNIT_SENSOR_AIRSPEED;
        case Failure::FailureUnit::SystemBattery:
            return FAILURE_UNIT_SYSTEM_BATTERY;
        case Failure::FailureUnit::SystemMotor:
            return FAILURE_UNIT_SYSTEM_MOTOR;
        case Failure::FailureUnit::SystemServo:
            return FAILURE_UNIT_SYSTEM_SERVO;
        case Failure::FailureUnit::SystemAvoidance:
            return FAILURE_UNIT_SYSTEM_AVOIDANCE;
        case Failure::FailureUnit::SystemRcSignal:
            return FAILURE_UNIT_SYSTEM_RC_SIGNAL;
        case Failure::FailureUnit::SystemMavlinkSignal:
            return FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL;
        default:
            return -1;
    }
}

float FailureImpl::failure_type_to_mavlink_enum(const Failure::FailureType& failure_type)
{
    switch (failure_type) {
        case Failure::FailureType::Ok:
            return FAILURE_TYPE_OK;
        case Failure::FailureType::Off:
            return FAILURE_TYPE_OFF;
        case Failure::FailureType::Stuck:
            return FAILURE_TYPE_STUCK;
        case Failure::FailureType::Garbage:
            return FAILURE_TYPE_GARBAGE;
        case Failure::FailureType::Wrong:
            return FAILURE_TYPE_WRONG;
        case Failure::FailureType::Slow:
            return FAILURE_TYPE_SLOW;
        case Failure::FailureType::Delayed:
            return FAILURE_TYPE_DELAYED;
        case Failure::FailureType::Intermittent:
            return FAILURE_TYPE_INTERMITTENT;
        default:
            return -1;
    }
}

Failure::Result
FailureImpl::failure_result_from_command_result(MavlinkCommandSender::Result command_result)
{
    switch (command_result) {
        case MavlinkCommandSender::Result::Success:
            return Failure::Result::Success;
        case MavlinkCommandSender::Result::NoSystem:
            return Failure::Result::NoSystem;
        case MavlinkCommandSender::Result::ConnectionError:
            return Failure::Result::ConnectionError;
        case MavlinkCommandSender::Result::Denied:
            // Fallthrough
        case MavlinkCommandSender::Result::TemporarilyRejected:
            return Failure::Result::Denied;
        case MavlinkCommandSender::Result::Unsupported:
            return Failure::Result::Unsupported;
        case MavlinkCommandSender::Result::Timeout:
            return Failure::Result::Timeout;
        default:
            return Failure::Result::Unknown;
    }
}

} // namespace mavsdk

1	#include "failure_impl.h"
2	#include "autopilot.h"
3
4	namespace mavsdk {
5
6	FailureImpl::FailureImpl(System& system) : PluginImplBase(system)	×
7	{
8	_system_impl->register_plugin(this);	×
9	}	×
10
11	FailureImpl::FailureImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
12	{
13	_system_impl->register_plugin(this);	×
14	}	×
15
16	FailureImpl::~FailureImpl()	×
17	{
18	_system_impl->unregister_plugin(this);	×
19	}	×
20
21	void FailureImpl::init() {}	×
22
23	void FailureImpl::deinit() {}	×
24
25	void FailureImpl::enable()	×
26	{
27	// Only PX4 implements the SYS_FAILURE_EN param.
NEW 28	if (_system_impl->effective_autopilot() != Autopilot::Px4) {	×
NEW 29	_enabled = EnabledState::Unknown;	×
NEW 30	return;	×
31	}
32
UNCOV 33	constexpr auto param_name = "SYS_FAILURE_EN";	×
34
35	_system_impl->get_param_int_async(	×
36	param_name,
37	[this](MavlinkParameterClient::Result result, int32_t value) {	×
38	if (result == MavlinkParameterClient::Result::Success) {	×
39	if (value == 1) {	×
40	_enabled = EnabledState::Enabled;	×
41	} else if (value == 0) {	×
42	_enabled = EnabledState::Disabled;	×
43	} else {
44	_enabled = EnabledState::Unknown;	×
45	}
46	} else {
47	_enabled = EnabledState::Unknown;	×
48	}
49	},	×
50	this);
51
52	_system_impl->subscribe_param_int(	×
53	param_name,
54	[this](int value) {	×
55	if (value == 1) {	×
56	_enabled = EnabledState::Enabled;	×
57	} else if (value == 0) {	×
58	_enabled = EnabledState::Disabled;	×
59	} else {
60	_enabled = EnabledState::Unknown;	×
61	}
62	},	×
63	this);
64	}
65
66	void FailureImpl::disable()	×
67	{
68	_enabled = EnabledState::Init;	×
69	}	×
70
71	Failure::Result FailureImpl::inject(	×
72	Failure::FailureUnit failure_unit, Failure::FailureType failure_type, int32_t instance)
73	{
74	while (_enabled == EnabledState::Init) {	×
75	std::this_thread::sleep_for(std::chrono::milliseconds(10));	×
76	}
77
78	// If the param is unknown we ignore it and try anyway.
79	if (_enabled == EnabledState::Disabled) {	×
80	return Failure::Result::Disabled;	×
81	}
82
83	MavlinkCommandSender::CommandLong command{};	×
84
85	command.command = MAV_CMD_INJECT_FAILURE;	×
86	command.params.maybe_param1 = failure_unit_to_mavlink_enum(failure_unit);	×
87	command.params.maybe_param2 = failure_type_to_mavlink_enum(failure_type);	×
88	command.params.maybe_param3 = static_cast<float>(instance);	×
89	command.target_component_id = _system_impl->get_autopilot_id();	×
90
91	return failure_result_from_command_result(_system_impl->send_command(command));	×
92	}
93
94	float FailureImpl::failure_unit_to_mavlink_enum(const Failure::FailureUnit& failure_unit)	×
95	{
96	switch (failure_unit) {	×
97	case Failure::FailureUnit::SensorGyro:	×
98	return FAILURE_UNIT_SENSOR_GYRO;	×
99	case Failure::FailureUnit::SensorAccel:	×
100	return FAILURE_UNIT_SENSOR_ACCEL;	×
101	case Failure::FailureUnit::SensorMag:	×
102	return FAILURE_UNIT_SENSOR_MAG;	×
103	case Failure::FailureUnit::SensorBaro:	×
104	return FAILURE_UNIT_SENSOR_BARO;	×
105	case Failure::FailureUnit::SensorGps:	×
106	return FAILURE_UNIT_SENSOR_GPS;	×
107	case Failure::FailureUnit::SensorOpticalFlow:	×
108	return FAILURE_UNIT_SENSOR_OPTICAL_FLOW;	×
109	case Failure::FailureUnit::SensorVio:	×
110	return FAILURE_UNIT_SENSOR_VIO;	×
111	case Failure::FailureUnit::SensorDistanceSensor:	×
112	return FAILURE_UNIT_SENSOR_DISTANCE_SENSOR;	×
113	case Failure::FailureUnit::SensorAirspeed:	×
114	return FAILURE_UNIT_SENSOR_AIRSPEED;	×
115	case Failure::FailureUnit::SystemBattery:	×
116	return FAILURE_UNIT_SYSTEM_BATTERY;	×
117	case Failure::FailureUnit::SystemMotor:	×
118	return FAILURE_UNIT_SYSTEM_MOTOR;	×
119	case Failure::FailureUnit::SystemServo:	×
120	return FAILURE_UNIT_SYSTEM_SERVO;	×
121	case Failure::FailureUnit::SystemAvoidance:	×
122	return FAILURE_UNIT_SYSTEM_AVOIDANCE;	×
123	case Failure::FailureUnit::SystemRcSignal:	×
124	return FAILURE_UNIT_SYSTEM_RC_SIGNAL;	×
125	case Failure::FailureUnit::SystemMavlinkSignal:	×
126	return FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL;	×
127	default:	×
128	return -1;	×
129	}
130	}
131
132	float FailureImpl::failure_type_to_mavlink_enum(const Failure::FailureType& failure_type)	×
133	{
134	switch (failure_type) {	×
135	case Failure::FailureType::Ok:	×
136	return FAILURE_TYPE_OK;	×
137	case Failure::FailureType::Off:	×
138	return FAILURE_TYPE_OFF;	×
139	case Failure::FailureType::Stuck:	×
140	return FAILURE_TYPE_STUCK;	×
141	case Failure::FailureType::Garbage:	×
142	return FAILURE_TYPE_GARBAGE;	×
143	case Failure::FailureType::Wrong:	×
144	return FAILURE_TYPE_WRONG;	×
145	case Failure::FailureType::Slow:	×
146	return FAILURE_TYPE_SLOW;	×
147	case Failure::FailureType::Delayed:	×
148	return FAILURE_TYPE_DELAYED;	×
149	case Failure::FailureType::Intermittent:	×
150	return FAILURE_TYPE_INTERMITTENT;	×
151	default:	×
152	return -1;	×
153	}
154	}
155
156	Failure::Result
157	FailureImpl::failure_result_from_command_result(MavlinkCommandSender::Result command_result)	×
158	{
159	switch (command_result) {	×
160	case MavlinkCommandSender::Result::Success:	×
161	return Failure::Result::Success;	×
162	case MavlinkCommandSender::Result::NoSystem:	×
163	return Failure::Result::NoSystem;	×
164	case MavlinkCommandSender::Result::ConnectionError:	×
165	return Failure::Result::ConnectionError;	×
166	case MavlinkCommandSender::Result::Denied:	×
167	// Fallthrough
168	case MavlinkCommandSender::Result::TemporarilyRejected:
169	return Failure::Result::Denied;	×
170	case MavlinkCommandSender::Result::Unsupported:	×
171	return Failure::Result::Unsupported;	×
172	case MavlinkCommandSender::Result::Timeout:	×
173	return Failure::Result::Timeout;	×
174	default:	×
175	return Failure::Result::Unknown;	×
176	}
177	}
178
179	} // namespace mavsdk

mavlink / MAVSDK / 20604212380

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous