4079483005

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #1976 from YalamareddyChaitanya/fix-mission-set-current-item

Run Details

1 of 1 new or added line in 1 file covered. (100.0%)

7427 of 24193 relevant lines covered (30.7%)

22.04 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

38.13

/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp

#include "mission_raw_impl.h"
#include "mission_import.h"
#include "system.h"
#include "callback_list.tpp"

#include <fstream> // for `std::ifstream`
#include <sstream> // for `std::stringstream`

namespace mavsdk {

template class CallbackList<MissionRaw::MissionProgress>;
template class CallbackList<bool>;

// This is an empty item that can be sent to ArduPilot to mimic clearing of mission.
constexpr MissionRaw::MissionItem empty_item{0, 3, 16, 1};

MissionRawImpl::MissionRawImpl(System& system) : PluginImplBase(system)
{
    _parent->register_plugin(this);
}

MissionRawImpl::MissionRawImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _parent->register_plugin(this);
}

MissionRawImpl::~MissionRawImpl()
{
    _parent->unregister_plugin(this);
}

void MissionRawImpl::init()
{
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_ACK,
        [this](const mavlink_message_t& message) { process_mission_ack(message); },
        this);

    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_CURRENT,
        [this](const mavlink_message_t& message) { process_mission_current(message); },
        this);

    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_ITEM_REACHED,
        [this](const mavlink_message_t& message) { process_mission_item_reached(message); },
        this);
}

void MissionRawImpl::enable() {}

void MissionRawImpl::disable()
{
    reset_mission_progress();
}

void MissionRawImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

void MissionRawImpl::reset_mission_progress()
{
    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    _mission_progress.last.current = -1;
    _mission_progress.last.total = -1;
    _mission_progress.last_reported.current = -1;
    _mission_progress.last_reported.total = -1;
    _mission_progress.last_reached = -1;
}

void MissionRawImpl::process_mission_ack(const mavlink_message_t& message)
{
    mavlink_mission_ack_t mission_ack;
    mavlink_msg_mission_ack_decode(&message, &mission_ack);

    if (mission_ack.type != MAV_MISSION_ACCEPTED ||
        mission_ack.mission_type != MAV_MISSION_TYPE_MISSION) {
        return;
    }

    // We assume that if the vehicle sends an ACCEPTED ack might have received
    // a new mission. In that case we need to notify our user.
    std::lock_guard<std::mutex> lock(_mission_changed.mutex);
    _mission_changed.callbacks.queue(
        true, [this](const auto& func) { _parent->call_user_callback(func); });
}

void MissionRawImpl::process_mission_current(const mavlink_message_t& message)
{
    mavlink_mission_current_t mission_current;
    mavlink_msg_mission_current_decode(&message, &mission_current);

    {
        std::lock_guard<std::mutex> lock(_mission_progress.mutex);

        // When the last waypoint is reached mission_current_seq does not increase
        // so we need to ignore that case.
        if (mission_current.seq != _mission_progress.last_reached) {
            _mission_progress.last.current = mission_current.seq;
        }
    }

    report_progress_current();
}

void MissionRawImpl::process_mission_item_reached(const mavlink_message_t& message)
{
    mavlink_mission_item_reached_t mission_item_reached;
    mavlink_msg_mission_item_reached_decode(&message, &mission_item_reached);

    {
        std::lock_guard<std::mutex> lock(_mission_progress.mutex);
        _mission_progress.last.current = mission_item_reached.seq + 1;
        _mission_progress.last_reached = mission_item_reached.seq;
    }

    report_progress_current();
}

MissionRaw::Result MissionRawImpl::upload_mission_items(
    std::vector<MissionRaw::MissionItem> mission_items, uint8_t type)
{
    auto prom = std::promise<MissionRaw::Result>();
    auto fut = prom.get_future();

    upload_mission_items_async(
        mission_items, type, [&prom](MissionRaw::Result result) { prom.set_value(result); });
    return fut.get();
}

void MissionRawImpl::upload_mission_items_async(
    const std::vector<MissionRaw::MissionItem>& mission_raw,
    uint8_t type,
    const MissionRaw::ResultCallback& callback)
{
    if (_last_upload.lock()) {
        _parent->call_user_callback([callback]() {
            if (callback) {
                callback(MissionRaw::Result::Busy);
            }
        });
        return;
    }

    reset_mission_progress();

    const auto int_items = convert_to_int_items(mission_raw);

    _last_upload = _parent->mission_transfer().upload_items_async(
        type, int_items, [this, callback, int_items](MavlinkMissionTransfer::Result result) {
            auto converted_result = convert_result(result);
            auto converted_items = convert_items(int_items);
            _parent->call_user_callback([callback, converted_result, converted_items]() {
                if (callback) {
                    callback(converted_result);
                }
            });
        });
}

MissionRaw::Result
MissionRawImpl::upload_mission(std::vector<MissionRaw::MissionItem> mission_items)
{
    return upload_mission_items(mission_items, MAV_MISSION_TYPE_MISSION);
}

void MissionRawImpl::upload_mission_async(
    const std::vector<MissionRaw::MissionItem>& mission_raw,
    const MissionRaw::ResultCallback& callback)
{
    upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_MISSION, callback);
}

MissionRaw::Result
MissionRawImpl::upload_geofence(std::vector<MissionRaw::MissionItem> mission_items)
{
    return upload_mission_items(mission_items, MAV_MISSION_TYPE_FENCE);
}

void MissionRawImpl::upload_geofence_async(
    const std::vector<MissionRaw::MissionItem>& mission_raw,
    const MissionRaw::ResultCallback& callback)
{
    upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_FENCE, callback);
}

MissionRaw::Result
MissionRawImpl::upload_rally_points(std::vector<MissionRaw::MissionItem> mission_items)
{
    return upload_mission_items(mission_items, MAV_MISSION_TYPE_RALLY);
}

void MissionRawImpl::upload_rally_points_async(
    const std::vector<MissionRaw::MissionItem>& mission_raw,
    const MissionRaw::ResultCallback& callback)
{
    upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_RALLY, callback);
}

MissionRaw::Result MissionRawImpl::cancel_mission_upload()
{
    auto ptr = _last_upload.lock();
    if (ptr) {
        ptr->cancel();
        return MissionRaw::Result::Success;
    } else {
        return MissionRaw::Result::Error;
    }
}

std::pair<MissionRaw::Result, std::vector<MissionRaw::MissionItem>>
MissionRawImpl::download_mission()
{
    auto prom = std::promise<std::pair<MissionRaw::Result, std::vector<MissionRaw::MissionItem>>>();
    auto fut = prom.get_future();

    download_mission_async(
        [&prom](MissionRaw::Result result, std::vector<MissionRaw::MissionItem> mission_items) {
            prom.set_value(std::make_pair<>(result, mission_items));
        });
    return fut.get();
}

void MissionRawImpl::download_mission_async(const MissionRaw::DownloadMissionCallback& callback)
{
    if (_last_download.lock()) {
        _parent->call_user_callback([callback]() {
            if (callback) {
                std::vector<MissionRaw::MissionItem> empty_items;
                callback(MissionRaw::Result::Busy, empty_items);
            }
        });
        return;
    }

    _last_download = _parent->mission_transfer().download_items_async(
        MAV_MISSION_TYPE_MISSION,
        [this, callback](
            MavlinkMissionTransfer::Result result,
            std::vector<MavlinkMissionTransfer::ItemInt> items) {
            auto converted_result = convert_result(result);
            auto converted_items = convert_items(items);
            _parent->call_user_callback([callback, converted_result, converted_items]() {
                callback(converted_result, converted_items);
            });
        });
}

MissionRaw::Result MissionRawImpl::cancel_mission_download()
{
    auto ptr = _last_download.lock();
    if (ptr) {
        ptr->cancel();
        return MissionRaw::Result::Success;
    } else {
        return MissionRaw::Result::Error;
    }
}

MavlinkMissionTransfer::ItemInt
MissionRawImpl::convert_mission_raw(const MissionRaw::MissionItem transfer_mission_raw)
{
    MavlinkMissionTransfer::ItemInt new_item_int;

    new_item_int.seq = transfer_mission_raw.seq;
    new_item_int.frame = transfer_mission_raw.frame;
    new_item_int.command = transfer_mission_raw.command;
    new_item_int.current = transfer_mission_raw.current;
    new_item_int.autocontinue = transfer_mission_raw.autocontinue;
    new_item_int.param1 = transfer_mission_raw.param1;
    new_item_int.param2 = transfer_mission_raw.param2;
    new_item_int.param3 = transfer_mission_raw.param3;
    new_item_int.param4 = transfer_mission_raw.param4;
    new_item_int.x = transfer_mission_raw.x;
    new_item_int.y = transfer_mission_raw.y;
    new_item_int.z = transfer_mission_raw.z;
    new_item_int.mission_type = transfer_mission_raw.mission_type;

    return new_item_int;
}

std::vector<MavlinkMissionTransfer::ItemInt>
MissionRawImpl::convert_to_int_items(const std::vector<MissionRaw::MissionItem>& mission_raw)
{
    std::vector<MavlinkMissionTransfer::ItemInt> int_items;

    for (const auto& item : mission_raw) {
        int_items.push_back(convert_mission_raw(item));
    }

    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    _mission_progress.last.total = int_items.size();

    return int_items;
}

MissionRaw::MissionItem
MissionRawImpl::convert_item(const MavlinkMissionTransfer::ItemInt& transfer_item)
{
    MissionRaw::MissionItem new_item;

    new_item.seq = transfer_item.seq;
    new_item.frame = transfer_item.frame;
    new_item.command = transfer_item.command;
    new_item.current = transfer_item.current;
    new_item.autocontinue = transfer_item.autocontinue;
    new_item.param1 = transfer_item.param1;
    new_item.param2 = transfer_item.param2;
    new_item.param3 = transfer_item.param3;
    new_item.param4 = transfer_item.param4;
    new_item.x = transfer_item.x;
    new_item.y = transfer_item.y;
    new_item.z = transfer_item.z;
    new_item.mission_type = transfer_item.mission_type;

    return new_item;
}

std::vector<MissionRaw::MissionItem>
MissionRawImpl::convert_items(const std::vector<MavlinkMissionTransfer::ItemInt>& transfer_items)
{
    std::vector<MissionRaw::MissionItem> new_items;
    new_items.reserve(transfer_items.size());

    for (const auto& transfer_item : transfer_items) {
        new_items.push_back(convert_item(transfer_item));
    }

    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    _mission_progress.last.total = new_items.size();

    return new_items;
}

MissionRaw::Result MissionRawImpl::start_mission()
{
    auto prom = std::promise<MissionRaw::Result>();
    auto fut = prom.get_future();

    start_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });
    return fut.get();
}

void MissionRawImpl::start_mission_async(const MissionRaw::ResultCallback& callback)
{
    _parent->set_flight_mode_async(
        FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) {
            report_flight_mode_change(callback, result);
        });
}

MissionRaw::Result MissionRawImpl::pause_mission()
{
    auto prom = std::promise<MissionRaw::Result>();
    auto fut = prom.get_future();

    pause_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });
    return fut.get();
}

void MissionRawImpl::pause_mission_async(const MissionRaw::ResultCallback& callback)
{
    _parent->set_flight_mode_async(
        FlightMode::Hold, [this, callback](MavlinkCommandSender::Result result, float) {
            report_flight_mode_change(callback, result);
        });
}

void MissionRawImpl::report_flight_mode_change(
    MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result)
{
    if (!callback) {
        return;
    }

    _parent->call_user_callback(
        [callback, result]() { callback(command_result_to_mission_result(result)); });
}

MissionRaw::Result
MissionRawImpl::command_result_to_mission_result(MavlinkCommandSender::Result result)
{
    switch (result) {
        case MavlinkCommandSender::Result::Success:
            return MissionRaw::Result::Success;
        case MavlinkCommandSender::Result::NoSystem:
            return MissionRaw::Result::NoSystem;
        case MavlinkCommandSender::Result::ConnectionError:
            return MissionRaw::Result::Error;
        case MavlinkCommandSender::Result::Busy:
            return MissionRaw::Result::Busy;
        case MavlinkCommandSender::Result::Denied:
            // FALLTHROUGH
        case MavlinkCommandSender::Result::TemporarilyRejected:
            return MissionRaw::Result::Denied;
        case MavlinkCommandSender::Result::Timeout:
            return MissionRaw::Result::Timeout;
        case MavlinkCommandSender::Result::InProgress:
            return MissionRaw::Result::Busy;
        case MavlinkCommandSender::Result::UnknownError:
            return MissionRaw::Result::Unknown;
        default:
            return MissionRaw::Result::Unknown;
    }
}

MissionRaw::Result MissionRawImpl::clear_mission()
{
    auto prom = std::promise<MissionRaw::Result>();
    auto fut = prom.get_future();

    clear_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });
    return fut.get();
}

void MissionRawImpl::clear_mission_async(const MissionRaw::ResultCallback& callback)
{
    reset_mission_progress();

    // For ArduPilot to clear a mission we need to upload an empty mission.
    if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) {
        std::vector<MissionRaw::MissionItem> mission_items{empty_item};
        upload_mission_async(mission_items, callback);
    } else {
        _parent->mission_transfer().clear_items_async(
            MAV_MISSION_TYPE_MISSION, [this, callback](MavlinkMissionTransfer::Result result) {
                auto converted_result = convert_result(result);
                _parent->call_user_callback([callback, converted_result]() {
                    if (callback) {
                        callback(converted_result);
                    }
                });
            });
    }
}

MissionRaw::Result MissionRawImpl::set_current_mission_item(int index)
{
    auto prom = std::promise<MissionRaw::Result>();
    auto fut = prom.get_future();

    set_current_mission_item_async(
        index, [&prom](MissionRaw::Result result) { prom.set_value(result); });
    return fut.get();
}

void MissionRawImpl::set_current_mission_item_async(
    int index, const MissionRaw::ResultCallback& callback)
{
    if (index < 0 || index >= _mission_progress.last.total) {
        _parent->call_user_callback([callback]() {
            if (callback) {
                callback(MissionRaw::Result::InvalidArgument);
                return;
            }
        });
    }

    _parent->mission_transfer().set_current_item_async(
        index, [this, callback](MavlinkMissionTransfer::Result result) {
            auto converted_result = convert_result(result);
            _parent->call_user_callback([callback, converted_result]() {
                if (callback) {
                    callback(converted_result);
                }
            });
        });
}

void MissionRawImpl::report_progress_current()
{
    std::lock_guard<std::mutex> lock(_mission_progress.mutex);

    if (_mission_progress.callbacks.empty()) {
        return;
    }

    bool should_report = false;
    {
        if (_mission_progress.last_reported.current != _mission_progress.last.current) {
            _mission_progress.last_reported.current = _mission_progress.last.current;
            should_report = true;
        }
        if (_mission_progress.last_reported.total != _mission_progress.last.total) {
            _mission_progress.last_reported.total = _mission_progress.last.total;
            should_report = true;
        }
    }

    if (should_report) {
        _mission_progress.callbacks.queue(_mission_progress.last, [this](const auto& func) {
            _parent->call_user_callback(func);
        });
    }
}

MissionRaw::MissionProgressHandle
MissionRawImpl::subscribe_mission_progress(const MissionRaw::MissionProgressCallback& callback)
{
    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    return _mission_progress.callbacks.subscribe(callback);
}

void MissionRawImpl::unsubscribe_mission_progress(MissionRaw::MissionProgressHandle handle)
{
    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    _mission_progress.callbacks.unsubscribe(handle);
}

MissionRaw::MissionProgress MissionRawImpl::mission_progress()
{
    std::lock_guard<std::mutex> lock(_mission_progress.mutex);
    return _mission_progress.last;
}

MissionRaw::MissionChangedHandle
MissionRawImpl::subscribe_mission_changed(const MissionRaw::MissionChangedCallback& callback)
{
    std::lock_guard<std::mutex> lock(_mission_changed.mutex);
    return _mission_changed.callbacks.subscribe(callback);
}

void MissionRawImpl::unsubscribe_mission_changed(MissionRaw::MissionChangedHandle handle)
{
    std::lock_guard<std::mutex> lock(_mission_changed.mutex);
    _mission_changed.callbacks.unsubscribe(handle);
}

std::pair<MissionRaw::Result, MissionRaw::MissionImportData>
MissionRawImpl::import_qgroundcontrol_mission(std::string qgc_plan_path)
{
    std::ifstream file(qgc_plan_path);
    if (!file) {
        return std::make_pair<MissionRaw::Result, MissionRaw::MissionImportData>(
            MissionRaw::Result::FailedToOpenQgcPlan, {});
    }

    std::stringstream buf;
    buf << file.rdbuf();
    file.close();

    return MissionImport::parse_json(buf.str(), _parent->autopilot());
}

std::pair<MissionRaw::Result, MissionRaw::MissionImportData>
MissionRawImpl::import_qgroundcontrol_mission_from_string(const std::string& qgc_plan)
{
    return MissionImport::parse_json(qgc_plan, _parent->autopilot());
}

MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransfer::Result result)
{
    switch (result) {
        case MavlinkMissionTransfer::Result::Success:
            return MissionRaw::Result::Success;
        case MavlinkMissionTransfer::Result::ConnectionError:
            return MissionRaw::Result::Error;
        case MavlinkMissionTransfer::Result::Denied:
            return MissionRaw::Result::Denied;
        case MavlinkMissionTransfer::Result::TooManyMissionItems:
            return MissionRaw::Result::TooManyMissionItems;
        case MavlinkMissionTransfer::Result::Timeout:
            return MissionRaw::Result::Timeout;
        case MavlinkMissionTransfer::Result::Unsupported:
            return MissionRaw::Result::Unsupported;
        case MavlinkMissionTransfer::Result::UnsupportedFrame:
            return MissionRaw::Result::Unsupported;
        case MavlinkMissionTransfer::Result::NoMissionAvailable:
            return MissionRaw::Result::NoMissionAvailable;
        case MavlinkMissionTransfer::Result::Cancelled:
            return MissionRaw::Result::TransferCancelled;
        case MavlinkMissionTransfer::Result::MissionTypeNotConsistent:
            return MissionRaw::Result::MissionTypeNotConsistent;
        case MavlinkMissionTransfer::Result::InvalidSequence:
            return MissionRaw::Result::InvalidSequence;
        case MavlinkMissionTransfer::Result::CurrentInvalid:
            return MissionRaw::Result::CurrentInvalid;
        case MavlinkMissionTransfer::Result::ProtocolError:
            return MissionRaw::Result::ProtocolError;
        case MavlinkMissionTransfer::Result::InvalidParam:
            return MissionRaw::Result::InvalidArgument;
        case MavlinkMissionTransfer::Result::IntMessagesNotSupported:
            return MissionRaw::Result::IntMessagesNotSupported;
        default:
            return MissionRaw::Result::Unknown;
    }
}
} // namespace mavsdk

1	#include "mission_raw_impl.h"
2	#include "mission_import.h"
3	#include "system.h"
4	#include "callback_list.tpp"
5
6	#include <fstream> // for `std::ifstream`
7	#include <sstream> // for `std::stringstream`
8
9	namespace mavsdk {
10
11	template class CallbackList<MissionRaw::MissionProgress>;
12	template class CallbackList<bool>;
13
14	// This is an empty item that can be sent to ArduPilot to mimic clearing of mission.
15	constexpr MissionRaw::MissionItem empty_item{0, 3, 16, 1};
16
17	MissionRawImpl::MissionRawImpl(System& system) : PluginImplBase(system)	×
18	{
19	_parent->register_plugin(this);	×
20	}	×
21
22	MissionRawImpl::MissionRawImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	1✔
23	{
24	_parent->register_plugin(this);	1✔
25	}	1✔
26
27	MissionRawImpl::~MissionRawImpl()	1✔
28	{
29	_parent->unregister_plugin(this);	1✔
30	}	1✔
31
32	void MissionRawImpl::init()	1✔
33	{
34	_parent->register_mavlink_message_handler(	1✔
35	MAVLINK_MSG_ID_MISSION_ACK,
36	[this](const mavlink_message_t& message) { process_mission_ack(message); },	1✔
37	this);
38
39	_parent->register_mavlink_message_handler(	1✔
40	MAVLINK_MSG_ID_MISSION_CURRENT,
41	[this](const mavlink_message_t& message) { process_mission_current(message); },	1✔
42	this);
43
44	_parent->register_mavlink_message_handler(	1✔
45	MAVLINK_MSG_ID_MISSION_ITEM_REACHED,
46	[this](const mavlink_message_t& message) { process_mission_item_reached(message); },	×
47	this);
48	}	1✔
49
50	void MissionRawImpl::enable() {}	1✔
51
52	void MissionRawImpl::disable()	1✔
53	{
54	reset_mission_progress();	1✔
55	}	1✔
56
57	void MissionRawImpl::deinit()	1✔
58	{
59	_parent->unregister_all_mavlink_message_handlers(this);	1✔
60	}	1✔
61
62	void MissionRawImpl::reset_mission_progress()	2✔
63	{
64	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	2✔
65	_mission_progress.last.current = -1;	2✔
66	_mission_progress.last.total = -1;	2✔
67	_mission_progress.last_reported.current = -1;	2✔
68	_mission_progress.last_reported.total = -1;	2✔
69	_mission_progress.last_reached = -1;	2✔
70	}	2✔
71
72	void MissionRawImpl::process_mission_ack(const mavlink_message_t& message)	1✔
73	{
74	mavlink_mission_ack_t mission_ack;	1✔
75	mavlink_msg_mission_ack_decode(&message, &mission_ack);	1✔
76
77	if (mission_ack.type != MAV_MISSION_ACCEPTED \|\|	1✔
78	mission_ack.mission_type != MAV_MISSION_TYPE_MISSION) {	1✔
79	return;	×
80	}
81
82	// We assume that if the vehicle sends an ACCEPTED ack might have received
83	// a new mission. In that case we need to notify our user.
84	std::lock_guard<std::mutex> lock(_mission_changed.mutex);	2✔
85	_mission_changed.callbacks.queue(	1✔
86	true, [this](const auto& func) { _parent->call_user_callback(func); });	×
87	}
88
89	void MissionRawImpl::process_mission_current(const mavlink_message_t& message)	1✔
90	{
91	mavlink_mission_current_t mission_current;	1✔
92	mavlink_msg_mission_current_decode(&message, &mission_current);	1✔
93
94	{
95	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	2✔
96
97	// When the last waypoint is reached mission_current_seq does not increase
98	// so we need to ignore that case.
99	if (mission_current.seq != _mission_progress.last_reached) {	1✔
100	_mission_progress.last.current = mission_current.seq;	1✔
101	}
102	}
103
104	report_progress_current();	1✔
105	}	1✔
106
107	void MissionRawImpl::process_mission_item_reached(const mavlink_message_t& message)	×
108	{
109	mavlink_mission_item_reached_t mission_item_reached;	×
110	mavlink_msg_mission_item_reached_decode(&message, &mission_item_reached);	×
111
112	{
113	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	×
114	_mission_progress.last.current = mission_item_reached.seq + 1;	×
115	_mission_progress.last_reached = mission_item_reached.seq;	×
116	}
117
118	report_progress_current();	×
119	}	×
120
121	MissionRaw::Result MissionRawImpl::upload_mission_items(	1✔
122	std::vector<MissionRaw::MissionItem> mission_items, uint8_t type)
123	{
124	auto prom = std::promise<MissionRaw::Result>();	2✔
125	auto fut = prom.get_future();	2✔
126
127	upload_mission_items_async(	1✔
128	mission_items, type, [&prom](MissionRaw::Result result) { prom.set_value(result); });	1✔
129	return fut.get();	1✔
130	}
131
132	void MissionRawImpl::upload_mission_items_async(	1✔
133	const std::vector<MissionRaw::MissionItem>& mission_raw,
134	uint8_t type,
135	const MissionRaw::ResultCallback& callback)
136	{
137	if (_last_upload.lock()) {	1✔
138	_parent->call_user_callback([callback]() {	×
139	if (callback) {
140	callback(MissionRaw::Result::Busy);
141	}
142	});
143	return;	×
144	}
145
146	reset_mission_progress();	1✔
147
148	const auto int_items = convert_to_int_items(mission_raw);	2✔
149
150	_last_upload = _parent->mission_transfer().upload_items_async(	2✔
151	type, int_items, [this, callback, int_items](MavlinkMissionTransfer::Result result) {	3✔
152	auto converted_result = convert_result(result);	1✔
153	auto converted_items = convert_items(int_items);	2✔
154	_parent->call_user_callback([callback, converted_result, converted_items]() {	2✔
155	if (callback) {
156	callback(converted_result);
157	}
158	});
159	});	1✔
160	}
161
162	MissionRaw::Result
163	MissionRawImpl::upload_mission(std::vector<MissionRaw::MissionItem> mission_items)	1✔
164	{
165	return upload_mission_items(mission_items, MAV_MISSION_TYPE_MISSION);	1✔
166	}
167
168	void MissionRawImpl::upload_mission_async(	×
169	const std::vector<MissionRaw::MissionItem>& mission_raw,
170	const MissionRaw::ResultCallback& callback)
171	{
172	upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_MISSION, callback);	×
173	}	×
174
175	MissionRaw::Result
176	MissionRawImpl::upload_geofence(std::vector<MissionRaw::MissionItem> mission_items)	×
177	{
178	return upload_mission_items(mission_items, MAV_MISSION_TYPE_FENCE);	×
179	}
180
181	void MissionRawImpl::upload_geofence_async(	×
182	const std::vector<MissionRaw::MissionItem>& mission_raw,
183	const MissionRaw::ResultCallback& callback)
184	{
185	upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_FENCE, callback);	×
186	}	×
187
188	MissionRaw::Result
189	MissionRawImpl::upload_rally_points(std::vector<MissionRaw::MissionItem> mission_items)	×
190	{
191	return upload_mission_items(mission_items, MAV_MISSION_TYPE_RALLY);	×
192	}
193
194	void MissionRawImpl::upload_rally_points_async(	×
195	const std::vector<MissionRaw::MissionItem>& mission_raw,
196	const MissionRaw::ResultCallback& callback)
197	{
198	upload_mission_items_async(mission_raw, MAV_MISSION_TYPE_RALLY, callback);	×
199	}	×
200
201	MissionRaw::Result MissionRawImpl::cancel_mission_upload()	×
202	{
203	auto ptr = _last_upload.lock();	×
204	if (ptr) {	×
205	ptr->cancel();	×
206	return MissionRaw::Result::Success;	×
207	} else {
208	return MissionRaw::Result::Error;	×
209	}
210	}
211
212	std::pair<MissionRaw::Result, std::vector<MissionRaw::MissionItem>>
213	MissionRawImpl::download_mission()	×
214	{
215	auto prom = std::promise<std::pair<MissionRaw::Result, std::vector<MissionRaw::MissionItem>>>();	×
216	auto fut = prom.get_future();	×
217
218	download_mission_async(	×
219	[&prom](MissionRaw::Result result, std::vector<MissionRaw::MissionItem> mission_items) {	×
220	prom.set_value(std::make_pair<>(result, mission_items));	×
221	});	×
222	return fut.get();	×
223	}
224
225	void MissionRawImpl::download_mission_async(const MissionRaw::DownloadMissionCallback& callback)	×
226	{
227	if (_last_download.lock()) {	×
228	_parent->call_user_callback([callback]() {	×
229	if (callback) {
230	std::vector<MissionRaw::MissionItem> empty_items;
231	callback(MissionRaw::Result::Busy, empty_items);
232	}
233	});
234	return;	×
235	}
236
237	_last_download = _parent->mission_transfer().download_items_async(	×
238	MAV_MISSION_TYPE_MISSION,
239	[this, callback](	×
240	MavlinkMissionTransfer::Result result,
241	std::vector<MavlinkMissionTransfer::ItemInt> items) {	×
242	auto converted_result = convert_result(result);	×
243	auto converted_items = convert_items(items);	×
244	_parent->call_user_callback([callback, converted_result, converted_items]() {	×
245	callback(converted_result, converted_items);
246	});
247	});	×
248	}
249
250	MissionRaw::Result MissionRawImpl::cancel_mission_download()	×
251	{
252	auto ptr = _last_download.lock();	×
253	if (ptr) {	×
254	ptr->cancel();	×
255	return MissionRaw::Result::Success;	×
256	} else {
257	return MissionRaw::Result::Error;	×
258	}
259	}
260
261	MavlinkMissionTransfer::ItemInt
262	MissionRawImpl::convert_mission_raw(const MissionRaw::MissionItem transfer_mission_raw)	6✔
263	{
264	MavlinkMissionTransfer::ItemInt new_item_int;
265
266	new_item_int.seq = transfer_mission_raw.seq;	6✔
267	new_item_int.frame = transfer_mission_raw.frame;	6✔
268	new_item_int.command = transfer_mission_raw.command;	6✔
269	new_item_int.current = transfer_mission_raw.current;	6✔
270	new_item_int.autocontinue = transfer_mission_raw.autocontinue;	6✔
271	new_item_int.param1 = transfer_mission_raw.param1;	6✔
272	new_item_int.param2 = transfer_mission_raw.param2;	6✔
273	new_item_int.param3 = transfer_mission_raw.param3;	6✔
274	new_item_int.param4 = transfer_mission_raw.param4;	6✔
275	new_item_int.x = transfer_mission_raw.x;	6✔
276	new_item_int.y = transfer_mission_raw.y;	6✔
277	new_item_int.z = transfer_mission_raw.z;	6✔
278	new_item_int.mission_type = transfer_mission_raw.mission_type;	6✔
279
280	return new_item_int;	6✔
281	}
282
283	std::vector<MavlinkMissionTransfer::ItemInt>
284	MissionRawImpl::convert_to_int_items(const std::vector<MissionRaw::MissionItem>& mission_raw)	1✔
285	{
286	std::vector<MavlinkMissionTransfer::ItemInt> int_items;	1✔
287
288	for (const auto& item : mission_raw) {	7✔
289	int_items.push_back(convert_mission_raw(item));	6✔
290	}
291
292	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	2✔
293	_mission_progress.last.total = int_items.size();	1✔
294
295	return int_items;	1✔
296	}
297
298	MissionRaw::MissionItem
299	MissionRawImpl::convert_item(const MavlinkMissionTransfer::ItemInt& transfer_item)	6✔
300	{
301	MissionRaw::MissionItem new_item;	6✔
302
303	new_item.seq = transfer_item.seq;	6✔
304	new_item.frame = transfer_item.frame;	6✔
305	new_item.command = transfer_item.command;	6✔
306	new_item.current = transfer_item.current;	6✔
307	new_item.autocontinue = transfer_item.autocontinue;	6✔
308	new_item.param1 = transfer_item.param1;	6✔
309	new_item.param2 = transfer_item.param2;	6✔
310	new_item.param3 = transfer_item.param3;	6✔
311	new_item.param4 = transfer_item.param4;	6✔
312	new_item.x = transfer_item.x;	6✔
313	new_item.y = transfer_item.y;	6✔
314	new_item.z = transfer_item.z;	6✔
315	new_item.mission_type = transfer_item.mission_type;	6✔
316
317	return new_item;	6✔
318	}
319
320	std::vector<MissionRaw::MissionItem>
321	MissionRawImpl::convert_items(const std::vector<MavlinkMissionTransfer::ItemInt>& transfer_items)	1✔
322	{
323	std::vector<MissionRaw::MissionItem> new_items;	1✔
324	new_items.reserve(transfer_items.size());	1✔
325
326	for (const auto& transfer_item : transfer_items) {	7✔
327	new_items.push_back(convert_item(transfer_item));	6✔
328	}
329
330	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	2✔
331	_mission_progress.last.total = new_items.size();	1✔
332
333	return new_items;	1✔
334	}
335
336	MissionRaw::Result MissionRawImpl::start_mission()	×
337	{
338	auto prom = std::promise<MissionRaw::Result>();	×
339	auto fut = prom.get_future();	×
340
341	start_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });	×
342	return fut.get();	×
343	}
344
345	void MissionRawImpl::start_mission_async(const MissionRaw::ResultCallback& callback)	×
346	{
347	_parent->set_flight_mode_async(	×
348	FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) {	×
349	report_flight_mode_change(callback, result);	×
350	});	×
351	}	×
352
353	MissionRaw::Result MissionRawImpl::pause_mission()	×
354	{
355	auto prom = std::promise<MissionRaw::Result>();	×
356	auto fut = prom.get_future();	×
357
358	pause_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });	×
359	return fut.get();	×
360	}
361
362	void MissionRawImpl::pause_mission_async(const MissionRaw::ResultCallback& callback)	×
363	{
364	_parent->set_flight_mode_async(	×
365	FlightMode::Hold, [this, callback](MavlinkCommandSender::Result result, float) {	×
366	report_flight_mode_change(callback, result);	×
367	});	×
368	}	×
369
370	void MissionRawImpl::report_flight_mode_change(	×
371	MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result)
372	{
373	if (!callback) {	×
374	return;	×
375	}
376
377	_parent->call_user_callback(	×
378	[callback, result]() { callback(command_result_to_mission_result(result)); });
379	}
380
381	MissionRaw::Result
382	MissionRawImpl::command_result_to_mission_result(MavlinkCommandSender::Result result)	×
383	{
384	switch (result) {	×
385	case MavlinkCommandSender::Result::Success:	×
386	return MissionRaw::Result::Success;	×
387	case MavlinkCommandSender::Result::NoSystem:	×
388	return MissionRaw::Result::NoSystem;	×
389	case MavlinkCommandSender::Result::ConnectionError:	×
390	return MissionRaw::Result::Error;	×
391	case MavlinkCommandSender::Result::Busy:	×
392	return MissionRaw::Result::Busy;	×
393	case MavlinkCommandSender::Result::Denied:	×
394	// FALLTHROUGH
395	case MavlinkCommandSender::Result::TemporarilyRejected:
396	return MissionRaw::Result::Denied;	×
397	case MavlinkCommandSender::Result::Timeout:	×
398	return MissionRaw::Result::Timeout;	×
399	case MavlinkCommandSender::Result::InProgress:	×
400	return MissionRaw::Result::Busy;	×
401	case MavlinkCommandSender::Result::UnknownError:	×
402	return MissionRaw::Result::Unknown;	×
403	default:	×
404	return MissionRaw::Result::Unknown;	×
405	}
406	}
407
408	MissionRaw::Result MissionRawImpl::clear_mission()	×
409	{
410	auto prom = std::promise<MissionRaw::Result>();	×
411	auto fut = prom.get_future();	×
412
413	clear_mission_async([&prom](MissionRaw::Result result) { prom.set_value(result); });	×
414	return fut.get();	×
415	}
416
417	void MissionRawImpl::clear_mission_async(const MissionRaw::ResultCallback& callback)	×
418	{
419	reset_mission_progress();	×
420
421	// For ArduPilot to clear a mission we need to upload an empty mission.
422	if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) {	×
423	std::vector<MissionRaw::MissionItem> mission_items{empty_item};	×
424	upload_mission_async(mission_items, callback);	×
425	} else {
426	_parent->mission_transfer().clear_items_async(	×
427	MAV_MISSION_TYPE_MISSION, [this, callback](MavlinkMissionTransfer::Result result) {	×
428	auto converted_result = convert_result(result);	×
429	_parent->call_user_callback([callback, converted_result]() {	×
430	if (callback) {
431	callback(converted_result);
432	}
433	});
434	});	×
435	}
436	}	×
437
438	MissionRaw::Result MissionRawImpl::set_current_mission_item(int index)	×
439	{
440	auto prom = std::promise<MissionRaw::Result>();	×
441	auto fut = prom.get_future();	×
442
443	set_current_mission_item_async(	×
444	index, [&prom](MissionRaw::Result result) { prom.set_value(result); });	×
445	return fut.get();	×
446	}
447
448	void MissionRawImpl::set_current_mission_item_async(	×
449	int index, const MissionRaw::ResultCallback& callback)
450	{
451	if (index < 0 \|\| index >= _mission_progress.last.total) {	×
452	_parent->call_user_callback([callback]() {	×
453	if (callback) {
454	callback(MissionRaw::Result::InvalidArgument);
455	return;
456	}
457	});
458	}
459
460	_parent->mission_transfer().set_current_item_async(	×
461	index, [this, callback](MavlinkMissionTransfer::Result result) {	×
462	auto converted_result = convert_result(result);	×
463	_parent->call_user_callback([callback, converted_result]() {	×
464	if (callback) {
465	callback(converted_result);
466	}
467	});
468	});	×
469	}	×
470
471	void MissionRawImpl::report_progress_current()	1✔
472	{
473	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	1✔
474
475	if (_mission_progress.callbacks.empty()) {	1✔
476	return;	1✔
477	}
478
479	bool should_report = false;	×
480	{
481	if (_mission_progress.last_reported.current != _mission_progress.last.current) {	×
482	_mission_progress.last_reported.current = _mission_progress.last.current;	×
483	should_report = true;	×
484	}
485	if (_mission_progress.last_reported.total != _mission_progress.last.total) {	×
486	_mission_progress.last_reported.total = _mission_progress.last.total;	×
487	should_report = true;	×
488	}
489	}
490
491	if (should_report) {	×
492	_mission_progress.callbacks.queue(_mission_progress.last, [this](const auto& func) {	×
493	_parent->call_user_callback(func);	×
494	});	×
495	}
496	}
497
498	MissionRaw::MissionProgressHandle
499	MissionRawImpl::subscribe_mission_progress(const MissionRaw::MissionProgressCallback& callback)	×
500	{
501	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	×
502	return _mission_progress.callbacks.subscribe(callback);	×
503	}
504
505	void MissionRawImpl::unsubscribe_mission_progress(MissionRaw::MissionProgressHandle handle)	×
506	{
507	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	×
508	_mission_progress.callbacks.unsubscribe(handle);	×
509	}	×
510
511	MissionRaw::MissionProgress MissionRawImpl::mission_progress()	×
512	{
513	std::lock_guard<std::mutex> lock(_mission_progress.mutex);	×
514	return _mission_progress.last;	×
515	}
516
517	MissionRaw::MissionChangedHandle
518	MissionRawImpl::subscribe_mission_changed(const MissionRaw::MissionChangedCallback& callback)	×
519	{
520	std::lock_guard<std::mutex> lock(_mission_changed.mutex);	×
521	return _mission_changed.callbacks.subscribe(callback);	×
522	}
523
524	void MissionRawImpl::unsubscribe_mission_changed(MissionRaw::MissionChangedHandle handle)	×
525	{
526	std::lock_guard<std::mutex> lock(_mission_changed.mutex);	×
527	_mission_changed.callbacks.unsubscribe(handle);	×
528	}	×
529
530	std::pair<MissionRaw::Result, MissionRaw::MissionImportData>
531	MissionRawImpl::import_qgroundcontrol_mission(std::string qgc_plan_path)	1✔
532	{
533	std::ifstream file(qgc_plan_path);	2✔
534	if (!file) {	1✔
535	return std::make_pair<MissionRaw::Result, MissionRaw::MissionImportData>(
536	MissionRaw::Result::FailedToOpenQgcPlan, {});	×
537	}
538
539	std::stringstream buf;	2✔
540	buf << file.rdbuf();	1✔
541	file.close();	1✔
542
543	return MissionImport::parse_json(buf.str(), _parent->autopilot());	1✔
544	}
545
546	std::pair<MissionRaw::Result, MissionRaw::MissionImportData>
547	MissionRawImpl::import_qgroundcontrol_mission_from_string(const std::string& qgc_plan)	×
548	{
549	return MissionImport::parse_json(qgc_plan, _parent->autopilot());	×
550	}
551
552	MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransfer::Result result)	1✔
553	{
554	switch (result) {	1✔
555	case MavlinkMissionTransfer::Result::Success:	1✔
556	return MissionRaw::Result::Success;	1✔
557	case MavlinkMissionTransfer::Result::ConnectionError:	×
558	return MissionRaw::Result::Error;	×
559	case MavlinkMissionTransfer::Result::Denied:	×
560	return MissionRaw::Result::Denied;	×
561	case MavlinkMissionTransfer::Result::TooManyMissionItems:	×
562	return MissionRaw::Result::TooManyMissionItems;	×
563	case MavlinkMissionTransfer::Result::Timeout:	×
564	return MissionRaw::Result::Timeout;	×
565	case MavlinkMissionTransfer::Result::Unsupported:	×
566	return MissionRaw::Result::Unsupported;	×
567	case MavlinkMissionTransfer::Result::UnsupportedFrame:	×
568	return MissionRaw::Result::Unsupported;	×
569	case MavlinkMissionTransfer::Result::NoMissionAvailable:	×
570	return MissionRaw::Result::NoMissionAvailable;	×
571	case MavlinkMissionTransfer::Result::Cancelled:	×
572	return MissionRaw::Result::TransferCancelled;	×
573	case MavlinkMissionTransfer::Result::MissionTypeNotConsistent:	×
574	return MissionRaw::Result::MissionTypeNotConsistent;	×
575	case MavlinkMissionTransfer::Result::InvalidSequence:	×
576	return MissionRaw::Result::InvalidSequence;	×
577	case MavlinkMissionTransfer::Result::CurrentInvalid:	×
578	return MissionRaw::Result::CurrentInvalid;	×
579	case MavlinkMissionTransfer::Result::ProtocolError:	×
580	return MissionRaw::Result::ProtocolError;	×
581	case MavlinkMissionTransfer::Result::InvalidParam:	×
582	return MissionRaw::Result::InvalidArgument;	×
583	case MavlinkMissionTransfer::Result::IntMessagesNotSupported:	×
584	return MissionRaw::Result::IntMessagesNotSupported;	×
585	default:	×
586	return MissionRaw::Result::Unknown;	×
587	}
588	}
589	} // namespace mavsdk

mavlink / MAVSDK / 4079483005

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous