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

Add support for COMMAND_INT and abstraction for the same including COMMAND_LONG #351

Merged
merged 5 commits into from
Apr 9, 2018
Merged

Add support for COMMAND_INT and abstraction for the same including COMMAND_LONG #351

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
9354ea0
core, plugins: Add support for COMMAND_INT #326
Apr 6, 2018
4e79d5d
core, plugins: Rename CmdInt -> CommandInt
Apr 6, 2018
c67db7a
core, plugins: Correct param names of `COMMAND_INT`
Apr 6, 2018
e09ac5a
Add "reserved_val" as argument to `set_as_reserved()`
Apr 6, 2018
eeece31
core: plugins: Rename cmd -> command for consistency
Apr 6, 2018
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
109 changes: 89 additions & 20 deletions core/mavlink_commands.cpp
View file
Open in desktop
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
Open in desktop
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