diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 2ca433d0661015535ffa7a00f19d35f9b3d8fde1..5e7a3d6447027fc820bcb27e2d0a7ac5aabe2aba 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -33,10 +33,12 @@ This file is part of the PIXHAWK project #include "UAS.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout +#define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages #define PROTOCOL_MAX_RETRIES 3 ///< maximum number of send retries (after timeout) UASWaypointManager::UASWaypointManager(UAS &_uas) : uas(_uas), + current_retries(0), current_wp_id(0), current_count(0), current_state(WP_IDLE), @@ -49,25 +51,59 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) void UASWaypointManager::timeout() { - protocol_timer.stop(); + if (current_retries > 0) + { + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries--; + qDebug() << "Timeout, retrying (retries left:" << current_retries << ")"; + if (current_state == WP_GETLIST) + { + sendWaypointRequestList(); + } + else if (current_state == WP_GETLIST_GETWPS) + { + sendWaypointRequest(current_wp_id); + } + else if (current_state == WP_SENDLIST) + { + sendWaypointCount(); + } + else if (current_state == WP_SENDLIST_SENDWPS) + { + sendWaypoint(current_wp_id); + } + else if (current_state == WP_CLEARLIST) + { + sendWaypointClearAll(); + } + else if (current_state == WP_SETCURRENT) + { + sendWaypointSetCurrent(current_wp_id); + } + } + else + { + protocol_timer.stop(); - qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE"; + qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE"; - emit updateStatusString("Operation timed out."); + emit updateStatusString("Operation timed out."); - current_state = WP_IDLE; - current_count = 0; - current_wp_id = 0; - current_partner_systemid = 0; - current_partner_compid = 0; + current_state = WP_IDLE; + current_count = 0; + current_wp_id = 0; + current_partner_systemid = 0; + current_partner_compid = 0; + } } void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid) { + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; + qDebug() << "got waypoint count (" << count << ") from ID " << systemId; if (count > 0) @@ -88,10 +124,11 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) { + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; + if(wp->seq == current_wp_id) { //update the UI FIXME @@ -130,30 +167,32 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - - if (systemId == current_partner_systemid && compId == current_partner_compid && (current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS)) + if (systemId == current_partner_systemid && compId == current_partner_compid) { - if(current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0) + if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)) { //all waypoints sent and ack received - current_state = WP_IDLE; - protocol_timer.stop(); + current_state = WP_IDLE; emit updateStatusString("done."); - qDebug() << "sent all waypoints to ID " << systemId; } + else if(current_state == WP_CLEARLIST) + { + protocol_timer.stop(); + current_state = WP_IDLE; + emit updateStatusString("done."); + qDebug() << "cleared waypoint list of ID " << systemId; + } } } void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)))) { - qDebug() << "handleWaypointRequest"; + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; if (wpr->seq < waypoint_buffer.count()) { @@ -180,6 +219,12 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m { if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER) { + if (current_state == WP_SETCURRENT) + { + protocol_timer.stop(); + current_state = WP_IDLE; + emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); + } qDebug() << "new current waypoint" << wpc->seq; emit currentWaypointChanged(wpc->seq); } @@ -190,60 +235,57 @@ void UASWaypointManager::clearWaypointList() if(current_state == WP_IDLE) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); - - mavlink_message_t message; - mavlink_waypoint_clear_all_t wpca; - - wpca.target_system = uas.getUASID(); - wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + current_retries = PROTOCOL_MAX_RETRIES; current_state = WP_CLEARLIST; current_wp_id = 0; current_partner_systemid = uas.getUASID(); current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; - const QString str = QString("clearing waypoint list..."); - emit updateStatusString(str); - - mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); - uas.sendMessage(message); + sendWaypointClearAll(); } } -void UASWaypointManager::requestWaypoints() +void UASWaypointManager::setCurrent(quint16 seq) { if(current_state == WP_IDLE) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; - mavlink_message_t message; - mavlink_waypoint_request_list_t wprl; + current_state = WP_SETCURRENT; + current_wp_id = seq; + current_partner_systemid = uas.getUASID(); + current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; - wprl.target_system = uas.getUASID(); - wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + sendWaypointSetCurrent(current_wp_id); + } +} + +void UASWaypointManager::readWaypoints() +{ + if(current_state == WP_IDLE) + { + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; current_state = WP_GETLIST; current_wp_id = 0; current_partner_systemid = uas.getUASID(); current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; - const QString str = QString("requesting waypoint list..."); - emit updateStatusString(str); - - mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); - uas.sendMessage(message); - - qDebug() << "sent waypoint list request to ID " << wprl.target_system; + sendWaypointRequestList(); } } -void UASWaypointManager::sendWaypoints() +void UASWaypointManager::writeWaypoints() { if (current_state == WP_IDLE) { if (waypoints.count() > 0) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; current_count = waypoints.count(); current_state = WP_SENDLIST; @@ -281,20 +323,7 @@ void UASWaypointManager::sendWaypoints() } //send the waypoint count to UAS (this starts the send transaction) - mavlink_message_t message; - mavlink_waypoint_count_t wpc; - - wpc.target_system = uas.getUASID(); - wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - wpc.count = current_count; - - const QString str = QString("start transmitting waypoints..."); - emit updateStatusString(str); - - mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); - uas.sendMessage(message); - - qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; + sendWaypointCount(); } } else @@ -304,6 +333,80 @@ void UASWaypointManager::sendWaypoints() } } +void UASWaypointManager::sendWaypointClearAll() +{ + mavlink_message_t message; + mavlink_waypoint_clear_all_t wpca; + + wpca.target_system = uas.getUASID(); + wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + + const QString str = QString("clearing waypoint list..."); + emit updateStatusString(str); + + mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); + uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); + + qDebug() << "sent waypoint clear all to ID " << wpca.target_system; +} + +void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) +{ + mavlink_message_t message; + mavlink_waypoint_set_current_t wpsc; + + wpsc.target_system = uas.getUASID(); + wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + wpsc.seq = seq; + + const QString str = QString("Updating target waypoint..."); + emit updateStatusString(str); + + mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc); + uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); + + qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system; +} + +void UASWaypointManager::sendWaypointCount() +{ + mavlink_message_t message; + mavlink_waypoint_count_t wpc; + + wpc.target_system = uas.getUASID(); + wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + wpc.count = current_count; + + const QString str = QString("start transmitting waypoints..."); + emit updateStatusString(str); + + mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); + uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); + + qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; +} + +void UASWaypointManager::sendWaypointRequestList() +{ + mavlink_message_t message; + mavlink_waypoint_request_list_t wprl; + + wprl.target_system = uas.getUASID(); + wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + + const QString str = QString("requesting waypoint list..."); + emit updateStatusString(str); + + mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); + uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); + + qDebug() << "sent waypoint list request to ID " << wprl.target_system; +} + void UASWaypointManager::sendWaypointRequest(quint16 seq) { mavlink_message_t message; @@ -318,6 +421,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system; } @@ -339,6 +443,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq) mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp); uas.sendMessage(message); + usleep(PROTOCOL_DELAY_MS * 1000); qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system; } @@ -355,6 +460,7 @@ void UASWaypointManager::sendWaypointAck(quint8 type) mavlink_msg_waypoint_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa); uas.sendMessage(message); + 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 4146c1ea319354bc7d014d822465fced0d0c1a2f..4ccd724309443daefb8560d4206a0c3d1c9ef8fb 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -58,7 +58,8 @@ private: WP_SENDLIST_SENDWPS,///< Sending waypoints WP_GETLIST, ///< Initial state for retrieving wayppoints from the MAV WP_GETLIST_GETWPS, ///< Receiving waypoints - WP_CLEARLIST ///< Clearing waypoint list on the MAV + WP_CLEARLIST, ///< Clearing waypoint list on the MAV + WP_SETCURRENT ///< Setting new current waypoint on the MAV }; ///< The possible states for the waypoint protocol public: @@ -77,15 +78,20 @@ public: QVector &getWaypointList(void) { return waypoints; } ///< Returns a reference to the local waypoint list. Gives full access to the internal data structure - Subject to change: Public const access and friend access for the waypoint list widget. private: + void sendWaypointClearAll(); + void sendWaypointSetCurrent(quint16 seq); + void sendWaypointCount(); + void sendWaypointRequestList(); void sendWaypointRequest(quint16 seq); ///< Requests a waypoint with sequence number seq void sendWaypoint(quint16 seq); ///< Sends a waypoint with sequence number seq void sendWaypointAck(quint8 type); ///< Sends a waypoint ack public slots: void timeout(); ///< Called by the timer if a response times out. Handles send retries. + void setCurrent(quint16 seq); ///< Sends the sequence number of the waypoint that should get the new target waypoint void clearWaypointList(); ///< Sends the waypoint clear all message to the MAV - void requestWaypoints(); ///< Requests the MAV's current waypoint list - void sendWaypoints(); ///< Sends the local waypoint list to the MAV + void readWaypoints(); ///< Requests the MAV's current waypoint list + void writeWaypoints(); ///< Sends the local waypoint list to the MAV signals: void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget @@ -94,6 +100,7 @@ signals: private: UAS &uas; ///< Reference to the corresponding UAS + quint32 current_retries; ///< The current number of retries left quint16 current_wp_id; ///< The last used waypoint ID in the current protocol transaction quint16 current_count; ///< The number of waypoints in the current protocol transaction WaypointState current_state; ///< The current protocol state diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 74da7f1f94ec5eaa3145a6b0014b5dcdeee62283..6c3828f091c2f0f3791eb90fe4fe46b85e99e965 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -126,9 +126,10 @@ void WaypointList::setUAS(UASInterface* uas) connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(this, SIGNAL(sendWaypoints()), &uas->getWaypointManager(), SLOT(sendWaypoints())); - connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); + connect(this, SIGNAL(sendWaypoints()), &uas->getWaypointManager(), SLOT(writeWaypoints())); + connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(readWaypoints())); connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); + connect(this, SIGNAL(setCurrent(quint16)), &uas->getWaypointManager(), SLOT(setCurrent(quint16))); } } @@ -149,6 +150,35 @@ void WaypointList::waypointReached(quint16 waypointId) } } +void WaypointList::changeCurrentWaypoint(quint16 seq) +{ + if (this->uas) + { + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[i]).value(); + + if (waypoints[i]->getId() == seq) + { + waypoints[i]->setCurrent(true); + widget->setCurrent(true); + emit setCurrent(seq); + } + else + { + waypoints[i]->setCurrent(false); + widget->setCurrent(false); + } + } + redrawList(); + } + } +} + void WaypointList::currentWaypointChanged(quint16 seq) { if (this->uas) @@ -250,6 +280,7 @@ void WaypointList::addWaypoint(Waypoint* wp) connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); } } } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 111f7a8867801a297e2bb499da36f820787f2f40..4b1236fd8105ec6b8d95e16b58816a822258252a 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -72,7 +72,8 @@ public slots: /** @brief sets statusLabel string */ void updateStatusLabel(const QString &string); - void currentWaypointChanged(quint16 seq); + void changeCurrentWaypoint(quint16 seq); ///< The user wants to change the current waypoint + void currentWaypointChanged(quint16 seq); ///< The waypointplanner changed the current waypoint void setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime); void addWaypoint(Waypoint* wp); void removeWaypoint(Waypoint* wp); @@ -84,6 +85,7 @@ signals: void sendWaypoints(); void requestWaypoints(); void clearWaypointList(); + void setCurrent(quint16); protected: virtual void changeEvent(QEvent *e); diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index bb79f74d47562782b9544d40477747dceba148ec..a9ec214de3707582e4048f0429d0c907b819457d 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -115,7 +115,7 @@ void WaypointView::changedCurrent(int state) else { wp->setCurrent(true); - emit currentWaypointChanged(wp->getId()); //the slot currentWayppointChanged() in WaypointList sets all other current flags to false + emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false } } diff --git a/src/ui/WaypointView.h b/src/ui/WaypointView.h index bf4fb1bee1e79945e88b1be4745da8f141a69f35..bee5e1855a3442b6c2d391216cf31d053cac4847 100644 --- a/src/ui/WaypointView.h +++ b/src/ui/WaypointView.h @@ -72,6 +72,7 @@ signals: void moveDownWaypoint(Waypoint*); void removeWaypoint(Waypoint*); void currentWaypointChanged(quint16); + void changeCurrentWaypoint(quint16); void setYaw(double); };