6568658631

Committed 19 Oct 2023 01:31AM UTC coverage: 31.23% (+0.02%) from 31.215%

Build # 6568658631

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2155 from mavlink/pr-static-fixes

Threading fixes, MAVLink sequence number cleanup

Run Details

1386 of 1386 new or added lines in 46 files covered. (100.0%)

7906 of 25315 relevant lines covered (31.23%)

23.54 hits per line

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

0.0

/src/mavsdk/plugins/manual_control/manual_control_impl.cpp

#include "manual_control_impl.h"
#include "mavlink_address.h"
#include <future>

namespace mavsdk {

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

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

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

void ManualControlImpl::init() {}

void ManualControlImpl::deinit() {}

void ManualControlImpl::enable() {}

void ManualControlImpl::disable() {}

void ManualControlImpl::start_position_control_async(const ManualControl::ResultCallback callback)
{
    if (_input == Input::NotSet) {
        if (callback) {
            auto temp_callback = callback;
            _system_impl->call_user_callback(
                [temp_callback]() { temp_callback(ManualControl::Result::InputNotSet); });
        }
        return;
    }

    _system_impl->set_flight_mode_async(
        FlightMode::Posctl, [this, callback](MavlinkCommandSender::Result result, float) {
            command_result_callback(result, callback);
        });
}

ManualControl::Result ManualControlImpl::start_position_control()
{
    if (_input == Input::NotSet) {
        return ManualControl::Result::InputNotSet;
    }

    auto prom = std::promise<ManualControl::Result>();
    auto fut = prom.get_future();

    start_position_control_async([&prom](ManualControl::Result result) { prom.set_value(result); });

    return fut.get();
}

void ManualControlImpl::start_altitude_control_async(const ManualControl::ResultCallback callback)
{
    if (_input == Input::NotSet) {
        if (callback) {
            auto temp_callback = callback;
            _system_impl->call_user_callback(
                [temp_callback]() { temp_callback(ManualControl::Result::InputNotSet); });
        }
        return;
    }
    _system_impl->set_flight_mode_async(
        FlightMode::Altctl, [this, callback](MavlinkCommandSender::Result result, float) {
            command_result_callback(result, callback);
        });
}

ManualControl::Result ManualControlImpl::start_altitude_control()
{
    if (_input == Input::NotSet) {
        return ManualControl::Result::InputNotSet;
    }
    auto prom = std::promise<ManualControl::Result>();
    auto fut = prom.get_future();

    start_altitude_control_async([&prom](ManualControl::Result result) { prom.set_value(result); });

    return fut.get();
}

ManualControl::Result
ManualControlImpl::set_manual_control_input(float x, float y, float z, float r)
{
    if (x > 1.f || x < -1.f || y > 1.f || y < -1.f || z > 1.f || z < 0.f || r > 1.f || r < -1.f) {
        return ManualControl::Result::InputOutOfRange;
    }

    if (_input == Input::NotSet) {
        _input = Input::Set;
    }

    // No buttons/extensions supported yet.
    const uint16_t buttons = 0;
    const uint16_t buttons2 = 0;
    const uint8_t enabled_extensions = 0;
    const int16_t pitch_only_axis = 0;
    const int16_t roll_only_axis = 0;

    return _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_manual_control_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _system_impl->get_system_id(),
            static_cast<int16_t>(x * 1000),
            static_cast<int16_t>(y * 1000),
            static_cast<int16_t>(z * 1000),
            static_cast<int16_t>(r * 1000),
            buttons,
            buttons2,
            enabled_extensions,
            pitch_only_axis,
            roll_only_axis);
        return message;
    }) ?
               ManualControl::Result::Success :
               ManualControl::Result::ConnectionError;
}

ManualControl::Result
ManualControlImpl::manual_control_result_from_command_result(MavlinkCommandSender::Result result)
{
    switch (result) {
        case MavlinkCommandSender::Result::Success:
            return ManualControl::Result::Success;
        case MavlinkCommandSender::Result::NoSystem:
            return ManualControl::Result::NoSystem;
        case MavlinkCommandSender::Result::ConnectionError:
            return ManualControl::Result::ConnectionError;
        case MavlinkCommandSender::Result::Busy:
            return ManualControl::Result::Busy;
        case MavlinkCommandSender::Result::Denied:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::TemporarilyRejected:
            return ManualControl::Result::CommandDenied;
        case MavlinkCommandSender::Result::Timeout:
            return ManualControl::Result::Timeout;
        default:
            return ManualControl::Result::Unknown;
    }
}

void ManualControlImpl::command_result_callback(
    MavlinkCommandSender::Result command_result, const ManualControl::ResultCallback& callback)
{
    ManualControl::Result action_result = manual_control_result_from_command_result(command_result);

    if (callback) {
        auto temp_callback = callback;
        _system_impl->call_user_callback(
            [temp_callback, action_result]() { temp_callback(action_result); });
    }
}

} // namespace mavsdk

1	#include "manual_control_impl.h"
2	#include "mavlink_address.h"
3	#include <future>
4
5	namespace mavsdk {
6
7	ManualControlImpl::ManualControlImpl(System& system) : PluginImplBase(system)	×
8	{
9	_system_impl->register_plugin(this);	×
10	}	×
11
12	ManualControlImpl::ManualControlImpl(std::shared_ptr<System> system) :	×
13	PluginImplBase(std::move(system))	×
14	{
15	_system_impl->register_plugin(this);	×
16	}	×
17
18	ManualControlImpl::~ManualControlImpl()	×
19	{
20	_system_impl->unregister_plugin(this);	×
21	}	×
22
23	void ManualControlImpl::init() {}	×
24
25	void ManualControlImpl::deinit() {}	×
26
27	void ManualControlImpl::enable() {}	×
28
29	void ManualControlImpl::disable() {}	×
30
31	void ManualControlImpl::start_position_control_async(const ManualControl::ResultCallback callback)	×
32	{
33	if (_input == Input::NotSet) {	×
34	if (callback) {	×
35	auto temp_callback = callback;	×
36	_system_impl->call_user_callback(	×
37	[temp_callback]() { temp_callback(ManualControl::Result::InputNotSet); });
38	}
39	return;	×
40	}
41
42	_system_impl->set_flight_mode_async(	×
43	FlightMode::Posctl, [this, callback](MavlinkCommandSender::Result result, float) {	×
44	command_result_callback(result, callback);	×
45	});	×
46	}
47
48	ManualControl::Result ManualControlImpl::start_position_control()	×
49	{
50	if (_input == Input::NotSet) {	×
51	return ManualControl::Result::InputNotSet;	×
52	}
53
54	auto prom = std::promise<ManualControl::Result>();	×
55	auto fut = prom.get_future();	×
56
57	start_position_control_async([&prom](ManualControl::Result result) { prom.set_value(result); });	×
58
59	return fut.get();	×
60	}
61
62	void ManualControlImpl::start_altitude_control_async(const ManualControl::ResultCallback callback)	×
63	{
64	if (_input == Input::NotSet) {	×
65	if (callback) {	×
66	auto temp_callback = callback;	×
67	_system_impl->call_user_callback(	×
68	[temp_callback]() { temp_callback(ManualControl::Result::InputNotSet); });
69	}
70	return;	×
71	}
72	_system_impl->set_flight_mode_async(	×
73	FlightMode::Altctl, [this, callback](MavlinkCommandSender::Result result, float) {	×
74	command_result_callback(result, callback);	×
75	});	×
76	}
77
78	ManualControl::Result ManualControlImpl::start_altitude_control()	×
79	{
80	if (_input == Input::NotSet) {	×
81	return ManualControl::Result::InputNotSet;	×
82	}
83	auto prom = std::promise<ManualControl::Result>();	×
84	auto fut = prom.get_future();	×
85
86	start_altitude_control_async([&prom](ManualControl::Result result) { prom.set_value(result); });	×
87
88	return fut.get();	×
89	}
90
91	ManualControl::Result
92	ManualControlImpl::set_manual_control_input(float x, float y, float z, float r)	×
93	{
94	if (x > 1.f \|\| x < -1.f \|\| y > 1.f \|\| y < -1.f \|\| z > 1.f \|\| z < 0.f \|\| r > 1.f \|\| r < -1.f) {	×
95	return ManualControl::Result::InputOutOfRange;	×
96	}
97
98	if (_input == Input::NotSet) {	×
99	_input = Input::Set;	×
100	}
101
102	// No buttons/extensions supported yet.
103	const uint16_t buttons = 0;	×
104	const uint16_t buttons2 = 0;	×
105	const uint8_t enabled_extensions = 0;	×
106	const int16_t pitch_only_axis = 0;	×
107	const int16_t roll_only_axis = 0;	×
108
109	return _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
110	mavlink_message_t message;
111	mavlink_msg_manual_control_pack_chan(	×
112	mavlink_address.system_id,	×
113	mavlink_address.component_id,	×
114	channel,
115	&message,
116	_system_impl->get_system_id(),	×
117	static_cast<int16_t>(x * 1000),	×
118	static_cast<int16_t>(y * 1000),	×
119	static_cast<int16_t>(z * 1000),	×
120	static_cast<int16_t>(r * 1000),	×
121	buttons,
122	buttons2,
123	enabled_extensions,
124	pitch_only_axis,
125	roll_only_axis);
126	return message;	×
127	}) ?	×
128	ManualControl::Result::Success :
129	ManualControl::Result::ConnectionError;	×
130	}
131
132	ManualControl::Result
133	ManualControlImpl::manual_control_result_from_command_result(MavlinkCommandSender::Result result)	×
134	{
135	switch (result) {	×
136	case MavlinkCommandSender::Result::Success:	×
137	return ManualControl::Result::Success;	×
138	case MavlinkCommandSender::Result::NoSystem:	×
139	return ManualControl::Result::NoSystem;	×
140	case MavlinkCommandSender::Result::ConnectionError:	×
141	return ManualControl::Result::ConnectionError;	×
142	case MavlinkCommandSender::Result::Busy:	×
143	return ManualControl::Result::Busy;	×
144	case MavlinkCommandSender::Result::Denied:	×
145	// FALLTHROUGH
146	case MavlinkCommandSender::Result::TemporarilyRejected:
147	return ManualControl::Result::CommandDenied;	×
148	case MavlinkCommandSender::Result::Timeout:	×
149	return ManualControl::Result::Timeout;	×
150	default:	×
151	return ManualControl::Result::Unknown;	×
152	}
153	}
154
155	void ManualControlImpl::command_result_callback(	×
156	MavlinkCommandSender::Result command_result, const ManualControl::ResultCallback& callback)
157	{
158	ManualControl::Result action_result = manual_control_result_from_command_result(command_result);	×
159
160	if (callback) {	×
161	auto temp_callback = callback;	×
162	_system_impl->call_user_callback(	×
163	[temp_callback, action_result]() { temp_callback(action_result); });
164	}
165	}	×
166
167	} // namespace mavsdk

mavlink / MAVSDK / 6568658631

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