Skip to content

Q
qgroundcontrol
Commit 3b177f57 authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge branch 'mission_protocol' of github.com:susurrus/qgroundcontrol into thread_test_integration

parents 9cfcdb79 5c62a683
Hide whitespace changes
Inline Side-by-side
Showing with 111 additions and 74 deletions
src/uas/UAS.cc
View file @ 3b177f57
......@@ -1112,58 +1112,100 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
{
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(&message, &wpc);
if(wpc.target_system == mavlink->getSystemId() || wpc.target_system == 0)
mavlink_mission_count_t mc;
mavlink_msg_mission_count_decode(&message, &mc);
// Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
if (mc.target_system == 0) {
mc.target_system = mavlink->getSystemId();
}
if (mc.target_component == 0) {
mc.target_component = mavlink->getComponentId();
}
// Check that this message applies to the UAS.
if(mc.target_system == mavlink->getSystemId() && mc.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count);
}
else
{
qDebug() << "Got waypoint message, but was wrong system id" << wpc.target_system;
qDebug() << tr("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system);
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
{
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(&message, &wp);
//qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
if(wp.target_system == mavlink->getSystemId() || wp.target_system == 0)
mavlink_mission_item_t mi;
mavlink_msg_mission_item_decode(&message, &mi);
// Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
if (mi.target_system == 0) {
mi.target_system = mavlink->getSystemId();
}
if (mi.target_component == 0) {
mi.target_component = mavlink->getComponentId();
}
// Check that the item pertains to this UAS.
if(mi.target_system == mavlink->getSystemId() && mi.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
waypointManager.handleWaypoint(message.sysid, message.compid, &mi);
}
else
{
qDebug() << "Got waypoint message, but was wrong system id" << wp.target_system;
qDebug() << tr("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system);
}
}
break;
case MAVLINK_MSG_ID_MISSION_ACK:
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(&message, &wpa);
if((wpa.target_system == mavlink->getSystemId() || wpa.target_system == 0) &&
(wpa.target_component == mavlink->getComponentId() || wpa.target_component == 0))
mavlink_mission_ack_t ma;
mavlink_msg_mission_ack_decode(&message, &ma);
// Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
if (ma.target_system == 0) {
ma.target_system = mavlink->getSystemId();
}
if (ma.target_component == 0) {
ma.target_component = mavlink->getComponentId();
}
// Check that the ack pertains to this UAS.
if(ma.target_system == mavlink->getSystemId() && ma.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypointAck(message.sysid, message.compid, &ma);
}
else
{
waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
qDebug() << tr("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system);
}
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(&message, &wpr);
if(wpr.target_system == mavlink->getSystemId() || wpr.target_system == 0)
mavlink_mission_request_t mr;
mavlink_msg_mission_request_decode(&message, &mr);
// Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
if (mr.target_system == 0) {
mr.target_system = mavlink->getSystemId();
}
if (mr.target_component == 0) {
mr.target_component = mavlink->getComponentId();
}
// Check that the request pertains to this UAS.
if(mr.target_system == mavlink->getSystemId() && mr.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr);
}
else
{
qDebug() << "Got waypoint message, but was wrong system id" << wpr.target_system;
qDebug() << tr("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system);
}
}
break;
......
src/uas/UASWaypointManager.cc
View file @ 3b177f57
......@@ -921,94 +921,91 @@ void UASWaypointManager::writeWaypoints()
}
else
{
//we're in another transaction, ignore command
qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
// We're in another transaction, ignore command
qDebug() << tr("UASWaypointManager::sendWaypoints() doing something else. Ignoring command");
}
}
void UASWaypointManager::sendWaypointClearAll()
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_clear_all_t wpca;
wpca.target_system = uasid;
wpca.target_component = MAV_COMP_ID_MISSIONPLANNER;
emit updateStatusString(QString("Clearing waypoint list..."));
// Send the message.
mavlink_message_t message;
mavlink_mission_clear_all_t wpca = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca);
uas->sendMessage(message);
// And update the UI.
emit updateStatusString(tr("Clearing waypoint list..."));
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_set_current_t wpsc;
wpsc.target_system = uasid;
wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER;
wpsc.seq = seq;
emit updateStatusString(QString("Updating target waypoint..."));
// Send the message.
mavlink_message_t message;
mavlink_mission_set_current_t wpsc = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc);
uas->sendMessage(message);
// And update the UI.
emit updateStatusString(tr("Updating target waypoint..."));
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
void UASWaypointManager::sendWaypointCount()
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_count_t wpc;
wpc.target_system = uasid;
wpc.target_component = MAV_COMP_ID_MISSIONPLANNER;
wpc.count = current_count;
emit updateStatusString(QString("Starting to transmit waypoints..."));
// Tell the UAS how many missions we'll sending.
mavlink_message_t message;
mavlink_mission_count_t wpc = {current_count, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc);
uas->sendMessage(message);
// And update the UI.
emit updateStatusString(tr("Starting to transmit waypoints..."));
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
void UASWaypointManager::sendWaypointRequestList()
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_request_list_t wprl;
wprl.target_system = uasid;
wprl.target_component = MAV_COMP_ID_MISSIONPLANNER;
// Send a MISSION_REQUEST message to the uas for this mission manager, using the MISSIONPLANNER component.
mavlink_message_t message;
mavlink_mission_request_list_t wprl = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
uas->sendMessage(message);
// And update the UI.
QString statusMsg(tr("Requesting waypoint list..."));
qDebug() << __FILE__ << __LINE__ << statusMsg;
emit updateStatusString(statusMsg);
mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
uas->sendMessage(message);
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
void UASWaypointManager::sendWaypointRequest(quint16 seq)
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_request_t wpr;
wpr.target_system = uasid;
wpr.target_component = MAV_COMP_ID_MISSIONPLANNER;
wpr.seq = seq;
emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
// Send a MISSION_REQUEST message to the UAS's MISSIONPLANNER component.
mavlink_message_t message;
mavlink_mission_request_t wpr = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr);
uas->sendMessage(message);
// And update the UI.
emit updateStatusString(tr("Retrieving waypoint ID %1 of %2").arg(wpr.seq).arg(current_count));
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
......@@ -1019,18 +1016,18 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
if (seq < waypoint_buffer.count()) {
mavlink_mission_item_t *wp;
wp = waypoint_buffer.at(seq);
// Fetch the current mission to send, and set it to apply to the curent UAS.
mavlink_mission_item_t *wp = waypoint_buffer.at(seq);
wp->target_system = uasid;
wp->target_component = MAV_COMP_ID_MISSIONPLANNER;
emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
// Transmit the new mission
mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp);
uas->sendMessage(message);
// And update the UI.
emit updateStatusString(tr("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
}
......@@ -1038,15 +1035,13 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
void UASWaypointManager::sendWaypointAck(quint8 type)
{
if (!uas) return;
mavlink_message_t message;
mavlink_mission_ack_t wpa;
wpa.target_system = uasid;
wpa.target_component = MAV_COMP_ID_MISSIONPLANNER;
wpa.type = type;
// Send the message.
mavlink_message_t message;
mavlink_mission_ack_t wpa = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER, type};
mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa);
uas->sendMessage(message);
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
......
src/uas/UASWaypointManager.h
View file @ 3b177f57
......@@ -177,7 +177,7 @@ private:
QList<mavlink_mission_item_t *> waypoint_buffer; ///< buffer for waypoints during communication
QTimer protocol_timer; ///< Timer to catch timeouts
bool standalone; ///< If standalone is set, do not write to UAS
quint16 uasid;
int uasid; ///< The ID of the current UAS. Retrieved via `uas->getUASID();`, stored as an `int` to match its return type.
// XXX export to settings
static const float defaultAltitudeHomeOffset; ///< Altitude offset in meters from home for new waypoints
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment