From 62fbf414c645f622abade3e39e02636e4b8adebb Mon Sep 17 00:00:00 2001 From: pixhawk Date: Wed, 14 Jul 2010 17:26:30 +0200 Subject: [PATCH] adapted UASWaypointManager to waypoint message changes --- src/uas/UAS.cc | 11 +++++ src/uas/UASWaypointManager.cc | 79 ++++++++++++++++++++++++++++------- src/uas/UASWaypointManager.h | 51 ++++++++++++++-------- 3 files changed, 107 insertions(+), 34 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index ee19485f5..428ec9586 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -437,6 +437,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(&message, &wpa); + if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) + { + waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); + } + } + break; + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { mavlink_waypoint_request_t wpr; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 9a54fcf78..ee2b65166 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -32,7 +32,8 @@ This file is part of the PIXHAWK project #include "UASWaypointManager.h" #include "UAS.h" -#define PROTOCOL_TIMEOUT_MS 2000 +#define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout +#define PROTOCOL_MAX_RETRIES 3 ///< maximum number of send retries (after timeout) UASWaypointManager::UASWaypointManager(UAS &_uas) : uas(_uas), @@ -94,7 +95,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { //update the UI FIXME - emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); + emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2); //get next waypoint current_wp_id++; @@ -105,6 +106,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ } else { + sendWaypointAck(0); + // all waypoints retrieved, change state to idle current_state = WP_IDLE; current_count = 0; @@ -125,11 +128,30 @@ 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(current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0) + { + //all waypoints sent and ack received + current_state = WP_IDLE; + + protocol_timer.stop(); + emit updateStatusString("done."); + + qDebug() << "sent all waypoints to 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)) || (current_state == WP_IDLE && wpr->seq == current_count-1))) + 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"; @@ -138,17 +160,6 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m current_state = WP_SENDLIST_SENDWPS; current_wp_id = wpr->seq; sendWaypoint(current_wp_id); - - if(current_wp_id == waypoint_buffer.count()-1) - { - //all waypoints sent, but we still have to wait for a possible rerequest of the last waypoint - current_state = WP_IDLE; - - protocol_timer.stop(); - emit updateStatusString("done."); - - qDebug() << "send all waypoints to ID " << systemId; - } } else { @@ -176,7 +187,27 @@ void UASWaypointManager::handleWaypointSetCurrent(quint8 systemId, quint8 compId 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_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); + } } void UASWaypointManager::requestWaypoints() @@ -237,8 +268,9 @@ void UASWaypointManager::sendWaypoints() cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent(); - cur_d->orbit = cur_s->getOrbit(); - cur_d->hold_time = cur_s->getHoldTime(); + cur_d->orbit = 0.f; //FIXME + cur_d->param1 = cur_s->getOrbit(); + cur_d->param2 = cur_s->getHoldTime(); cur_d->type = 1; //FIXME cur_d->seq = i; cur_d->x = cur_s->getX(); @@ -310,3 +342,18 @@ void UASWaypointManager::sendWaypoint(quint16 seq) qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system; } } + +void UASWaypointManager::sendWaypointAck(quint8 type) +{ + mavlink_message_t message; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = uas.getUASID(); + wpa.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa); + uas.sendMessage(message); + + qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system; +} diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index f8432d31b..282bf2b12 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -39,38 +39,53 @@ This file is part of the PIXHAWK project #include class UAS; +/** + * @brief Implementation of the MAVLINK waypoint protocol + * + * This class handles the communication with a waypoint manager on the MAV. + * All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget. + * Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data. + * + * See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states. + */ class UASWaypointManager : public QObject { Q_OBJECT private: enum WaypointState { - WP_IDLE = 0, - WP_SENDLIST, - WP_SENDLIST_SENDWPS, - WP_GETLIST, - WP_GETLIST_GETWPS + WP_IDLE = 0, ///< Waiting for commands + WP_SENDLIST, ///< Initial state for sending waypoints to the MAV + 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 }; ///< The possible states for the waypoint protocol public: - UASWaypointManager(UAS&); + UASWaypointManager(UAS&); ///< Standard constructor. - void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count); - void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp); - void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr); - void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr); - void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr); + /** @name Protocol handlers */ + /*@{*/ + void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count); ///< Handles received waypoint count messages + void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp); ///< Handles received waypoint messages + void handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa); ///< Handles received waypoint ack messages + void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr); ///< Handles received waypoint request messages + void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr); ///< Handles received waypoint reached messages + void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr); ///< Handles received set current waypoint messages + /*@}*/ - QVector &getWaypointList(void) { return waypoints; } + 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 sendWaypointRequest(quint16 seq); - void sendWaypoint(quint16 seq); + 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(); - void clearWaypointList(); - void requestWaypoints(); - void sendWaypoints(); + void timeout(); ///< Called by the timer if a response times out. Handles send retries. + 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 signals: void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget -- 2.22.0