Skip to content

Commit

Permalink
Merge branch 'develop' into add-example-multiple-vehicles
Browse files Browse the repository at this point in the history
  • Loading branch information
@JonasVautherin
JonasVautherin committed Feb 20, 2019
2 parents dcc2e10 + 7697c99 commit a0ff2fd
Show file tree
Hide file tree
Showing 2 changed files with 101 additions and 62 deletions.
148 changes: 91 additions & 57 deletions plugins/mission/mission_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,11 @@ void MissionImpl::process_mission_request_int(const mavlink_message_t &message)

{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::SET_MISSION) {
if (_activity.state == Activity::State::SET_MISSION_COUNT) {
_activity.state = Activity::State::SET_MISSION_ITEM;
}

if (_activity.state != Activity::State::SET_MISSION_ITEM) {
if (_activity.state != Activity::State::ABORTED) {
LogWarn() << "Ignoring mission request int, not active";
}
Expand All @@ -110,19 +114,21 @@ void MissionImpl::process_mission_request_int(const mavlink_message_t &message)
{
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
_mission_data.retries = 0;
_mission_data.last_mission_item_to_upload = mission_request_int.seq;
}

upload_mission_item(mission_request_int.seq);
upload_mission_item();

// Reset the timeout because we're still communicating.
_parent->refresh_timeout_handler(_timeout_cookie);
}

void MissionImpl::process_mission_ack(const mavlink_message_t &message)
{
// LogDebug() << "Received mission ack";
{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::SET_MISSION) {
if (_activity.state != Activity::State::SET_MISSION_ITEM) {
if (_activity.state != Activity::State::ABORTED) {
LogWarn() << "Error: not sure how to process Mission ack.";
}
Expand Down Expand Up @@ -223,9 +229,11 @@ void MissionImpl::process_mission_count(const mavlink_message_t &message)
{
{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::GET_MISSION) {
if (_activity.state != Activity::State::GET_MISSION_LIST) {
return;
}

_activity.state = Activity::State::GET_MISSION_REQUEST;
}

mavlink_mission_count_t mission_count;
Expand All @@ -235,28 +243,28 @@ void MissionImpl::process_mission_count(const mavlink_message_t &message)
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
_mission_data.num_mission_items_to_download = mission_count.count;
_mission_data.next_mission_item_to_download = 0;
_mission_data.retries = 0;
}

// We are now requesting mission items and use a lower timeout for this.
_parent->unregister_timeout_handler(_timeout_cookie);
_parent->refresh_timeout_handler(_timeout_cookie);

_parent->register_timeout_handler(
std::bind(&MissionImpl::process_timeout, this), RETRY_TIMEOUT_S, &_timeout_cookie);
download_next_mission_item();
}

void MissionImpl::process_mission_item_int(const mavlink_message_t &message)
{
{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::GET_MISSION) {
if (_activity.state != Activity::State::GET_MISSION_REQUEST) {
return;
}
}

auto mission_item_int_ptr = std::make_shared<mavlink_mission_item_int_t>();
mavlink_msg_mission_item_int_decode(&message, mission_item_int_ptr.get());

// LogDebug() << "Received mission item int: " << int(mission_item_int_ptr->seq);

{
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
if (mission_item_int_ptr->seq == _mission_data.next_mission_item_to_download) {
Expand Down Expand Up @@ -336,6 +344,24 @@ void MissionImpl::upload_mission_async(

assemble_mavlink_messages();

_parent->register_timeout_handler(
std::bind(&MissionImpl::process_timeout, this), RETRY_TIMEOUT_S, &_timeout_cookie);

{
std::lock_guard<std::mutex> lock(_activity.mutex);
_activity.state = Activity::State::SET_MISSION_COUNT;
}
{
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
_mission_data.result_callback = callback;
_mission_data.retries = 0;
}

send_count();
}

void MissionImpl::send_count()
{
mavlink_message_t message;
mavlink_msg_mission_count_pack(GCSClient::system_id,
GCSClient::component_id,
Expand All @@ -346,22 +372,9 @@ void MissionImpl::upload_mission_async(
MAV_MISSION_TYPE_MISSION);

if (!_parent->send_message(message)) {
report_mission_result(callback, Mission::Result::ERROR);
return;
}

// We use the longer process timeout here because essentially the autopilot needs to pull
// the items up.
_parent->register_timeout_handler(
std::bind(&MissionImpl::process_timeout, this), PROCESS_TIMEOUT_S, &_timeout_cookie);

{
std::lock_guard<std::mutex> lock(_activity.mutex);
_activity.state = Activity::State::SET_MISSION;
}
{
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
_mission_data.result_callback = callback;
report_mission_result(_mission_data.result_callback, Mission::Result::ERROR);
return;
}
}

Expand All @@ -371,7 +384,8 @@ void MissionImpl::upload_mission_cancel()

{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::SET_MISSION) {
if (_activity.state != Activity::State::SET_MISSION_COUNT &&
_activity.state != Activity::State::SET_MISSION_ITEM) {
LogWarn() << "No mission upload in progress";
return;
}
Expand Down Expand Up @@ -420,26 +434,12 @@ void MissionImpl::download_mission_async(
return;
}

mavlink_message_t message;
mavlink_msg_mission_request_list_pack(GCSClient::system_id,
GCSClient::component_id,
&message,
_parent->get_system_id(),
_parent->get_autopilot_id(),
MAV_MISSION_TYPE_MISSION);

if (!_parent->send_message(message)) {
report_mission_items_and_result(callback, Mission::Result::ERROR);
return;
}

// We retry the list request and mission item request, so we use the lower timeout.
_parent->register_timeout_handler(
std::bind(&MissionImpl::process_timeout, this), RETRY_TIMEOUT_S, &_timeout_cookie);

{
std::lock_guard<std::mutex> lock(_activity.mutex);
_activity.state = Activity::State::GET_MISSION;
_activity.state = Activity::State::GET_MISSION_LIST;
}

{
Expand All @@ -449,6 +449,26 @@ void MissionImpl::download_mission_async(
_mission_data.retries = 0;
_mission_data.mission_items_and_result_callback = callback;
}

request_list();
}

void MissionImpl::request_list()
{
mavlink_message_t message;
mavlink_msg_mission_request_list_pack(GCSClient::system_id,
GCSClient::component_id,
&message,
_parent->get_system_id(),
_parent->get_autopilot_id(),
MAV_MISSION_TYPE_MISSION);

if (!_parent->send_message(message)) {
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
report_mission_items_and_result(_mission_data.mission_items_and_result_callback,
Mission::Result::CANCELLED);
return;
}
}

void MissionImpl::download_mission_cancel()
Expand All @@ -457,7 +477,8 @@ void MissionImpl::download_mission_cancel()

{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state != Activity::State::GET_MISSION) {
if (_activity.state != Activity::State::GET_MISSION_LIST &&
_activity.state != Activity::State::GET_MISSION_REQUEST) {
LogWarn() << "No mission download in progress";
return;
}
Expand Down Expand Up @@ -1082,16 +1103,18 @@ void MissionImpl::set_current_mission_item_async(int current, Mission::result_ca
}
}

void MissionImpl::upload_mission_item(uint16_t seq)
void MissionImpl::upload_mission_item()
{
std::lock_guard<std::recursive_mutex> lock(_mission_data.mutex);
LogDebug() << "Send mission item " << int(seq);
if (seq >= _mission_data.mavlink_mission_item_messages.size()) {
LogDebug() << "Send mission item " << _mission_data.last_mission_item_to_upload;
if (_mission_data.last_mission_item_to_upload >=
int(_mission_data.mavlink_mission_item_messages.size())) {
LogErr() << "Mission item requested out of bounds.";
return;
}

_parent->send_message(*_mission_data.mavlink_mission_item_messages.at(seq));
_parent->send_message(
*_mission_data.mavlink_mission_item_messages.at(_mission_data.last_mission_item_to_upload));
}

void MissionImpl::copy_mission_item_vector(
Expand Down Expand Up @@ -1257,15 +1280,12 @@ void MissionImpl::process_timeout()
{
std::lock_guard<std::mutex> lock(_activity.mutex);

if (_activity.state == Activity::State::SET_MISSION) {
// We can't retry this, the autopilot should be requesting the items
// again.
_activity.state = Activity::State::NONE;
LogWarn() << "Mission handling timed out while uploading mission.";
report_mission_result(_mission_data.result_callback, Mission::Result::TIMEOUT);
return;

} else if (_activity.state == Activity::State::GET_MISSION) {
if (_activity.state == Activity::State::SET_MISSION_ITEM) {
should_retry = true;
} else if (_activity.state == Activity::State::SET_MISSION_COUNT) {
should_retry = true;
} else if (_activity.state == Activity::State::GET_MISSION_LIST ||
_activity.state == Activity::State::GET_MISSION_REQUEST) {
should_retry = true;
} else {
LogWarn() << "unknown mission timeout";
Expand All @@ -1289,11 +1309,25 @@ void MissionImpl::process_timeout()
} else {
_mission_data.mutex.unlock();

LogWarn() << "Retrying requesting mission item...";
// We are retrying, so we use the lower timeout.
_parent->register_timeout_handler(
std::bind(&MissionImpl::process_timeout, this), RETRY_TIMEOUT_S, &_timeout_cookie);
download_next_mission_item();

{
std::lock_guard<std::mutex> lock(_activity.mutex);
if (_activity.state == Activity::State::GET_MISSION_LIST) {
LogWarn() << "Retrying requesting mission list...";
request_list();
} else if (_activity.state == Activity::State::GET_MISSION_REQUEST) {
LogWarn() << "Retrying requesting mission item...";
download_next_mission_item();
} else if (_activity.state == Activity::State::SET_MISSION_COUNT) {
LogWarn() << "Retrying send mission count...";
send_count();
} else if (_activity.state == Activity::State::SET_MISSION_ITEM) {
LogWarn() << "Retrying send mission count...";
upload_mission_item();
}
}
}
}
}
Expand Down
15 changes: 10 additions & 5 deletions plugins/mission/mission_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class MissionImpl : public PluginImplBase {

void process_timeout();

void upload_mission_item(uint16_t seq);
void upload_mission_item();

void copy_mission_item_vector(const std::vector<std::shared_ptr<MissionItem>> &mission_items);

Expand All @@ -82,6 +82,9 @@ class MissionImpl : public PluginImplBase {
const Mission::result_callback_t callback);

void download_next_mission_item();
void request_list();
void send_count();

void assemble_mission_items();

static Mission::Result import_mission_items(Mission::mission_items_t &mission_items,
Expand All @@ -96,8 +99,10 @@ class MissionImpl : public PluginImplBase {
enum class State {
NONE,
SET_CURRENT,
SET_MISSION,
GET_MISSION,
SET_MISSION_COUNT,
SET_MISSION_ITEM,
GET_MISSION_LIST,
GET_MISSION_REQUEST,
ABORTED,
SEND_COMMAND
} state{Activity::State::NONE};
Expand All @@ -113,6 +118,7 @@ class MissionImpl : public PluginImplBase {
std::map<int, int> mavlink_mission_item_to_mission_item_indices{};
int num_mission_items_to_download{-1};
int next_mission_item_to_download{-1};
int last_mission_item_to_upload{-1};
std::vector<std::shared_ptr<mavlink_mission_item_int_t>> mavlink_mission_items_downloaded{};
Mission::result_callback_t result_callback{nullptr};
Mission::mission_items_and_result_callback_t mission_items_and_result_callback{nullptr};
Expand All @@ -129,7 +135,7 @@ class MissionImpl : public PluginImplBase {
// Ultimate it needs a setter.
bool _enable_absolute_gimbal_yaw_angle{true};

static constexpr unsigned MAX_RETRIES = 3;
static constexpr unsigned MAX_RETRIES = 10;

static constexpr uint8_t VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1;

Expand All @@ -141,7 +147,6 @@ class MissionImpl : public PluginImplBase {
static constexpr uint8_t PX4_CUSTOM_SUB_MODE_AUTO_MISSION = 4;

static constexpr double RETRY_TIMEOUT_S = 0.250;
static constexpr double PROCESS_TIMEOUT_S = 1.5;
};

} // namespace dronecode_sdk

0 comments on commit a0ff2fd

Please sign in to comment.