Skip to content

Commit

Permalink
core, plugins: Add support for COMMAND_INT #326
Browse files Browse the repository at this point in the history 
Adds new types `MAVLinkCommands::CmdInt` and `MAVLinkCommands::CmdLong`
types that abstract to those of MAVLink protocol:
http://mavlink.org/messages/common/#COMMAND_INT
http://mavlink.org/messages/common/#COMMAND_LONG

Also includes the changes in core and plugins, wherever these
commands are used. `COMMAND_INT` is used only in Gimbal plugin as of
now for `MAV_CMD_DO_SET_ROI_LOCATION`.
  • Loading branch information
Shakthi Prashanth M committed Apr 6, 2018
1 parent c05235f commit 9354ea0
Show file tree
Hide file tree
Showing 7 changed files with 402 additions and 225 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::CmdInt &cmd)
{
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(cmd,
[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::CmdLong &cmd)
{
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>>();

cmd.target_system_id = _parent.get_system_id();

queue_command_async(cmd,
[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(CmdInt &cmd,
command_result_callback_t callback)
{
// LogDebug() << "Command " << (int)(cmd.command) << " to send to "
// << (int)(cmd.target_system_id)<< ", " << (int)(cmd.target_component_id;

Work new_work {};
mavlink_msg_command_int_pack(GCSClient::system_id,
GCSClient::component_id,
&new_work.mavlink_message,
cmd.target_system_id,
cmd.target_component_id,
cmd.frame,
cmd.command,
cmd.current,
cmd.autocontinue,
cmd.params.param1,
cmd.params.param2,
cmd.params.param3,
cmd.params.param4,
cmd.params.lat_deg,
cmd.params.lon_deg,
cmd.params.alt_m);

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

void
MAVLinkCommands::queue_command_async(CmdLong &cmd,
command_result_callback_t callback)
{
// LogDebug() << "Command " << (int)command << " to send to " << (int)target_system_id << ", "
// << (int)target_component_id;
// LogDebug() << "Command " << (int)(cmd.command) << " to send to "
// << (int)(cmd.target_system_id)<< ", " << (int)(cmd.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]);
cmd.target_system_id,
cmd.target_component_id,
cmd.command,
cmd.confirmation,
cmd.params.param1,
cmd.params.param2,
cmd.params.param3,
cmd.params.param4,
cmd.params.param5,
cmd.params.param6,
cmd.params.param7);

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

Expand Down
68 changes: 57 additions & 11 deletions core/mavlink_commands.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <string>
#include <functional>
#include <mutex>
#include <memory>

namespace dronecore {

Expand All @@ -30,20 +31,65 @@ class MAVLinkCommands

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

struct Params {
float v[7];
struct CmdInt {
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;
struct Params {
float param1 = NAN;
float param2 = NAN;
float param3 = NAN;
float param4 = NAN;
int32_t lat_deg = 0;
int32_t lon_deg = 0;
float alt_m = NAN;
} params;
static void set_as_reserved(Params &params)
{
params.param1 = 0.f;
params.param2 = 0.f;
params.param3 = 0.f;
params.param4 = 0.f;
params.lat_deg = 0;
params.lon_deg = 0;
params.alt_m = 0.f;
}
};

Result send_command(uint16_t command,
const Params params,
uint8_t target_system_id,
uint8_t target_component_id);
struct CmdLong {
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)
{
params.param1 = 0.f;
params.param2 = 0.f;
params.param3 = 0.f;
params.param4 = 0.f;
params.param5 = 0.f;
params.param6 = 0.f;
params.param7 = 0.f;
}
};

Result send_command(CmdInt &cmd);
Result send_command(CmdLong &cmd);

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(CmdInt &cmd, command_result_callback_t callback);
void queue_command_async(CmdLong &cmd, command_result_callback_t callback);

void do_work();

Expand Down
Loading

0 comments on commit 9354ea0

Please sign in to comment.