Skip to content

Commit

Permalink
core: add deprecated messages to RequestMessage class
Browse files Browse the repository at this point in the history 
This is a first step towards consistently using the new REQUEST_MESSAGE
commands everywhere while still being able to fall back to the
deprecated commands on retries.
  • Loading branch information
@julianoes
julianoes committed Aug 23, 2024
1 parent 769783a commit 06f485d
Show file tree
Hide file tree
Showing 2 changed files with 125 additions and 14 deletions.
133 changes: 121 additions & 12 deletions src/mavsdk/core/request_message.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@
#include "log.h"
#include "request_message.h"
#include "system_impl.h"
#include <cstdint>
#include <string>

namespace mavsdk {

Expand Down Expand Up @@ -50,32 +52,141 @@ void RequestMessage::request(
this);

// And send off command
send_request(message_id, target_component);
send_request(_work_items.back());
}

void RequestMessage::send_request(uint32_t message_id, uint8_t target_component)
void RequestMessage::send_request(WorkItem& item)
{
// Every second time, starting with first retry, we try the old request commands.
// If no old commands exist, we of course just send the new one again.
if (item.retries % 2 != 0) {
if (!try_sending_request_using_old_command(item)) {
send_request_using_new_command(item);
}
} else {
send_request_using_new_command(item);
}
}

void RequestMessage::send_request_using_new_command(WorkItem& item)
{
if (item.retries > 0) {
LogWarn() << "Request message " << item.message_id
<< " again using REQUEST_MESSAGE (retries: " << item.retries << ")";
} else {
LogDebug() << "Request message " << item.message_id << " using REQUEST_MESSAGE";
}

MavlinkCommandSender::CommandLong command_request_message{};
command_request_message.command = MAV_CMD_REQUEST_MESSAGE;
command_request_message.target_system_id = _system_impl.get_system_id();
command_request_message.target_component_id = target_component;
command_request_message.params.maybe_param1 = {static_cast<float>(message_id)};
command_request_message.target_component_id = item.target_component;
command_request_message.params.maybe_param1 = {static_cast<float>(item.message_id)};
_command_sender.queue_command_async(
command_request_message, [this, message_id](MavlinkCommandSender::Result result, float) {
command_request_message,
[this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
if (result != MavlinkCommandSender::Result::InProgress) {
handle_command_result(message_id, result);
}
});
}

bool RequestMessage::try_sending_request_using_old_command(WorkItem& item)
{
MavlinkCommandSender::CommandLong command_request_message{};
command_request_message.target_system_id = _system_impl.get_system_id();
command_request_message.target_component_id = item.target_component;

std::string command_name;

switch (item.message_id) {
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
command_request_message.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
command_request_message.params.maybe_param1 = {1.f};
command_name = "REQUEST_AUTOPILOT_CAPABILITIES";
break;

case MAVLINK_MSG_ID_CAMERA_INFORMATION:
command_request_message.command = MAV_CMD_REQUEST_CAMERA_INFORMATION;
command_request_message.params.maybe_param1 = {1.0f};
command_name = "REQUEST_CAMERA_INFORMATION";
break;

case MAVLINK_MSG_ID_CAMERA_SETTINGS:
command_request_message.command = MAV_CMD_REQUEST_CAMERA_SETTINGS;
command_request_message.params.maybe_param1 = {1.0f};
command_name = "REQUEST_CAMERA_SETTINGS";

break;

case MAVLINK_MSG_ID_STORAGE_INFORMATION:
command_request_message.command = MAV_CMD_REQUEST_STORAGE_INFORMATION;
command_request_message.params.maybe_param1 = {
static_cast<float>(item.param2)}; // storage ID
command_request_message.params.maybe_param2 = {1.0f};
command_name = "REQUEST_STORAGE_INFORMATION";
break;

case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
command_request_message.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS;
command_request_message.params.maybe_param1 = {1.0f};
command_name = "REQUEST_CAMERA_CAPTURE_STATUS";
break;

case MAVLINK_MSG_ID_FLIGHT_INFORMATION:
command_request_message.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION;
command_request_message.params.maybe_param1 = {1.0f};
command_name = "REQUEST_FLIGHT_INFORMATION";
break;

case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_STATUS;
command_request_message.params.maybe_param1 = {
static_cast<float>(item.param2)}; // stream ID
command_name = "REQUEST_VIDEO_STREAM_STATUS";
break;

case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
command_request_message.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION;
command_request_message.params.maybe_param1 = {
static_cast<float>(item.param2)}; // stream ID
command_name = "REQUEST_VIDEO_STREAM_INFORMATION";
break;

case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
command_request_message.command = MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE;
command_request_message.params.maybe_param1 = {
static_cast<float>(item.param2)}; // sequence number
command_name = "REQUEST_CAMERA_IMAGE_CAPTURE";
break;

default:
// No alternative command for this message.
return false;
}

LogWarn() << "Request message " << item.message_id << " again using " << command_name
<< " (retries: " << item.retries << ")";

_command_sender.queue_command_async(
command_request_message,
[this, message_id = item.message_id](MavlinkCommandSender::Result result, float) {
if (result != MavlinkCommandSender::Result::InProgress) {
handle_command_result(message_id, result);
}
});

return true;
}

void RequestMessage::handle_any_message(const mavlink_message_t& message)
{
std::unique_lock<std::mutex> lock(_mutex);

for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
// Check if we're waiting for this message.
// TODO: check if params are correct.
if (it->message_id != message.msgid) {
if (it->message_id != message.msgid && it->target_component == message.compid) {
continue;
}

Expand Down Expand Up @@ -159,23 +270,21 @@ void RequestMessage::handle_timeout(uint32_t message_id, uint8_t target_componen

for (auto it = _work_items.begin(); it != _work_items.end(); ++it) {
// Check if we're waiting for this result
if (it->message_id != message_id) {
if (it->message_id != message_id || it->target_component != target_component) {
continue;
}

if (it->retries > 2) {
if (++it->retries > 3) {
// We have already retried, let's give up.
auto temp_callback = it->callback;
_message_handler.unregister_one(it->message_id, this);
_work_items.erase(it);
lock.unlock();
temp_callback(MavlinkCommandSender::Result::Timeout, {});
return;
} else {
send_request(message_id, target_component);
LogWarn() << "Requesting message again (retries: " << it->retries << ")";
it->retries += 1;
}

send_request(*it);
}
}

Expand Down
6 changes: 4 additions & 2 deletions src/mavsdk/core/request_message.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,12 +37,14 @@ class RequestMessage {
uint8_t target_component{0};
RequestMessageCallback callback{};
uint32_t param2{0};
std::size_t retries{0};
unsigned retries{0};
TimeoutHandler::Cookie timeout_cookie{};
std::optional<MavlinkCommandSender::Result> maybe_result{};
};

void send_request(uint32_t message_id, uint8_t target_component);
void send_request(WorkItem& item);
void send_request_using_new_command(WorkItem& item);
bool try_sending_request_using_old_command(WorkItem& item);
void handle_any_message(const mavlink_message_t& message);
void handle_command_result(uint32_t message_id, MavlinkCommandSender::Result result);
void handle_timeout(uint32_t message_id, uint8_t target_component);
Expand Down

0 comments on commit 06f485d

Please sign in to comment.