Skip to content

Commit

Permalink
Merge pull request mavlink#351 from dronecore/326-add-support-for-COM…
Browse files Browse the repository at this point in the history 
…MAND_INT

Add support for COMMAND_INT and abstraction for the same including `COMMAND_LONG`
  • Loading branch information
@julianoes
julianoes committed Apr 9, 2018
2 parents 6496b2e + 06afff9 commit cb41f94
Show file tree
Hide file tree
Showing 10 changed files with 451 additions and 268 deletions.
109 changes: 89 additions & 20 deletions core/mavlink_commands.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,8 @@ MAVLinkCommands::~MAVLinkCommands()
_parent.unregister_all_mavlink_message_handlers(this);
}

MAVLinkCommands::Result MAVLinkCommands::send_command(uint16_t command,
const MAVLinkCommands::Params params,
uint8_t target_system_id,
uint8_t target_component_id)
MAVLinkCommands::Result
MAVLinkCommands::send_command(MAVLinkCommands::CommandInt &command)
{
struct PromiseResult {
Result result;
Expand All @@ -45,14 +43,50 @@ MAVLinkCommands::Result MAVLinkCommands::send_command(uint16_t command,
std::shared_ptr<std::promise<PromiseResult>> prom =
std::make_shared<std::promise<PromiseResult>>();

queue_command_async(command, params, target_system_id, target_component_id,
queue_command_async(command,
[prom](Result result, float progress) {
PromiseResult promise_result {};
promise_result.result = result;
promise_result.progress = progress;
prom->set_value(promise_result);
});

std::future<PromiseResult> res = prom->get_future();
while (true) {
// Block now to wait for result.
res.wait();

PromiseResult promise_result = res.get();

if (promise_result.result == Result::IN_PROGRESS) {
LogInfo() << "In progress: " << promise_result.progress;
continue;
}
return promise_result.result;
}
);
}

MAVLinkCommands::Result
MAVLinkCommands::send_command(MAVLinkCommands::CommandLong &command)
{
struct PromiseResult {
Result result;
float progress;
};

// We wrap the async call with a promise and future.
std::shared_ptr<std::promise<PromiseResult>> prom =
std::make_shared<std::promise<PromiseResult>>();

command.target_system_id = _parent.get_system_id();

queue_command_async(command,
[prom](Result result, float progress) {
PromiseResult promise_result {};
promise_result.result = result;
promise_result.progress = progress;
prom->set_value(promise_result);
});

std::future<PromiseResult> res = prom->get_future();
while (true) {
Expand All @@ -70,27 +104,62 @@ MAVLinkCommands::Result MAVLinkCommands::send_command(uint16_t command,
}


void MAVLinkCommands::queue_command_async(uint16_t command,
const MAVLinkCommands::Params params,
uint8_t target_system_id,
uint8_t target_component_id,
command_result_callback_t callback)

void
MAVLinkCommands::queue_command_async(CommandInt &command,
command_result_callback_t callback)
{
// LogDebug() << "Command " << (int)(command.command) << " to send to "
// << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

Work new_work {};
mavlink_msg_command_int_pack(GCSClient::system_id,
GCSClient::component_id,
&new_work.mavlink_message,
command.target_system_id,
command.target_component_id,
command.frame,
command.command,
command.current,
command.autocontinue,
command.params.param1,
command.params.param2,
command.params.param3,
command.params.param4,
command.params.x,
command.params.y,
command.params.z);

new_work.callback = callback;
new_work.mavlink_command = command.command;
_work_queue.push_back(new_work);
}

void
MAVLinkCommands::queue_command_async(CommandLong &command,
command_result_callback_t callback)
{
// LogDebug() << "Command " << (int)command << " to send to " << (int)target_system_id << ", "
// << (int)target_component_id;
// LogDebug() << "Command " << (int)(command.command) << " to send to "
// << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;

Work new_work {};
mavlink_msg_command_long_pack(GCSClient::system_id,
GCSClient::component_id,
&new_work.mavlink_message,
target_system_id,
target_component_id,
command,
0,
params.v[0], params.v[1], params.v[2], params.v[3],
params.v[4], params.v[5], params.v[6]);
command.target_system_id,
command.target_component_id,
command.command,
command.confirmation,
command.params.param1,
command.params.param2,
command.params.param3,
command.params.param4,
command.params.param5,
command.params.param6,
command.params.param7);

new_work.callback = callback;
new_work.mavlink_command = command;
new_work.mavlink_command = command.command;
_work_queue.push_back(new_work);
}

Expand Down
74 changes: 63 additions & 11 deletions core/mavlink_commands.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,20 +30,72 @@ class MAVLinkCommands

typedef std::function<void(Result, float)> command_result_callback_t;

struct Params {
float v[7];
struct CommandInt {
uint8_t target_system_id;
uint8_t target_component_id;
MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
uint16_t command;
bool current = 0;
bool autocontinue = false;
// Most of the "Reserved" values in MAVLink spec are NAN.
struct Params {
float param1 = NAN;
float param2 = NAN;
float param3 = NAN;
float param4 = NAN;
int32_t x = 0;
int32_t y = 0;
float z = NAN;
} params;

// In some cases "Reserved" value could be "0".
// This utility method can be used in such case.
static
void set_as_reserved(Params &params, float reserved_value = NAN)
{
params.param1 = reserved_value;
params.param2 = reserved_value;
params.param3 = reserved_value;
params.param4 = reserved_value;
params.x = 0;
params.y = 0;
params.z = reserved_value;
}
};

Result send_command(uint16_t command,
const Params params,
uint8_t target_system_id,
uint8_t target_component_id);
struct CommandLong {
uint8_t target_system_id;
uint8_t target_component_id;
uint16_t command;
uint8_t confirmation = 0;
struct Params {
float param1 = NAN;
float param2 = NAN;
float param3 = NAN;
float param4 = NAN;
float param5 = NAN;
float param6 = NAN;
float param7 = NAN;
} params;

static
void set_as_reserved(Params &params, float reserved_value = NAN)
{
params.param1 = reserved_value;
params.param2 = reserved_value;
params.param3 = reserved_value;
params.param4 = reserved_value;
params.param5 = reserved_value;
params.param6 = reserved_value;
params.param7 = reserved_value;
}
};

Result send_command(CommandInt &command);
Result send_command(CommandLong &command);

void queue_command_async(uint16_t command,
const Params params,
uint8_t target_system_id,
uint8_t target_component_id,
command_result_callback_t callback);
void queue_command_async(CommandInt &command, command_result_callback_t callback);
void queue_command_async(CommandLong &command, command_result_callback_t callback);

void do_work();

Expand Down
Loading

0 comments on commit cb41f94

Please sign in to comment.