16665088909

Committed 01 Aug 2025 02:55AM UTC coverage: 46.166% (-0.1%) from 46.31%

Build # 16665088909

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2630 from mavlink/pr-segfault-fixes

Stack-use-after-free and thread-safety fixes, CI additions, clang-format-19

Run Details

241 of 320 new or added lines in 32 files covered. (75.31%)

39 existing lines in 10 files now uncovered.

16101 of 34876 relevant lines covered (46.17%)

361.1 hits per line

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

18.27

/src/mavsdk/plugins/action_server/action_server_impl.cpp

#include "action_server_impl.h"
#include "unused.h"
#include "flight_mode.h"
#include "callback_list.tpp"
#include "px4_custom_mode.h"

namespace mavsdk {

template class CallbackList<ActionServer::Result, ActionServer::ArmDisarm>;
template class CallbackList<ActionServer::Result, ActionServer::FlightMode>;
template class CallbackList<ActionServer::Result, bool>;

ActionServer::FlightMode
ActionServerImpl::telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)
{
    switch (flight_mode) {
        case FlightMode::Ready:
            return ActionServer::FlightMode::Ready;
        case FlightMode::Takeoff:
            return ActionServer::FlightMode::Takeoff;
        case FlightMode::Hold:
            return ActionServer::FlightMode::Hold;
        case FlightMode::Mission:
            return ActionServer::FlightMode::Mission;
        case FlightMode::ReturnToLaunch:
            return ActionServer::FlightMode::ReturnToLaunch;
        case FlightMode::Land:
            return ActionServer::FlightMode::Land;
        case FlightMode::Offboard:
            return ActionServer::FlightMode::Offboard;
        case FlightMode::FollowMe:
            return ActionServer::FlightMode::FollowMe;
        case FlightMode::Manual:
            return ActionServer::FlightMode::Manual;
        case FlightMode::Posctl:
            return ActionServer::FlightMode::Posctl;
        case FlightMode::Altctl:
            return ActionServer::FlightMode::Altctl;
        case FlightMode::Acro:
            return ActionServer::FlightMode::Acro;
        case FlightMode::Stabilized:
            return ActionServer::FlightMode::Stabilized;
        default:
            return ActionServer::FlightMode::Unknown;
    }
}

uint32_t ActionServerImpl::to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode)
{
    px4::px4_custom_mode px4_mode;
    switch (flight_mode) {
        case ActionServer::FlightMode::Ready:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_READY;
            break;
        case ActionServer::FlightMode::Takeoff:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
            break;
        case ActionServer::FlightMode::Hold:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
            break;
        case ActionServer::FlightMode::Mission:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
            break;
        case ActionServer::FlightMode::ReturnToLaunch:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
            break;
        case ActionServer::FlightMode::Land:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
            break;
        case ActionServer::FlightMode::Offboard:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
            px4_mode.sub_mode = 0;
            break;
        case ActionServer::FlightMode::FollowMe:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
            break;
        case ActionServer::FlightMode::Manual:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
            px4_mode.sub_mode = 0;
            break;
        case ActionServer::FlightMode::Posctl:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
            px4_mode.sub_mode = 0;
            break;
        case ActionServer::FlightMode::Altctl:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
            px4_mode.sub_mode = 0;
            break;
        case ActionServer::FlightMode::Acro:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;
            px4_mode.sub_mode = 0;
            break;
        case ActionServer::FlightMode::Stabilized:
            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;
            px4_mode.sub_mode = 0;
            break;
        default:
            px4_mode.main_mode = 0; // unknown
            px4_mode.sub_mode = 0;
            break;
    }
    return px4_mode.data;
}

ActionServerImpl::ActionServerImpl(std::shared_ptr<ServerComponent> server_component) :
    ServerPluginImplBase(server_component)
{
    _server_component_impl->register_plugin(this);
}

ActionServerImpl::~ActionServerImpl()
{
    _server_component_impl->unregister_plugin(this);
}

void ActionServerImpl::init()
{
    _send_version_cookie = _server_component_impl->add_call_every(
        [this]() { _server_component_impl->send_autopilot_version(); }, 1.0);

    // Arming / Disarm / Kill
    _server_component_impl->register_mavlink_command_handler(
        MAV_CMD_COMPONENT_ARM_DISARM,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            ActionServer::ArmDisarm armDisarm{
                command.params.param1 == 1, command.params.param2 == 21196};

            std::lock_guard<std::mutex> lock(_callback_mutex);

            // Check arm states - Ugly.
            auto request_ack = MAV_RESULT_UNSUPPORTED;
            bool force = armDisarm.force;
            if (armDisarm.arm) {
                request_ack = (_armable || (force && _force_armable)) ?
                                  MAV_RESULT::MAV_RESULT_ACCEPTED :
                                  MAV_RESULT_TEMPORARILY_REJECTED;
            } else {
                request_ack = (_disarmable || (force && _force_disarmable)) ?
                                  MAV_RESULT::MAV_RESULT_ACCEPTED :
                                  MAV_RESULT_TEMPORARILY_REJECTED;
            }

            if (request_ack == MAV_RESULT::MAV_RESULT_ACCEPTED) {
                set_server_armed(armDisarm.arm);
            }

            auto result = (request_ack == MAV_RESULT::MAV_RESULT_ACCEPTED) ?
                              ActionServer::Result::Success :
                              ActionServer::Result::CommandDenied;

            _arm_disarm_callbacks.queue(result, armDisarm, [this](const auto& func) {
                _server_component_impl->call_user_callback(func);
            });

            return _server_component_impl->make_command_ack_message(command, request_ack);
        },
        this);

    _server_component_impl->register_mavlink_command_handler(
        MAV_CMD_NAV_TAKEOFF,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            std::lock_guard<std::mutex> lock(_callback_mutex);

            if (_allow_takeoff) {
                _takeoff_callbacks.queue(
                    ActionServer::Result::Success, true, [this](const auto& func) {
                        _server_component_impl->call_user_callback(func);
                    });

                return _server_component_impl->make_command_ack_message(
                    command, MAV_RESULT::MAV_RESULT_ACCEPTED);
            } else {
                _takeoff_callbacks.queue(
                    ActionServer::Result::CommandDenied, false, [this](const auto& func) {
                        _server_component_impl->call_user_callback(func);
                    });

                return _server_component_impl->make_command_ack_message(
                    command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
            }
        },
        this);

    // Flight mode
    _server_component_impl->register_mavlink_command_handler(
        MAV_CMD_DO_SET_MODE,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            auto base_mode = static_cast<uint8_t>(command.params.param1);
            auto is_custom = (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) ==
                             MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            ActionServer::FlightMode request_flight_mode = ActionServer::FlightMode::Unknown;

            auto custom_mode = static_cast<uint8_t>(command.params.param2);
            auto sub_custom_mode = static_cast<uint8_t>(command.params.param3);

            if (is_custom) {
                // PX4 uses custom modes
                px4::px4_custom_mode px4_mode{};
                px4_mode.main_mode = custom_mode;
                px4_mode.sub_mode = sub_custom_mode;
                auto system_flight_mode = to_flight_mode_from_px4_mode(px4_mode.data);
                request_flight_mode = telemetry_flight_mode_from_flight_mode(system_flight_mode);
            } else {
                // TO DO: non PX4 flight modes...
                // Just bug out now if not using PX4 modes
                std::lock_guard<std::mutex> callback_lock(_callback_mutex);
                _flight_mode_change_callbacks.queue(
                    ActionServer::Result::ParameterError,
                    request_flight_mode,
                    [this](const auto& func) { _server_component_impl->call_user_callback(func); });

                return _server_component_impl->make_command_ack_message(
                    command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
            }

            bool allow_mode = false;
            {
                std::lock_guard<std::mutex> flight_mode_lock(_flight_mode_mutex);
                switch (request_flight_mode) {
                    case ActionServer::FlightMode::Manual:
                        allow_mode = true;
                        break;
                    case ActionServer::FlightMode::Mission:
                        allow_mode = _allowed_flight_modes.can_auto_mode;
                        break;
                    case ActionServer::FlightMode::Stabilized:
                        allow_mode = _allowed_flight_modes.can_stabilize_mode;
                        break;
                    case ActionServer::FlightMode::Offboard:
                        allow_mode = _allowed_flight_modes.can_guided_mode;
                        break;
                    default:
                        allow_mode = false;
                        break;
                }
            }

            // PX4...
            px4::px4_custom_mode px4_mode{};
            px4_mode.data = get_custom_mode();

            if (allow_mode) {
                px4_mode.main_mode = custom_mode;
                px4_mode.sub_mode = sub_custom_mode;
                set_custom_mode(px4_mode.data);
            }

            {
                std::lock_guard<std::mutex> callback_lock(_callback_mutex);
                _flight_mode_change_callbacks.queue(
                    allow_mode ? ActionServer::Result::Success :
                                 ActionServer::Result::CommandDenied,
                    request_flight_mode,
                    [this](const auto& func) { _server_component_impl->call_user_callback(func); });
            }

            return _server_component_impl->make_command_ack_message(
                command,
                allow_mode ? MAV_RESULT::MAV_RESULT_ACCEPTED : MAV_RESULT_TEMPORARILY_REJECTED);
        },
        this);
}

void ActionServerImpl::deinit()
{
    _server_component_impl->unregister_all_mavlink_command_handlers(this);
    _server_component_impl->remove_call_every(_send_version_cookie);
    _server_component_impl->unregister_all_mavlink_command_handlers(this);
}

ActionServer::ArmDisarmHandle
ActionServerImpl::subscribe_arm_disarm(const ActionServer::ArmDisarmCallback& callback)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _arm_disarm_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_arm_disarm(ActionServer::ArmDisarmHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _arm_disarm_callbacks.unsubscribe(handle);
}

ActionServer::FlightModeChangeHandle ActionServerImpl::subscribe_flight_mode_change(
    const ActionServer::FlightModeChangeCallback& callback)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _flight_mode_change_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_flight_mode_change(ActionServer::FlightModeChangeHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _flight_mode_change_callbacks.unsubscribe(handle);
}

ActionServer::TakeoffHandle
ActionServerImpl::subscribe_takeoff(const ActionServer::TakeoffCallback& callback)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _takeoff_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_takeoff(ActionServer::TakeoffHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _takeoff_callbacks.unsubscribe(handle);
}

ActionServer::LandHandle
ActionServerImpl::subscribe_land(const ActionServer::LandCallback& callback)
{
    // TODO: implement
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _land_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_land(ActionServer::LandHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _land_callbacks.unsubscribe(handle);
}

ActionServer::RebootHandle
ActionServerImpl::subscribe_reboot(const ActionServer::RebootCallback& callback)
{
    // TODO: implement
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _reboot_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_reboot(ActionServer::RebootHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _reboot_callbacks.unsubscribe(handle);
}

ActionServer::ShutdownHandle
ActionServerImpl::subscribe_shutdown(const ActionServer::ShutdownCallback& callback)
{
    // TODO: implement
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _shutdown_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_shutdown(ActionServer::ShutdownHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _shutdown_callbacks.unsubscribe(handle);
}

ActionServer::TerminateHandle
ActionServerImpl::subscribe_terminate(const ActionServer::TerminateCallback& callback)
{
    // TODO: implement
    std::lock_guard<std::mutex> lock(_callback_mutex);
    return _terminate_callbacks.subscribe(callback);
}

void ActionServerImpl::unsubscribe_terminate(ActionServer::TerminateHandle handle)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    _terminate_callbacks.unsubscribe(handle);
}

ActionServer::Result ActionServerImpl::set_allow_takeoff(bool allow_takeoff)
{
    _allow_takeoff = allow_takeoff;
    return ActionServer::Result::Success;
}

ActionServer::Result ActionServerImpl::set_armable(bool armable, bool force_armable)
{
    std::lock_guard<std::mutex> lock(_flight_mode_mutex);
    _armable = armable;
    _force_armable = force_armable;
    return ActionServer::Result::Success;
}

ActionServer::Result ActionServerImpl::set_disarmable(bool disarmable, bool force_disarmable)
{
    std::lock_guard<std::mutex> lock(_flight_mode_mutex);
    _disarmable = disarmable;
    _force_disarmable = force_disarmable;
    return ActionServer::Result::Success;
}

ActionServer::Result
ActionServerImpl::set_allowable_flight_modes(ActionServer::AllowableFlightModes flight_modes)
{
    std::lock_guard<std::mutex> lock(_flight_mode_mutex);
    _allowed_flight_modes = flight_modes;
    return ActionServer::Result::Success;
}

ActionServer::AllowableFlightModes ActionServerImpl::get_allowable_flight_modes()
{
    std::lock_guard<std::mutex> lock(_flight_mode_mutex);
    return _allowed_flight_modes;
}

ActionServer::Result ActionServerImpl::set_armed_state(bool armed)
{
    std::lock_guard<std::mutex> lock(_callback_mutex);
    ActionServer::ArmDisarm arm_disarm{};

    arm_disarm.arm = armed;
    arm_disarm.force = true;
    set_server_armed(armed);
    _arm_disarm_callbacks.queue(
        ActionServer::Result::Success, arm_disarm, [this](const auto& func) {
            _server_component_impl->call_user_callback(func);
        });
    return ActionServer::Result::Success;
}

ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode flight_mode)
{
    // note: currently on receipt of MAV_CMD_DO_SET_MODE, we error out if the mode
    // is *not* PX4 custom. For symmetry we will also convert from FlightMode to
    // PX4 custom when set directly.
    std::lock_guard<std::mutex> lock(_callback_mutex);

    px4::px4_custom_mode px4_mode{};
    px4_mode.data = to_px4_mode_from_flight_mode(flight_mode);

    uint8_t base_mode = get_base_mode();
    base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
    set_base_mode(base_mode);
    set_custom_mode(px4_mode.data);

    _flight_mode_change_callbacks.queue(
        ActionServer::Result::Success, flight_mode, [this](const auto& func) {
            _server_component_impl->call_user_callback(func);
        });

    return ActionServer::Result::Success;
}

void ActionServerImpl::set_base_mode(uint8_t base_mode)
{
    _server_component_impl->set_base_mode(base_mode);
}

uint8_t ActionServerImpl::get_base_mode() const
{
    return _server_component_impl->get_base_mode();
}

void ActionServerImpl::set_custom_mode(uint32_t custom_mode)
{
    _server_component_impl->set_custom_mode(custom_mode);
}

uint32_t ActionServerImpl::get_custom_mode() const
{
    return _server_component_impl->get_custom_mode();
}

void ActionServerImpl::set_server_armed(bool armed)
{
    uint8_t base_mode = get_base_mode();
    if (armed) {
        base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    } else {
        base_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
    }
    set_base_mode(base_mode);
}

} // namespace mavsdk

1	#include "action_server_impl.h"
2	#include "unused.h"
3	#include "flight_mode.h"
4	#include "callback_list.tpp"
5	#include "px4_custom_mode.h"
6
7	namespace mavsdk {
8
9	template class CallbackList<ActionServer::Result, ActionServer::ArmDisarm>;
10	template class CallbackList<ActionServer::Result, ActionServer::FlightMode>;
11	template class CallbackList<ActionServer::Result, bool>;
12
13	ActionServer::FlightMode
14	ActionServerImpl::telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)	×
15	{
16	switch (flight_mode) {	×
17	case FlightMode::Ready:	×
18	return ActionServer::FlightMode::Ready;	×
19	case FlightMode::Takeoff:	×
20	return ActionServer::FlightMode::Takeoff;	×
21	case FlightMode::Hold:	×
22	return ActionServer::FlightMode::Hold;	×
23	case FlightMode::Mission:	×
24	return ActionServer::FlightMode::Mission;	×
25	case FlightMode::ReturnToLaunch:	×
26	return ActionServer::FlightMode::ReturnToLaunch;	×
27	case FlightMode::Land:	×
28	return ActionServer::FlightMode::Land;	×
29	case FlightMode::Offboard:	×
30	return ActionServer::FlightMode::Offboard;	×
31	case FlightMode::FollowMe:	×
32	return ActionServer::FlightMode::FollowMe;	×
33	case FlightMode::Manual:	×
34	return ActionServer::FlightMode::Manual;	×
35	case FlightMode::Posctl:	×
36	return ActionServer::FlightMode::Posctl;	×
37	case FlightMode::Altctl:	×
38	return ActionServer::FlightMode::Altctl;	×
39	case FlightMode::Acro:	×
40	return ActionServer::FlightMode::Acro;	×
41	case FlightMode::Stabilized:	×
42	return ActionServer::FlightMode::Stabilized;	×
43	default:	×
44	return ActionServer::FlightMode::Unknown;	×
45	}
46	}
47
48	uint32_t ActionServerImpl::to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode)	×
49	{
50	px4::px4_custom_mode px4_mode;
51	switch (flight_mode) {	×
52	case ActionServer::FlightMode::Ready:	×
53	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
54	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_READY;	×
55	break;	×
56	case ActionServer::FlightMode::Takeoff:	×
57	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
58	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;	×
59	break;	×
60	case ActionServer::FlightMode::Hold:	×
61	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
62	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;	×
63	break;	×
64	case ActionServer::FlightMode::Mission:	×
65	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
66	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;	×
67	break;	×
68	case ActionServer::FlightMode::ReturnToLaunch:	×
69	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
70	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;	×
71	break;	×
72	case ActionServer::FlightMode::Land:	×
73	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
74	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;	×
75	break;	×
76	case ActionServer::FlightMode::Offboard:	×
77	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;	×
78	px4_mode.sub_mode = 0;	×
79	break;	×
80	case ActionServer::FlightMode::FollowMe:	×
81	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
82	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;	×
83	break;	×
84	case ActionServer::FlightMode::Manual:	×
85	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;	×
86	px4_mode.sub_mode = 0;	×
87	break;	×
88	case ActionServer::FlightMode::Posctl:	×
89	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;	×
90	px4_mode.sub_mode = 0;	×
91	break;	×
92	case ActionServer::FlightMode::Altctl:	×
93	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;	×
94	px4_mode.sub_mode = 0;	×
95	break;	×
96	case ActionServer::FlightMode::Acro:	×
97	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;	×
98	px4_mode.sub_mode = 0;	×
99	break;	×
100	case ActionServer::FlightMode::Stabilized:	×
101	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;	×
102	px4_mode.sub_mode = 0;	×
103	break;	×
104	default:	×
105	px4_mode.main_mode = 0; // unknown	×
106	px4_mode.sub_mode = 0;	×
107	break;	×
108	}
109	return px4_mode.data;	×
110	}
111
112	ActionServerImpl::ActionServerImpl(std::shared_ptr<ServerComponent> server_component) :	1✔
113	ServerPluginImplBase(server_component)	1✔
114	{
115	_server_component_impl->register_plugin(this);	1✔
116	}	1✔
117
118	ActionServerImpl::~ActionServerImpl()	2✔
119	{
120	_server_component_impl->unregister_plugin(this);	1✔
121	}	2✔
122
123	void ActionServerImpl::init()	1✔
124	{
125	_send_version_cookie = _server_component_impl->add_call_every(	1✔
126	[this]() { _server_component_impl->send_autopilot_version(); }, 1.0);	1✔
127
128	// Arming / Disarm / Kill
129	_server_component_impl->register_mavlink_command_handler(	1✔
130	MAV_CMD_COMPONENT_ARM_DISARM,
131	[this](const MavlinkCommandReceiver::CommandLong& command) {	18✔
132	ActionServer::ArmDisarm armDisarm{
133	command.params.param1 == 1, command.params.param2 == 21196};	4✔
134
135	std::lock_guard<std::mutex> lock(_callback_mutex);	4✔
136
137	// Check arm states - Ugly.
138	auto request_ack = MAV_RESULT_UNSUPPORTED;	4✔
139	bool force = armDisarm.force;	4✔
140	if (armDisarm.arm) {	4✔
141	request_ack = (_armable \|\| (force && _force_armable)) ?	2✔
142	MAV_RESULT::MAV_RESULT_ACCEPTED :
143	MAV_RESULT_TEMPORARILY_REJECTED;
144	} else {
145	request_ack = (_disarmable \|\| (force && _force_disarmable)) ?	2✔
146	MAV_RESULT::MAV_RESULT_ACCEPTED :
147	MAV_RESULT_TEMPORARILY_REJECTED;
148	}
149
150	if (request_ack == MAV_RESULT::MAV_RESULT_ACCEPTED) {	4✔
151	set_server_armed(armDisarm.arm);	2✔
152	}
153
154	auto result = (request_ack == MAV_RESULT::MAV_RESULT_ACCEPTED) ?	4✔
155	ActionServer::Result::Success :
156	ActionServer::Result::CommandDenied;
157
158	_arm_disarm_callbacks.queue(result, armDisarm, [this](const auto& func) {	4✔
159	_server_component_impl->call_user_callback(func);	×
160	});	×
161
162	return _server_component_impl->make_command_ack_message(command, request_ack);	4✔
163	},	4✔
164	this);
165
166	_server_component_impl->register_mavlink_command_handler(	1✔
167	MAV_CMD_NAV_TAKEOFF,
168	[this](const MavlinkCommandReceiver::CommandLong& command) {	×
169	std::lock_guard<std::mutex> lock(_callback_mutex);	×
170
171	if (_allow_takeoff) {	×
172	_takeoff_callbacks.queue(	×
173	ActionServer::Result::Success, true, [this](const auto& func) {	×
174	_server_component_impl->call_user_callback(func);	×
175	});	×
176
177	return _server_component_impl->make_command_ack_message(	×
178	command, MAV_RESULT::MAV_RESULT_ACCEPTED);	×
179	} else {
180	_takeoff_callbacks.queue(	×
181	ActionServer::Result::CommandDenied, false, [this](const auto& func) {	×
182	_server_component_impl->call_user_callback(func);	×
183	});	×
184
185	return _server_component_impl->make_command_ack_message(	×
186	command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);	×
187	}
188	},	×
189	this);
190
191	// Flight mode
192	_server_component_impl->register_mavlink_command_handler(	1✔
193	MAV_CMD_DO_SET_MODE,
194	[this](const MavlinkCommandReceiver::CommandLong& command) {	×
195	auto base_mode = static_cast<uint8_t>(command.params.param1);	×
196	auto is_custom = (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) ==	×
197	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
198	ActionServer::FlightMode request_flight_mode = ActionServer::FlightMode::Unknown;	×
199
200	auto custom_mode = static_cast<uint8_t>(command.params.param2);	×
201	auto sub_custom_mode = static_cast<uint8_t>(command.params.param3);	×
202
203	if (is_custom) {	×
204	// PX4 uses custom modes
205	px4::px4_custom_mode px4_mode{};	×
206	px4_mode.main_mode = custom_mode;	×
207	px4_mode.sub_mode = sub_custom_mode;	×
208	auto system_flight_mode = to_flight_mode_from_px4_mode(px4_mode.data);	×
209	request_flight_mode = telemetry_flight_mode_from_flight_mode(system_flight_mode);	×
210	} else {
211	// TO DO: non PX4 flight modes...
212	// Just bug out now if not using PX4 modes
NEW 213	std::lock_guard<std::mutex> callback_lock(_callback_mutex);	×
UNCOV 214	_flight_mode_change_callbacks.queue(	×
215	ActionServer::Result::ParameterError,
216	request_flight_mode,
217	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
218
219	return _server_component_impl->make_command_ack_message(	×
220	command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);	×
UNCOV 221	}	×
222
223	bool allow_mode = false;	×
224	{
NEW 225	std::lock_guard<std::mutex> flight_mode_lock(_flight_mode_mutex);	×
NEW 226	switch (request_flight_mode) {	×
NEW 227	case ActionServer::FlightMode::Manual:	×
NEW 228	allow_mode = true;	×
NEW 229	break;	×
NEW 230	case ActionServer::FlightMode::Mission:	×
NEW 231	allow_mode = _allowed_flight_modes.can_auto_mode;	×
NEW 232	break;	×
NEW 233	case ActionServer::FlightMode::Stabilized:	×
NEW 234	allow_mode = _allowed_flight_modes.can_stabilize_mode;	×
NEW 235	break;	×
NEW 236	case ActionServer::FlightMode::Offboard:	×
NEW 237	allow_mode = _allowed_flight_modes.can_guided_mode;	×
NEW 238	break;	×
NEW 239	default:	×
NEW 240	allow_mode = false;	×
NEW 241	break;	×
242	}
UNCOV 243	}	×
244
245	// PX4...
246	px4::px4_custom_mode px4_mode{};	×
247	px4_mode.data = get_custom_mode();	×
248
249	if (allow_mode) {	×
250	px4_mode.main_mode = custom_mode;	×
251	px4_mode.sub_mode = sub_custom_mode;	×
252	set_custom_mode(px4_mode.data);	×
253	}
254
255	{
NEW 256	std::lock_guard<std::mutex> callback_lock(_callback_mutex);	×
NEW 257	_flight_mode_change_callbacks.queue(	×
258	allow_mode ? ActionServer::Result::Success :
259	ActionServer::Result::CommandDenied,
260	request_flight_mode,
NEW 261	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
NEW 262	}	×
263
264	return _server_component_impl->make_command_ack_message(	×
265	command,
266	allow_mode ? MAV_RESULT::MAV_RESULT_ACCEPTED : MAV_RESULT_TEMPORARILY_REJECTED);	×
267	},
268	this);
269	}	1✔
270
271	void ActionServerImpl::deinit()	1✔
272	{
273	_server_component_impl->unregister_all_mavlink_command_handlers(this);	1✔
274	_server_component_impl->remove_call_every(_send_version_cookie);	1✔
275	_server_component_impl->unregister_all_mavlink_command_handlers(this);	1✔
276	}	1✔
277
278	ActionServer::ArmDisarmHandle
279	ActionServerImpl::subscribe_arm_disarm(const ActionServer::ArmDisarmCallback& callback)	×
280	{
281	std::lock_guard<std::mutex> lock(_callback_mutex);	×
282	return _arm_disarm_callbacks.subscribe(callback);	×
283	}	×
284
285	void ActionServerImpl::unsubscribe_arm_disarm(ActionServer::ArmDisarmHandle handle)	×
286	{
287	std::lock_guard<std::mutex> lock(_callback_mutex);	×
288	_arm_disarm_callbacks.unsubscribe(handle);	×
289	}	×
290
291	ActionServer::FlightModeChangeHandle ActionServerImpl::subscribe_flight_mode_change(	×
292	const ActionServer::FlightModeChangeCallback& callback)
293	{
294	std::lock_guard<std::mutex> lock(_callback_mutex);	×
295	return _flight_mode_change_callbacks.subscribe(callback);	×
296	}	×
297
298	void ActionServerImpl::unsubscribe_flight_mode_change(ActionServer::FlightModeChangeHandle handle)	×
299	{
300	std::lock_guard<std::mutex> lock(_callback_mutex);	×
301	_flight_mode_change_callbacks.unsubscribe(handle);	×
302	}	×
303
304	ActionServer::TakeoffHandle
305	ActionServerImpl::subscribe_takeoff(const ActionServer::TakeoffCallback& callback)	×
306	{
307	std::lock_guard<std::mutex> lock(_callback_mutex);	×
308	return _takeoff_callbacks.subscribe(callback);	×
309	}	×
310
311	void ActionServerImpl::unsubscribe_takeoff(ActionServer::TakeoffHandle handle)	×
312	{
313	std::lock_guard<std::mutex> lock(_callback_mutex);	×
314	_takeoff_callbacks.unsubscribe(handle);	×
315	}	×
316
317	ActionServer::LandHandle
318	ActionServerImpl::subscribe_land(const ActionServer::LandCallback& callback)	×
319	{
320	// TODO: implement
321	std::lock_guard<std::mutex> lock(_callback_mutex);	×
322	return _land_callbacks.subscribe(callback);	×
323	}	×
324
325	void ActionServerImpl::unsubscribe_land(ActionServer::LandHandle handle)	×
326	{
327	std::lock_guard<std::mutex> lock(_callback_mutex);	×
328	_land_callbacks.unsubscribe(handle);	×
329	}	×
330
331	ActionServer::RebootHandle
332	ActionServerImpl::subscribe_reboot(const ActionServer::RebootCallback& callback)	×
333	{
334	// TODO: implement
335	std::lock_guard<std::mutex> lock(_callback_mutex);	×
336	return _reboot_callbacks.subscribe(callback);	×
337	}	×
338
339	void ActionServerImpl::unsubscribe_reboot(ActionServer::RebootHandle handle)	×
340	{
341	std::lock_guard<std::mutex> lock(_callback_mutex);	×
342	_reboot_callbacks.unsubscribe(handle);	×
343	}	×
344
345	ActionServer::ShutdownHandle
346	ActionServerImpl::subscribe_shutdown(const ActionServer::ShutdownCallback& callback)	×
347	{
348	// TODO: implement
349	std::lock_guard<std::mutex> lock(_callback_mutex);	×
350	return _shutdown_callbacks.subscribe(callback);	×
351	}	×
352
353	void ActionServerImpl::unsubscribe_shutdown(ActionServer::ShutdownHandle handle)	×
354	{
355	std::lock_guard<std::mutex> lock(_callback_mutex);	×
356	_shutdown_callbacks.unsubscribe(handle);	×
357	}	×
358
359	ActionServer::TerminateHandle
360	ActionServerImpl::subscribe_terminate(const ActionServer::TerminateCallback& callback)	×
361	{
362	// TODO: implement
363	std::lock_guard<std::mutex> lock(_callback_mutex);	×
364	return _terminate_callbacks.subscribe(callback);	×
365	}	×
366
367	void ActionServerImpl::unsubscribe_terminate(ActionServer::TerminateHandle handle)	×
368	{
369	std::lock_guard<std::mutex> lock(_callback_mutex);	×
370	_terminate_callbacks.unsubscribe(handle);	×
371	}	×
372
373	ActionServer::Result ActionServerImpl::set_allow_takeoff(bool allow_takeoff)	×
374	{
375	_allow_takeoff = allow_takeoff;	×
376	return ActionServer::Result::Success;	×
377	}
378
379	ActionServer::Result ActionServerImpl::set_armable(bool armable, bool force_armable)	1✔
380	{
381	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	1✔
382	_armable = armable;	1✔
383	_force_armable = force_armable;	1✔
384	return ActionServer::Result::Success;	2✔
385	}	1✔
386
387	ActionServer::Result ActionServerImpl::set_disarmable(bool disarmable, bool force_disarmable)	1✔
388	{
389	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	1✔
390	_disarmable = disarmable;	1✔
391	_force_disarmable = force_disarmable;	1✔
392	return ActionServer::Result::Success;	2✔
393	}	1✔
394
395	ActionServer::Result
396	ActionServerImpl::set_allowable_flight_modes(ActionServer::AllowableFlightModes flight_modes)	×
397	{
398	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	×
399	_allowed_flight_modes = flight_modes;	×
400	return ActionServer::Result::Success;	×
401	}	×
402
403	ActionServer::AllowableFlightModes ActionServerImpl::get_allowable_flight_modes()	×
404	{
405	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	×
406	return _allowed_flight_modes;	×
407	}	×
408
409	ActionServer::Result ActionServerImpl::set_armed_state(bool armed)	×
410	{
411	std::lock_guard<std::mutex> lock(_callback_mutex);	×
412	ActionServer::ArmDisarm arm_disarm{};	×
413
414	arm_disarm.arm = armed;	×
415	arm_disarm.force = true;	×
416	set_server_armed(armed);	×
417	_arm_disarm_callbacks.queue(	×
418	ActionServer::Result::Success, arm_disarm, [this](const auto& func) {	×
419	_server_component_impl->call_user_callback(func);	×
420	});	×
421	return ActionServer::Result::Success;	×
422	}	×
423
424	ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode flight_mode)	×
425	{
426	// note: currently on receipt of MAV_CMD_DO_SET_MODE, we error out if the mode
427	// is not PX4 custom. For symmetry we will also convert from FlightMode to
428	// PX4 custom when set directly.
429	std::lock_guard<std::mutex> lock(_callback_mutex);	×
430
431	px4::px4_custom_mode px4_mode{};	×
432	px4_mode.data = to_px4_mode_from_flight_mode(flight_mode);	×
433
434	uint8_t base_mode = get_base_mode();	×
435	base_mode \|= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;	×
436	set_base_mode(base_mode);	×
437	set_custom_mode(px4_mode.data);	×
438
439	_flight_mode_change_callbacks.queue(	×
440	ActionServer::Result::Success, flight_mode, [this](const auto& func) {	×
441	_server_component_impl->call_user_callback(func);	×
442	});	×
443
444	return ActionServer::Result::Success;	×
445	}	×
446
447	void ActionServerImpl::set_base_mode(uint8_t base_mode)	2✔
448	{
449	_server_component_impl->set_base_mode(base_mode);	2✔
450	}	2✔
451
452	uint8_t ActionServerImpl::get_base_mode() const	2✔
453	{
454	return _server_component_impl->get_base_mode();	2✔
455	}
456
457	void ActionServerImpl::set_custom_mode(uint32_t custom_mode)	×
458	{
459	_server_component_impl->set_custom_mode(custom_mode);	×
460	}	×
461
462	uint32_t ActionServerImpl::get_custom_mode() const	×
463	{
464	return _server_component_impl->get_custom_mode();	×
465	}
466
467	void ActionServerImpl::set_server_armed(bool armed)	2✔
468	{
469	uint8_t base_mode = get_base_mode();	2✔
470	if (armed) {	2✔
471	base_mode \|= MAV_MODE_FLAG_SAFETY_ARMED;	1✔
472	} else {
473	base_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;	1✔
474	}
475	set_base_mode(base_mode);	2✔
476	}	2✔
477
478	} // namespace mavsdk

mavlink / MAVSDK / 16665088909

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