13826010889

Committed 13 Mar 2025 02:51AM UTC coverage: 44.43% (-0.1%) from 44.565%

Build # 13826010889

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2512 from jonathanreeves/feature/public-set-armed

ActionServer: allow direct setting of arm/disarm state and vehicle flight mode

Run Details

0 of 92 new or added lines in 2 files covered. (0.0%)

8 existing lines in 3 files now uncovered.

14594 of 32847 relevant lines covered (44.43%)

282.8 hits per line

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

18.51

/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);

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

            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
                _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;
            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);
            }

            _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);
}

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
NEW 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
NEW 48	uint32_t ActionServerImpl::to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode)	×
49	{
50	px4::px4_custom_mode px4_mode;
NEW 51	switch (flight_mode) {	×
NEW 52	case ActionServer::FlightMode::Ready:	×
NEW 53	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 54	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_READY;	×
NEW 55	break;	×
NEW 56	case ActionServer::FlightMode::Takeoff:	×
NEW 57	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 58	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;	×
NEW 59	break;	×
NEW 60	case ActionServer::FlightMode::Hold:	×
NEW 61	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 62	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;	×
NEW 63	break;	×
NEW 64	case ActionServer::FlightMode::Mission:	×
NEW 65	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 66	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;	×
NEW 67	break;	×
NEW 68	case ActionServer::FlightMode::ReturnToLaunch:	×
NEW 69	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 70	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;	×
NEW 71	break;	×
NEW 72	case ActionServer::FlightMode::Land:	×
NEW 73	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 74	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;	×
NEW 75	break;	×
NEW 76	case ActionServer::FlightMode::Offboard:	×
NEW 77	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;	×
NEW 78	px4_mode.sub_mode = 0;	×
NEW 79	break;	×
NEW 80	case ActionServer::FlightMode::FollowMe:	×
NEW 81	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;	×
NEW 82	px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;	×
NEW 83	break;	×
NEW 84	case ActionServer::FlightMode::Manual:	×
NEW 85	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;	×
NEW 86	px4_mode.sub_mode = 0;	×
NEW 87	break;	×
NEW 88	case ActionServer::FlightMode::Posctl:	×
NEW 89	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;	×
NEW 90	px4_mode.sub_mode = 0;	×
NEW 91	break;	×
NEW 92	case ActionServer::FlightMode::Altctl:	×
NEW 93	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;	×
NEW 94	px4_mode.sub_mode = 0;	×
NEW 95	break;	×
NEW 96	case ActionServer::FlightMode::Acro:	×
NEW 97	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;	×
NEW 98	px4_mode.sub_mode = 0;	×
NEW 99	break;	×
NEW 100	case ActionServer::FlightMode::Stabilized:	×
NEW 101	px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;	×
NEW 102	px4_mode.sub_mode = 0;	×
NEW 103	break;	×
NEW 104	default:	×
NEW 105	px4_mode.main_mode = 0; // unknown	×
NEW 106	px4_mode.sub_mode = 0;	×
NEW 107	break;	×
108	}
NEW 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);	2✔
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) {	26✔
132	ActionServer::ArmDisarm armDisarm{	4✔
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	std::lock_guard<std::mutex> lock(_callback_mutex);	×
204
205	if (is_custom) {	×
206	// PX4 uses custom modes
NEW 207	px4::px4_custom_mode px4_mode{};	×
208	px4_mode.main_mode = custom_mode;	×
209	px4_mode.sub_mode = sub_custom_mode;	×
210	auto system_flight_mode = to_flight_mode_from_px4_mode(px4_mode.data);	×
211	request_flight_mode = telemetry_flight_mode_from_flight_mode(system_flight_mode);	×
212	} else {
213	// TO DO: non PX4 flight modes...
214	// Just bug out now if not using PX4 modes
215	_flight_mode_change_callbacks.queue(	×
216	ActionServer::Result::ParameterError,
217	request_flight_mode,
218	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
219
220	return _server_component_impl->make_command_ack_message(	×
221	command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);	×
222	}
223
224	bool allow_mode = false;	×
225	switch (request_flight_mode) {	×
226	case ActionServer::FlightMode::Manual:	×
227	allow_mode = true;	×
228	break;	×
229	case ActionServer::FlightMode::Mission:	×
230	allow_mode = _allowed_flight_modes.can_auto_mode;	×
231	break;	×
232	case ActionServer::FlightMode::Stabilized:	×
233	allow_mode = _allowed_flight_modes.can_stabilize_mode;	×
234	break;	×
235	case ActionServer::FlightMode::Offboard:	×
236	allow_mode = _allowed_flight_modes.can_guided_mode;	×
237	break;	×
238	default:	×
239	allow_mode = false;	×
240	break;	×
241	}
242
243	// PX4...
NEW 244	px4::px4_custom_mode px4_mode{};	×
245	px4_mode.data = get_custom_mode();	×
246
247	if (allow_mode) {	×
248	px4_mode.main_mode = custom_mode;	×
249	px4_mode.sub_mode = sub_custom_mode;	×
250	set_custom_mode(px4_mode.data);	×
251	}
252
253	_flight_mode_change_callbacks.queue(	×
254	allow_mode ? ActionServer::Result::Success : ActionServer::Result::CommandDenied,
255	request_flight_mode,
256	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
257
258	return _server_component_impl->make_command_ack_message(	×
259	command,
260	allow_mode ? MAV_RESULT::MAV_RESULT_ACCEPTED : MAV_RESULT_TEMPORARILY_REJECTED);	×
261	},	×
262	this);
263	}	1✔
264
265	void ActionServerImpl::deinit()	1✔
266	{
267	_server_component_impl->unregister_all_mavlink_command_handlers(this);	1✔
268	_server_component_impl->remove_call_every(_send_version_cookie);	1✔
269	}	1✔
270
271	ActionServer::ArmDisarmHandle
272	ActionServerImpl::subscribe_arm_disarm(const ActionServer::ArmDisarmCallback& callback)	×
273	{
274	std::lock_guard<std::mutex> lock(_callback_mutex);	×
275	return _arm_disarm_callbacks.subscribe(callback);	×
276	}	×
277
278	void ActionServerImpl::unsubscribe_arm_disarm(ActionServer::ArmDisarmHandle handle)	×
279	{
280	std::lock_guard<std::mutex> lock(_callback_mutex);	×
281	_arm_disarm_callbacks.unsubscribe(handle);	×
282	}	×
283
284	ActionServer::FlightModeChangeHandle ActionServerImpl::subscribe_flight_mode_change(	×
285	const ActionServer::FlightModeChangeCallback& callback)
286	{
287	std::lock_guard<std::mutex> lock(_callback_mutex);	×
288	return _flight_mode_change_callbacks.subscribe(callback);	×
289	}	×
290
291	void ActionServerImpl::unsubscribe_flight_mode_change(ActionServer::FlightModeChangeHandle handle)	×
292	{
293	std::lock_guard<std::mutex> lock(_callback_mutex);	×
294	_flight_mode_change_callbacks.unsubscribe(handle);	×
295	}	×
296
297	ActionServer::TakeoffHandle
298	ActionServerImpl::subscribe_takeoff(const ActionServer::TakeoffCallback& callback)	×
299	{
300	std::lock_guard<std::mutex> lock(_callback_mutex);	×
301	return _takeoff_callbacks.subscribe(callback);	×
302	}	×
303
304	void ActionServerImpl::unsubscribe_takeoff(ActionServer::TakeoffHandle handle)	×
305	{
306	std::lock_guard<std::mutex> lock(_callback_mutex);	×
307	_takeoff_callbacks.unsubscribe(handle);	×
308	}	×
309
310	ActionServer::LandHandle
311	ActionServerImpl::subscribe_land(const ActionServer::LandCallback& callback)	×
312	{
313	// TODO: implement
314	std::lock_guard<std::mutex> lock(_callback_mutex);	×
315	return _land_callbacks.subscribe(callback);	×
316	}	×
317
318	void ActionServerImpl::unsubscribe_land(ActionServer::LandHandle handle)	×
319	{
320	std::lock_guard<std::mutex> lock(_callback_mutex);	×
321	_land_callbacks.unsubscribe(handle);	×
322	}	×
323
324	ActionServer::RebootHandle
325	ActionServerImpl::subscribe_reboot(const ActionServer::RebootCallback& callback)	×
326	{
327	// TODO: implement
328	std::lock_guard<std::mutex> lock(_callback_mutex);	×
329	return _reboot_callbacks.subscribe(callback);	×
330	}	×
331
332	void ActionServerImpl::unsubscribe_reboot(ActionServer::RebootHandle handle)	×
333	{
334	std::lock_guard<std::mutex> lock(_callback_mutex);	×
335	_reboot_callbacks.unsubscribe(handle);	×
336	}	×
337
338	ActionServer::ShutdownHandle
339	ActionServerImpl::subscribe_shutdown(const ActionServer::ShutdownCallback& callback)	×
340	{
341	// TODO: implement
342	std::lock_guard<std::mutex> lock(_callback_mutex);	×
343	return _shutdown_callbacks.subscribe(callback);	×
344	}	×
345
346	void ActionServerImpl::unsubscribe_shutdown(ActionServer::ShutdownHandle handle)	×
347	{
348	std::lock_guard<std::mutex> lock(_callback_mutex);	×
349	_shutdown_callbacks.unsubscribe(handle);	×
350	}	×
351
352	ActionServer::TerminateHandle
353	ActionServerImpl::subscribe_terminate(const ActionServer::TerminateCallback& callback)	×
354	{
355	// TODO: implement
356	std::lock_guard<std::mutex> lock(_callback_mutex);	×
357	return _terminate_callbacks.subscribe(callback);	×
358	}	×
359
360	void ActionServerImpl::unsubscribe_terminate(ActionServer::TerminateHandle handle)	×
361	{
362	std::lock_guard<std::mutex> lock(_callback_mutex);	×
363	_terminate_callbacks.unsubscribe(handle);	×
364	}	×
365
366	ActionServer::Result ActionServerImpl::set_allow_takeoff(bool allow_takeoff)	×
367	{
368	_allow_takeoff = allow_takeoff;	×
369	return ActionServer::Result::Success;	×
370	}
371
372	ActionServer::Result ActionServerImpl::set_armable(bool armable, bool force_armable)	1✔
373	{
374	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	1✔
375	_armable = armable;	1✔
376	_force_armable = force_armable;	1✔
377	return ActionServer::Result::Success;	2✔
378	}	1✔
379
380	ActionServer::Result ActionServerImpl::set_disarmable(bool disarmable, bool force_disarmable)	1✔
381	{
382	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	1✔
383	_disarmable = disarmable;	1✔
384	_force_disarmable = force_disarmable;	1✔
385	return ActionServer::Result::Success;	2✔
386	}	1✔
387
388	ActionServer::Result
389	ActionServerImpl::set_allowable_flight_modes(ActionServer::AllowableFlightModes flight_modes)	×
390	{
391	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	×
392	_allowed_flight_modes = flight_modes;	×
393	return ActionServer::Result::Success;	×
394	}	×
395
396	ActionServer::AllowableFlightModes ActionServerImpl::get_allowable_flight_modes()	×
397	{
398	std::lock_guard<std::mutex> lock(_flight_mode_mutex);	×
399	return _allowed_flight_modes;	×
400	}	×
401
NEW 402	ActionServer::Result ActionServerImpl::set_armed_state(bool armed)	×
403	{
NEW 404	std::lock_guard<std::mutex> lock(_callback_mutex);	×
NEW 405	ActionServer::ArmDisarm arm_disarm{};	×
406
NEW 407	arm_disarm.arm = armed;	×
NEW 408	arm_disarm.force = true;	×
NEW 409	set_server_armed(armed);	×
NEW 410	_arm_disarm_callbacks.queue(	×
NEW 411	ActionServer::Result::Success, arm_disarm, [this](const auto& func) {	×
NEW 412	_server_component_impl->call_user_callback(func);	×
NEW 413	});	×
NEW 414	return ActionServer::Result::Success;	×
NEW 415	}	×
416
NEW 417	ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode flight_mode)	×
418	{
419	// note: currently on receipt of MAV_CMD_DO_SET_MODE, we error out if the mode
420	// is not PX4 custom. For symmetry we will also convert from FlightMode to
421	// PX4 custom when set directly.
NEW 422	std::lock_guard<std::mutex> lock(_callback_mutex);	×
423
NEW 424	px4::px4_custom_mode px4_mode{};	×
NEW 425	px4_mode.data = to_px4_mode_from_flight_mode(flight_mode);	×
426
NEW 427	uint8_t base_mode = get_base_mode();	×
NEW 428	base_mode \|= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;	×
NEW 429	set_base_mode(base_mode);	×
NEW 430	set_custom_mode(px4_mode.data);	×
431
NEW 432	_flight_mode_change_callbacks.queue(	×
NEW 433	ActionServer::Result::Success, flight_mode, [this](const auto& func) {	×
NEW 434	_server_component_impl->call_user_callback(func);	×
NEW 435	});	×
436
NEW 437	return ActionServer::Result::Success;	×
NEW 438	}	×
439
440	void ActionServerImpl::set_base_mode(uint8_t base_mode)	2✔
441	{
442	_server_component_impl->set_base_mode(base_mode);	2✔
443	}	2✔
444
445	uint8_t ActionServerImpl::get_base_mode() const	2✔
446	{
447	return _server_component_impl->get_base_mode();	2✔
448	}
449
450	void ActionServerImpl::set_custom_mode(uint32_t custom_mode)	×
451	{
452	_server_component_impl->set_custom_mode(custom_mode);	×
453	}	×
454
455	uint32_t ActionServerImpl::get_custom_mode() const	×
456	{
457	return _server_component_impl->get_custom_mode();	×
458	}
459
460	void ActionServerImpl::set_server_armed(bool armed)	2✔
461	{
462	uint8_t base_mode = get_base_mode();	2✔
463	if (armed) {	2✔
464	base_mode \|= MAV_MODE_FLAG_SAFETY_ARMED;	1✔
465	} else {
466	base_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;	1✔
467	}
468	set_base_mode(base_mode);	2✔
469	}	2✔
470
471	} // namespace mavsdk

mavlink / MAVSDK / 13826010889

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