4899817366

Build Type

github

Committed by GitHub

Commit Message

Merge c0bb90862 into e4e9c71dd

Pull Request Pull Request #1772: Refactor MAVLinkParameters into client and server classes

Run Details

2192 of 2192 new or added lines in 34 files covered. (100.0%)

7708 of 24812 relevant lines covered (31.07%)

19.96 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"

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()
{
    constexpr auto param_name = "SYS_FAILURE_EN";

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

mavlink / MAVSDK / 4899817366

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