21342096616

Committed 26 Jan 2026 12:15AM UTC coverage: 49.09% (-0.1%) from 49.23%

Build # 21342096616

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2746 from mavlink/pr-autopilot-cleanup

Add MAVLink pure mode and config/component cleanup

Run Details

58 of 197 new or added lines in 19 files covered. (29.44%)

43 existing lines in 13 files now uncovered.

18310 of 37299 relevant lines covered (49.09%)

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

mavlink / MAVSDK / 21342096616

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