Skip to content

Commit

Permalink
Added public set armed and set flight mode interfaces
Browse files Browse the repository at this point in the history
  • Loading branch information
Jon Reeves committed Feb 19, 2025
1 parent 19ce85a commit a48b101
Show file tree
Hide file tree
Showing 11 changed files with 4,480 additions and 1,731 deletions.
38 changes: 38 additions & 0 deletions docs/en/cpp/api_reference/classmavsdk_1_1_action_server.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ void | [unsubscribe_terminate](#classmavsdk_1_1_action_server_1a3e236694f1f0beae
[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_disarmable](#classmavsdk_1_1_action_server_1afae1336100d7a91a4f4521cee56a1ecb) (bool disarmable, bool force_disarmable)const | Can the vehicle disarm when requested.
[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_allowable_flight_modes](#classmavsdk_1_1_action_server_1a3041d1b923a3b01021433ad43ab93b3a) ([AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) flight_modes)const | Set which modes the vehicle can transition to (Manual always allowed)
[ActionServer::AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) | [get_allowable_flight_modes](#classmavsdk_1_1_action_server_1a0960a6ec243823729a418a3c68feaf2a) () const | Get which modes the vehicle can transition to (Manual always allowed)
[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_armed_state](#classmavsdk_1_1_action_server_1a8830660884933029f104c49c31b7af24) (bool is_armed)const | Set/override the armed/disarmed state of the vehicle directly, and notify subscribers.
[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_flight_mode](#classmavsdk_1_1_action_server_1ac5ba6d26aef83881826361aa20a9bd65) ([FlightMode](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1aee12027f5d9380f2c13fa7813c6ae1d8) flight_mode)const | Set/override the flight mode of the vehicle directly, and notify subscribers.
const [ActionServer](classmavsdk_1_1_action_server.md) & | [operator=](#classmavsdk_1_1_action_server_1aa80e34dd72d2e31005085c78892bab8c) (const [ActionServer](classmavsdk_1_1_action_server.md) &)=delete | Equality operator (object is not copyable).


Expand Down Expand Up @@ -613,6 +615,42 @@ This function is blocking.

 [ActionServer::AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) - Result of request.

### set_armed_state() {#classmavsdk_1_1_action_server_1a8830660884933029f104c49c31b7af24}
```cpp
Result mavsdk::ActionServer::set_armed_state(bool is_armed) const
```
Set/override the armed/disarmed state of the vehicle directly, and notify subscribers.
This function is blocking.
**Parameters**
* bool **is_armed** -
**Returns**
 [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.
### set_flight_mode() {#classmavsdk_1_1_action_server_1ac5ba6d26aef83881826361aa20a9bd65}
```cpp
Result mavsdk::ActionServer::set_flight_mode(FlightMode flight_mode) const
```


Set/override the flight mode of the vehicle directly, and notify subscribers.

This function is blocking.

**Parameters**

* [FlightMode](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1aee12027f5d9380f2c13fa7813c6ae1d8) **flight_mode** -

**Returns**

 [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.

### operator=() {#classmavsdk_1_1_action_server_1aa80e34dd72d2e31005085c78892bab8c}
```cpp
const ActionServer & mavsdk::ActionServer::operator=(const ActionServer &)=delete
Expand Down
2 changes: 1 addition & 1 deletion proto
Submodule proto updated 1 files
+20 −0 protos/action_server/action_server.proto
10 changes: 10 additions & 0 deletions src/mavsdk/plugins/action_server/action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,16 @@ ActionServer::AllowableFlightModes ActionServer::get_allowable_flight_modes() co
return _impl->get_allowable_flight_modes();
}

ActionServer::Result ActionServer::set_armed_state(bool is_armed) const
{
return _impl->set_armed_state(is_armed);
}

ActionServer::Result ActionServer::set_flight_mode(FlightMode flight_mode) const
{
return _impl->set_flight_mode(flight_mode);
}

bool operator==(
const ActionServer::AllowableFlightModes& lhs, const ActionServer::AllowableFlightModes& rhs)
{
Expand Down
110 changes: 107 additions & 3 deletions src/mavsdk/plugins/action_server/action_server_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,16 @@
#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 telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)
ActionServer::FlightMode
ActionServerImpl::telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)
{
switch (flight_mode) {
case FlightMode::Ready:
Expand Down Expand Up @@ -43,6 +45,70 @@ ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode fligh
}
}

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)
{
Expand Down Expand Up @@ -138,7 +204,7 @@ void ActionServerImpl::init()

if (is_custom) {
// PX4 uses custom modes
px4_custom_mode px4_mode{};
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);
Expand Down Expand Up @@ -175,7 +241,7 @@ void ActionServerImpl::init()
}

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

if (allow_mode) {
Expand Down Expand Up @@ -333,6 +399,44 @@ ActionServer::AllowableFlightModes ActionServerImpl::get_allowable_flight_modes(
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);
Expand Down
17 changes: 7 additions & 10 deletions src/mavsdk/plugins/action_server/action_server_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,14 @@ class ActionServerImpl : public ServerPluginImplBase {

ActionServer::AllowableFlightModes get_allowable_flight_modes();

ActionServer::Result set_armed_state(bool armed);

ActionServer::Result set_flight_mode(ActionServer::FlightMode flight_mode);

private:
static ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode flight_mode);
static uint32_t to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode);

void set_base_mode(uint8_t base_mode);
uint8_t get_base_mode() const;

Expand All @@ -75,16 +82,6 @@ class ActionServerImpl : public ServerPluginImplBase {
std::atomic<bool> _force_disarmable = false;
std::atomic<bool> _allow_takeoff = false;

union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};

std::mutex _flight_mode_mutex;
ActionServer::AllowableFlightModes _allowed_flight_modes{};
std::atomic<ActionServer::FlightMode> _flight_mode{ActionServer::FlightMode::Unknown};
Expand Down
22 changes: 22 additions & 0 deletions src/mavsdk/plugins/action_server/include/plugins/action_server/action_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -350,6 +350,28 @@ class ActionServer : public ServerPluginBase {
*/
ActionServer::AllowableFlightModes get_allowable_flight_modes() const;

/**
* @brief Set/override the armed/disarmed state of the vehicle directly, and notify subscribers
*
* This function is blocking.
*
* @return Result of request.
*/
Result set_armed_state(bool is_armed) const;

/**
* @brief Set/override the flight mode of the vehicle directly, and notify subscribers
*
* This function is blocking.
*
* @return Result of request.
*/
Result set_flight_mode(FlightMode flight_mode) const;

/**
* @brief Copy constructor.
*/
Expand Down
Loading

0 comments on commit a48b101

Please sign in to comment.