Skip to content

Commit b84eae3

Browse files
jonathanreevesJon Reevesjulianoes
authored
Expanded capabilities for setting flight mode (#2761)
* Expand set_flight_mode capabilities including support for SET_MODE --------- Co-authored-by: Jon Reeves <[email protected]> Co-authored-by: Julian Oes <[email protected]>
1 parent 31d20d9 commit b84eae3

File tree

12 files changed

+1997
-259
lines changed

12 files changed

+1997
-259
lines changed

‎docs/en/cpp/api_reference/classmavsdk_1_1_action_server.md‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ void | [unsubscribe_terminate](#classmavsdk_1_1_action_server_1a3e236694f1f0beae
6666
[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)
6767
[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.
6868
[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.
69+
[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_flight_mode_internal](#classmavsdk_1_1_action_server_1a428bdf9077437da3300ef06487f4ee14) ([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 *do not* notify subscribers.
6970
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).
7071

7172

@@ -651,6 +652,24 @@ This function is blocking.
651652

652653
&emsp;[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.
653654

655+
### set_flight_mode_internal() {#classmavsdk_1_1_action_server_1a428bdf9077437da3300ef06487f4ee14}
656+
```cpp
657+
Result mavsdk::ActionServer::set_flight_mode_internal(FlightMode flight_mode) const
658+
```
659+
660+
661+
Set/override the flight mode of the vehicle directly, and *do not* notify subscribers.
662+
663+
This function is blocking.
664+
665+
**Parameters**
666+
667+
* [FlightMode](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1aee12027f5d9380f2c13fa7813c6ae1d8) **flight_mode** -
668+
669+
**Returns**
670+
671+
&emsp;[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.
672+
654673
### operator=() {#classmavsdk_1_1_action_server_1aa80e34dd72d2e31005085c78892bab8c}
655674
```cpp
656675
const ActionServer & mavsdk::ActionServer::operator=(const ActionServer &)=delete

‎docs/en/cpp/api_reference/structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,14 @@ bool [can_guided_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_mode
1616

1717
bool [can_stabilize_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a341661c5e1d018f3f4595f8cc9e67ed3) {} - Stabilize mode.
1818

19+
bool [can_auto_rtl_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a4f298ed0c035e1929aa5b1aab027df04) {} - Auto RTL mode.
20+
21+
bool [can_auto_takeoff_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a2591aa231cc7f545bb18bc386d285c51) {} - Auto takeoff mode.
22+
23+
bool [can_auto_land_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1aed9f3a1828b0798f2ddb2dfb21328aa9) {} - Auto land mode.
24+
25+
bool [can_auto_loiter_mode](#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a94c389da64a473d4c97cbb299669def2) {} - Auto hold/loiter mode.
26+
1927

2028
## Field Documentation
2129

@@ -49,3 +57,43 @@ bool mavsdk::ActionServer::AllowableFlightModes::can_stabilize_mode {}
4957

5058
Stabilize mode.
5159

60+
61+
### can_auto_rtl_mode {#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a4f298ed0c035e1929aa5b1aab027df04}
62+
63+
```cpp
64+
bool mavsdk::ActionServer::AllowableFlightModes::can_auto_rtl_mode {}
65+
```
66+
67+
68+
Auto RTL mode.
69+
70+
71+
### can_auto_takeoff_mode {#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a2591aa231cc7f545bb18bc386d285c51}
72+
73+
```cpp
74+
bool mavsdk::ActionServer::AllowableFlightModes::can_auto_takeoff_mode {}
75+
```
76+
77+
78+
Auto takeoff mode.
79+
80+
81+
### can_auto_land_mode {#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1aed9f3a1828b0798f2ddb2dfb21328aa9}
82+
83+
```cpp
84+
bool mavsdk::ActionServer::AllowableFlightModes::can_auto_land_mode {}
85+
```
86+
87+
88+
Auto land mode.
89+
90+
91+
### can_auto_loiter_mode {#structmavsdk_1_1_action_server_1_1_allowable_flight_modes_1a94c389da64a473d4c97cbb299669def2}
92+
93+
```cpp
94+
bool mavsdk::ActionServer::AllowableFlightModes::can_auto_loiter_mode {}
95+
```
96+
97+
98+
Auto hold/loiter mode.
99+

‎proto‎

‎src/mavsdk/plugins/action_server/action_server.cpp‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -127,12 +127,21 @@ ActionServer::Result ActionServer::set_flight_mode(FlightMode flight_mode) const
127127
return _impl->set_flight_mode(flight_mode);
128128
}
129129

130+
ActionServer::Result ActionServer::set_flight_mode_internal(FlightMode flight_mode) const
131+
{
132+
return _impl->set_flight_mode_internal(flight_mode);
133+
}
134+
130135
bool operator==(
131136
const ActionServer::AllowableFlightModes& lhs, const ActionServer::AllowableFlightModes& rhs)
132137
{
133138
return (rhs.can_auto_mode == lhs.can_auto_mode) &&
134139
(rhs.can_guided_mode == lhs.can_guided_mode) &&
135-
(rhs.can_stabilize_mode == lhs.can_stabilize_mode);
140+
(rhs.can_stabilize_mode == lhs.can_stabilize_mode) &&
141+
(rhs.can_auto_rtl_mode == lhs.can_auto_rtl_mode) &&
142+
(rhs.can_auto_takeoff_mode == lhs.can_auto_takeoff_mode) &&
143+
(rhs.can_auto_land_mode == lhs.can_auto_land_mode) &&
144+
(rhs.can_auto_loiter_mode == lhs.can_auto_loiter_mode);
136145
}
137146

138147
std::ostream&
@@ -143,6 +152,10 @@ operator<<(std::ostream& str, ActionServer::AllowableFlightModes const& allowabl
143152
str << " can_auto_mode: " << allowable_flight_modes.can_auto_mode << '\n';
144153
str << " can_guided_mode: " << allowable_flight_modes.can_guided_mode << '\n';
145154
str << " can_stabilize_mode: " << allowable_flight_modes.can_stabilize_mode << '\n';
155+
str << " can_auto_rtl_mode: " << allowable_flight_modes.can_auto_rtl_mode << '\n';
156+
str << " can_auto_takeoff_mode: " << allowable_flight_modes.can_auto_takeoff_mode << '\n';
157+
str << " can_auto_land_mode: " << allowable_flight_modes.can_auto_land_mode << '\n';
158+
str << " can_auto_loiter_mode: " << allowable_flight_modes.can_auto_loiter_mode << '\n';
146159
str << '}';
147160
return str;
148161
}

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

Lines changed: 115 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -188,7 +188,7 @@ void ActionServerImpl::init()
188188
},
189189
this);
190190

191-
// Flight mode
191+
// Flight mode (long)
192192
_server_component_impl->register_mavlink_command_handler(
193193
MAV_CMD_DO_SET_MODE,
194194
[this](const MavlinkCommandReceiver::CommandLong& command) {
@@ -230,6 +230,15 @@ void ActionServerImpl::init()
230230
case ActionServer::FlightMode::Mission:
231231
allow_mode = _allowed_flight_modes.can_auto_mode;
232232
break;
233+
case ActionServer::FlightMode::ReturnToLaunch:
234+
allow_mode = _allowed_flight_modes.can_auto_rtl_mode;
235+
break;
236+
case ActionServer::FlightMode::Takeoff:
237+
allow_mode = _allowed_flight_modes.can_auto_takeoff_mode;
238+
break;
239+
case ActionServer::FlightMode::Land:
240+
allow_mode = _allowed_flight_modes.can_auto_land_mode;
241+
break;
233242
case ActionServer::FlightMode::Stabilized:
234243
allow_mode = _allowed_flight_modes.can_stabilize_mode;
235244
break;
@@ -249,6 +258,9 @@ void ActionServerImpl::init()
249258
if (allow_mode) {
250259
px4_mode.main_mode = custom_mode;
251260
px4_mode.sub_mode = sub_custom_mode;
261+
uint8_t system_base_mode = get_base_mode();
262+
system_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
263+
set_base_mode(system_base_mode);
252264
set_custom_mode(px4_mode.data);
253265
}
254266

@@ -266,6 +278,96 @@ void ActionServerImpl::init()
266278
allow_mode ? MAV_RESULT::MAV_RESULT_ACCEPTED : MAV_RESULT_TEMPORARILY_REJECTED);
267279
},
268280
this);
281+
282+
// Flight mode (legacy int, standard for PX4)
283+
_server_component_impl->register_mavlink_message_handler(
284+
MAVLINK_MSG_ID_SET_MODE,
285+
[this](const mavlink_message_t& message) {
286+
mavlink_set_mode_t set_mode;
287+
mavlink_msg_set_mode_decode(&message, &set_mode);
288+
289+
auto base_mode = set_mode.base_mode;
290+
auto is_custom = (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) ==
291+
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
292+
ActionServer::FlightMode request_flight_mode = ActionServer::FlightMode::Unknown;
293+
294+
std::lock_guard<std::mutex> lock(_callback_mutex);
295+
296+
if (is_custom) {
297+
// for now, custom mode is assumed to be PX4
298+
auto system_flight_mode = to_flight_mode_from_px4_mode(set_mode.custom_mode);
299+
request_flight_mode = telemetry_flight_mode_from_flight_mode(system_flight_mode);
300+
} else {
301+
// TO DO: non PX4 flight modes...
302+
// Just bug out now if not using PX4 modes
303+
_flight_mode_change_callbacks.queue(
304+
ActionServer::Result::ParameterError,
305+
request_flight_mode,
306+
[this](const auto& func) { _server_component_impl->call_user_callback(func); });
307+
308+
// make_command_ack_message
309+
mavlink_command_ack_t command_ack{};
310+
command_ack.command = MAVLINK_MSG_ID_SET_MODE;
311+
command_ack.result = MAV_RESULT::MAV_RESULT_UNSUPPORTED;
312+
command_ack.progress = std::numeric_limits<uint8_t>::max();
313+
command_ack.result_param2 = 0;
314+
command_ack.target_system = message.sysid;
315+
command_ack.target_component = message.compid;
316+
_server_component_impl->send_command_ack(command_ack);
317+
}
318+
319+
bool allow_mode = false;
320+
switch (request_flight_mode) {
321+
case ActionServer::FlightMode::Manual:
322+
allow_mode = true;
323+
break;
324+
case ActionServer::FlightMode::Mission:
325+
allow_mode = _allowed_flight_modes.can_auto_mode;
326+
break;
327+
case ActionServer::FlightMode::ReturnToLaunch:
328+
allow_mode = _allowed_flight_modes.can_auto_rtl_mode;
329+
break;
330+
case ActionServer::FlightMode::Takeoff:
331+
allow_mode = _allowed_flight_modes.can_auto_takeoff_mode;
332+
break;
333+
case ActionServer::FlightMode::Land:
334+
allow_mode = _allowed_flight_modes.can_auto_land_mode;
335+
break;
336+
case ActionServer::FlightMode::Stabilized:
337+
allow_mode = _allowed_flight_modes.can_stabilize_mode;
338+
break;
339+
case ActionServer::FlightMode::Offboard:
340+
allow_mode = _allowed_flight_modes.can_guided_mode;
341+
break;
342+
default:
343+
allow_mode = false;
344+
break;
345+
}
346+
347+
if (allow_mode) {
348+
uint8_t system_base_mode = get_base_mode();
349+
system_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
350+
set_base_mode(system_base_mode);
351+
set_custom_mode(set_mode.custom_mode);
352+
}
353+
354+
_flight_mode_change_callbacks.queue(
355+
allow_mode ? ActionServer::Result::Success : ActionServer::Result::CommandDenied,
356+
request_flight_mode,
357+
[this](const auto& func) { _server_component_impl->call_user_callback(func); });
358+
359+
// make_command_ack_message
360+
mavlink_command_ack_t command_ack{};
361+
command_ack.command = MAVLINK_MSG_ID_SET_MODE;
362+
command_ack.result =
363+
allow_mode ? MAV_RESULT::MAV_RESULT_ACCEPTED : MAV_RESULT_TEMPORARILY_REJECTED;
364+
command_ack.progress = std::numeric_limits<uint8_t>::max();
365+
command_ack.result_param2 = 0;
366+
command_ack.target_system = message.sysid;
367+
command_ack.target_component = message.compid;
368+
_server_component_impl->send_command_ack(command_ack);
369+
},
370+
this);
269371
}
270372

271373
void ActionServerImpl::deinit()
@@ -423,7 +525,18 @@ ActionServer::Result ActionServerImpl::set_armed_state(bool armed)
423525

424526
ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode flight_mode)
425527
{
426-
// note: currently on receipt of MAV_CMD_DO_SET_MODE, we error out if the mode
528+
ActionServer::Result res = set_flight_mode_internal(flight_mode);
529+
_flight_mode_change_callbacks.queue(res, flight_mode, [this](const auto& func) {
530+
_server_component_impl->call_user_callback(func);
531+
});
532+
533+
return res;
534+
}
535+
536+
ActionServer::Result
537+
ActionServerImpl::set_flight_mode_internal(ActionServer::FlightMode flight_mode)
538+
{
539+
// note: currently on receipt of MAV_CMD_DO_SET_MODE/MAV_CMD_SET_MODE, we error out if the mode
427540
// is *not* PX4 custom. For symmetry we will also convert from FlightMode to
428541
// PX4 custom when set directly.
429542
std::lock_guard<std::mutex> lock(_callback_mutex);
@@ -435,12 +548,6 @@ ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode
435548
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
436549
set_base_mode(base_mode);
437550
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-
444551
return ActionServer::Result::Success;
445552
}
446553

‎src/mavsdk/plugins/action_server/action_server_impl.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,8 @@ class ActionServerImpl : public ServerPluginImplBase {
5555

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

58+
ActionServer::Result set_flight_mode_internal(ActionServer::FlightMode flight_mode);
59+
5860
private:
5961
static ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode flight_mode);
6062
static uint32_t to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode);

‎src/mavsdk/plugins/action_server/include/plugins/action_server/action_server.h‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,10 @@ class ActionServer : public ServerPluginBase {
8383
bool can_auto_mode{}; /**< @brief Auto/mission mode */
8484
bool can_guided_mode{}; /**< @brief Guided mode */
8585
bool can_stabilize_mode{}; /**< @brief Stabilize mode */
86+
bool can_auto_rtl_mode{}; /**< @brief Auto RTL mode */
87+
bool can_auto_takeoff_mode{}; /**< @brief Auto takeoff mode */
88+
bool can_auto_land_mode{}; /**< @brief Auto land mode */
89+
bool can_auto_loiter_mode{}; /**< @brief Auto hold/loiter mode */
8690
};
8791

8892
/**
@@ -372,6 +376,17 @@ class ActionServer : public ServerPluginBase {
372376
*/
373377
Result set_flight_mode(FlightMode flight_mode) const;
374378

379+
/**
380+
* @brief Set/override the flight mode of the vehicle directly, and *do not* notify subscribers
381+
*
382+
* This function is blocking.
383+
*
384+
385+
* @return Result of request.
386+
387+
*/
388+
Result set_flight_mode_internal(FlightMode flight_mode) const;
389+
375390
/**
376391
* @brief Copy constructor.
377392
*/

0 commit comments

Comments
 (0)