diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 557a74df92ac1f179f25e2d30047313ba942706b..0022109ee5abcaf5ab8c0a708fd81dc90ea38312 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -46,13 +46,30 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) current_partner_systemid(0), current_partner_compid(0), protocol_timer(this), - currentWaypointEditable(NULL) + currentWaypointEditable(NULL), + standalone(false), + uasid(_uasid) { connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); connect(&uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); connect(&uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,quint64))); } +UASWaypointManager::UASWaypointManager() + : current_retries(0), + current_wp_id(0), + current_count(0), + current_state(WP_IDLE), + current_partner_systemid(0), + current_partner_compid(0), + protocol_timer(this), + currentWaypointEditable(NULL), + standalone(true), + uasid(0) +{ + connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); +} + void UASWaypointManager::timeout() { if (current_retries > 0) { @@ -224,14 +241,16 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr) { - if (systemId == uas.getUASID()) { + if (standalone) return; + if (systemId == uasid) { emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq)); } } void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc) { - if (systemId == uas.getUASID()) { + if (standalone) return; + if (systemId == uasid) { // FIXME Petri if (current_state == WP_SETCURRENT) { protocol_timer.stop(); @@ -260,20 +279,20 @@ void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp) // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId(); // If only one waypoint was changed, emit only WP signal if (wp != NULL) { - emit waypointEditableChanged(uas.getUASID(), wp); + emit waypointEditableChanged(uasid, wp); } else { emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } void UASWaypointManager::notifyOfChangeViewOnly(Waypoint* wp) { if (wp != NULL) { - emit waypointViewOnlyChanged(uas.getUASID(), wp); + emit waypointViewOnlyChanged(uasid, wp); } else { emit waypointViewOnlyListChanged(); - emit waypointViewOnlyListChanged(uas.getUASID()); + emit waypointViewOnlyListChanged(uasid); } } @@ -289,7 +308,7 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) current_state = WP_SETCURRENT; current_wp_id = seq; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; sendWaypointSetCurrent(current_wp_id); @@ -330,7 +349,7 @@ void UASWaypointManager::addWaypointViewOnly(Waypoint *wp) connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeViewOnly(Waypoint*))); emit waypointViewOnlyListChanged(); - emit waypointViewOnlyListChanged(uas.getUASID()); + emit waypointViewOnlyListChanged(uasid); } } @@ -354,7 +373,7 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } @@ -374,7 +393,7 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); return wp; } @@ -393,7 +412,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq) } emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); return 0; } return -1; @@ -420,7 +439,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq) waypointsEditable[new_seq] = t; emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } } @@ -486,7 +505,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) emit loadWPFile(); emit waypointEditableListChanged(); - emit waypointEditableListChanged(uas.getUASID()); + emit waypointEditableListChanged(uasid); } void UASWaypointManager::clearWaypointList() @@ -498,7 +517,7 @@ void UASWaypointManager::clearWaypointList() current_state = WP_CLEARLIST; current_wp_id = 0; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; sendWaypointClearAll(); @@ -746,7 +765,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit) current_state = WP_GETLIST; current_wp_id = 0; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; sendWaypointRequestList(); @@ -765,7 +784,7 @@ void UASWaypointManager::writeWaypoints() current_count = waypointsEditable.count(); current_state = WP_SENDLIST; current_wp_id = 0; - current_partner_systemid = uas.getUASID(); + current_partner_systemid = uasid; current_partner_compid = MAV_COMP_ID_MISSIONPLANNER; //clear local buffer @@ -821,16 +840,17 @@ void UASWaypointManager::writeWaypoints() void UASWaypointManager::sendWaypointClearAll() { + if (standalone) return; mavlink_message_t message; mavlink_mission_clear_all_t wpca; - wpca.target_system = uas.getUASID(); + wpca.target_system = uasid; wpca.target_component = MAV_COMP_ID_MISSIONPLANNER; emit updateStatusString(QString("Clearing waypoint list...")); mavlink_msg_mission_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint clear all to ID " << wpca.target_system; @@ -838,17 +858,18 @@ void UASWaypointManager::sendWaypointClearAll() void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) { + if (standalone) return; mavlink_message_t message; mavlink_mission_set_current_t wpsc; - wpsc.target_system = uas.getUASID(); + wpsc.target_system = uasid; wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER; wpsc.seq = seq; emit updateStatusString(QString("Updating target waypoint...")); mavlink_msg_mission_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system; @@ -856,10 +877,11 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) void UASWaypointManager::sendWaypointCount() { + if (standalone) return; mavlink_message_t message; mavlink_mission_count_t wpc; - wpc.target_system = uas.getUASID(); + wpc.target_system = uasid; wpc.target_component = MAV_COMP_ID_MISSIONPLANNER; wpc.count = current_count; @@ -867,7 +889,7 @@ void UASWaypointManager::sendWaypointCount() emit updateStatusString(QString("Starting to transmit waypoints...")); mavlink_msg_mission_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; @@ -875,16 +897,17 @@ void UASWaypointManager::sendWaypointCount() void UASWaypointManager::sendWaypointRequestList() { + if (standalone) return; mavlink_message_t message; mavlink_mission_request_list_t wprl; - wprl.target_system = uas.getUASID(); + wprl.target_system = uasid; wprl.target_component = MAV_COMP_ID_MISSIONPLANNER; emit updateStatusString(QString("Requesting waypoint list...")); mavlink_msg_mission_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint list request to ID " << wprl.target_system; @@ -894,17 +917,18 @@ void UASWaypointManager::sendWaypointRequestList() void UASWaypointManager::sendWaypointRequest(quint16 seq) { + if (standalone) return; mavlink_message_t message; mavlink_mission_request_t wpr; - wpr.target_system = uas.getUASID(); + 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)); mavlink_msg_mission_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system; @@ -912,6 +936,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) void UASWaypointManager::sendWaypoint(quint16 seq) { + if (standalone) return; mavlink_message_t message; // // qDebug() <<" WP Buffer count: "<target_system = uas.getUASID(); + 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)); @@ -929,7 +954,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq) // // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<getSystemId(), uas.mavlink->getComponentId(), &message, wp); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); } @@ -937,15 +962,16 @@ void UASWaypointManager::sendWaypoint(quint16 seq) void UASWaypointManager::sendWaypointAck(quint8 type) { + if (standalone) return; mavlink_message_t message; mavlink_mission_ack_t wpa; - wpa.target_system = uas.getUASID(); + wpa.target_system = uasid; wpa.target_component = MAV_COMP_ID_MISSIONPLANNER; wpa.type = type; mavlink_msg_mission_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa); - uas.sendMessage(message); + if (!standalone) uas.sendMessage(message); MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); // // qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system; diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 6f35996cb6a4ba0e207e52fc83d15d2f2b0350c7..ecb03554d9b89baf726de1eb61ae799e32362557 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -64,7 +64,8 @@ private: }; ///< The possible states for the waypoint protocol public: - UASWaypointManager(UAS&); ///< Standard constructor. + UASWaypointManager(UAS&); ///< Standard constructor + UASWaypointManager(); ///< Standalone mode /** @name Received message handlers */ /*@{*/ @@ -171,6 +172,8 @@ private: Waypoint* currentWaypointEditable; ///< The currently used waypoint QVector 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 + int uasid; }; #endif // UASWAYPOINTMANAGER_H