Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improve CommandReceiver API #1323

Merged
merged 2 commits into from
Feb 18, 2021
Merged

Improve CommandReceiver API #1323

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
dd8f1bd
mavlink_commands: store origin sysid/compid
julianoes Feb 18, 2021
569ebfd
core: add API to send immediate ACK
julianoes Feb 18, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 16 additions & 12 deletions src/core/mavlink_commands.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -341,40 +341,44 @@ MavlinkCommandReceiver::~MavlinkCommandReceiver()

void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& message)
{
mavlink_command_int_t command_int;
mavlink_msg_command_int_decode(&message, &command_int);
MavlinkCommandReceiver::CommandInt cmd(command_int);
MavlinkCommandReceiver::CommandInt cmd(message);

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

for (auto it = _mavlink_command_int_handler_table.begin();
it != _mavlink_command_int_handler_table.end();
++it) {
if (it->cmd_id == command_int.command) {
it->callback(cmd);
if (it->cmd_id == cmd.command) {
// The client side can pack a COMMAND_ACK as a response to receiving the command.
auto maybe_message = it->callback(cmd);
if (maybe_message) {
_parent.send_message(maybe_message.value());
}
}
}
}

void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& message)
{
mavlink_command_long_t command_long;
mavlink_msg_command_long_decode(&message, &command_long);
MavlinkCommandReceiver::CommandLong cmd(command_long);
MavlinkCommandReceiver::CommandLong cmd(message);

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

for (auto it = _mavlink_command_long_handler_table.begin();
it != _mavlink_command_long_handler_table.end();
++it) {
if (it->cmd_id == command_long.command) {
it->callback(cmd);
if (it->cmd_id == cmd.command) {
// The client side can pack a COMMAND_ACK as a response to receiving the command.
auto maybe_message = it->callback(cmd);
if (maybe_message) {
_parent.send_message(maybe_message.value());
}
}
}
}

void MavlinkCommandReceiver::register_mavlink_command_handler(
uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie)
uint16_t cmd_id, MavlinkCommandIntHandler callback, const void* cookie)
{
std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

Expand All @@ -383,7 +387,7 @@ void MavlinkCommandReceiver::register_mavlink_command_handler(
}

void MavlinkCommandReceiver::register_mavlink_command_handler(
uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie)
uint16_t cmd_id, MavlinkCommandLongHandler callback, const void* cookie)
{
std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

Expand Down
33 changes: 25 additions & 8 deletions src/core/mavlink_commands.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <string>
#include <functional>
#include <mutex>
#include <optional>

namespace mavsdk {

Expand Down Expand Up @@ -136,6 +137,8 @@ class MavlinkCommandReceiver {
struct CommandInt {
uint8_t target_system_id{0};
uint8_t target_component_id{0};
uint8_t origin_system_id{0};
uint8_t origin_component_id{0};
MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
uint16_t command{0};
bool current{0};
Expand All @@ -151,10 +154,15 @@ class MavlinkCommandReceiver {
float z = NAN;
} params{};

CommandInt(const mavlink_command_int_t& command_int)
CommandInt(const mavlink_message_t& message)
{
mavlink_command_int_t command_int;
mavlink_msg_command_int_decode(&message, &command_int);

target_system_id = command_int.target_system;
target_component_id = command_int.target_component;
origin_system_id = message.sysid;
origin_component_id = message.compid;
command = command_int.command;
frame = static_cast<MAV_FRAME>(command_int.frame);
current = command_int.current;
Expand All @@ -172,6 +180,8 @@ class MavlinkCommandReceiver {
struct CommandLong {
uint8_t target_system_id{0};
uint8_t target_component_id{0};
uint8_t origin_system_id{0};
uint8_t origin_component_id{0};
uint16_t command{0};
uint8_t confirmation{0};
struct Params {
Expand All @@ -184,10 +194,15 @@ class MavlinkCommandReceiver {
float param7 = NAN;
} params{};

CommandLong(const mavlink_command_long_t& command_long)
CommandLong(const mavlink_message_t& message)
{
mavlink_command_long_t command_long;
mavlink_msg_command_long_decode(&message, &command_long);

target_system_id = command_long.target_system;
target_component_id = command_long.target_component;
origin_system_id = message.sysid;
origin_component_id = message.compid;
command = command_long.command;
confirmation = command_long.confirmation;
params.param1 = command_long.param1;
Expand All @@ -200,13 +215,15 @@ class MavlinkCommandReceiver {
}
};

typedef std::function<void(const CommandInt&)> mavlink_command_int_handler_t;
typedef std::function<void(const CommandLong&)> mavlink_command_long_handler_t;
using MavlinkCommandIntHandler =
std::function<std::optional<mavlink_message_t>(const CommandInt&)>;
using MavlinkCommandLongHandler =
std::function<std::optional<mavlink_message_t>(const CommandLong&)>;

void register_mavlink_command_handler(
uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie);
uint16_t cmd_id, MavlinkCommandIntHandler callback, const void* cookie);
void register_mavlink_command_handler(
uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie);
uint16_t cmd_id, MavlinkCommandLongHandler callback, const void* cookie);

void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie);
void unregister_all_mavlink_command_handlers(const void* cookie);
Expand All @@ -219,13 +236,13 @@ class MavlinkCommandReceiver {

struct MAVLinkCommandIntHandlerTableEntry {
uint16_t cmd_id;
mavlink_command_int_handler_t callback;
MavlinkCommandIntHandler callback;
const void* cookie; // This is the identification to unregister.
};

struct MAVLinkCommandLongHandlerTableEntry {
uint16_t cmd_id;
mavlink_command_long_handler_t callback;
MavlinkCommandLongHandler callback;
const void* cookie; // This is the identification to unregister.
};

Expand Down
8 changes: 2 additions & 6 deletions src/core/system_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1219,17 +1219,13 @@ void SystemImpl::intercept_outgoing_messages(std::function<bool(mavlink_message_
}

void SystemImpl::register_mavlink_command_handler(
uint16_t cmd_id,
MavlinkCommandReceiver::mavlink_command_int_handler_t callback,
const void* cookie)
uint16_t cmd_id, MavlinkCommandReceiver::MavlinkCommandIntHandler callback, const void* cookie)
{
_receive_commands.register_mavlink_command_handler(cmd_id, callback, cookie);
}

void SystemImpl::register_mavlink_command_handler(
uint16_t cmd_id,
MavlinkCommandReceiver::mavlink_command_long_handler_t callback,
const void* cookie)
uint16_t cmd_id, MavlinkCommandReceiver::MavlinkCommandLongHandler callback, const void* cookie)
{
_receive_commands.register_mavlink_command_handler(cmd_id, callback, cookie);
}
Expand Down
4 changes: 2 additions & 2 deletions src/core/system_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -240,11 +240,11 @@ class SystemImpl : public Sender {

void register_mavlink_command_handler(
uint16_t cmd_id,
MavlinkCommandReceiver::mavlink_command_int_handler_t callback,
MavlinkCommandReceiver::MavlinkCommandIntHandler callback,
const void* cookie);
void register_mavlink_command_handler(
uint16_t cmd_id,
MavlinkCommandReceiver::mavlink_command_long_handler_t callback,
MavlinkCommandReceiver::MavlinkCommandLongHandler callback,
const void* cookie);
void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie);
void unregister_all_mavlink_command_handlers(const void* cookie);
Expand Down