From 91c1b8262c7f9f23115e3d45d21dfdcfc807f5d1 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Tue, 8 Jun 2010 15:07:15 +0200 Subject: [PATCH] merged --- src/uas/UAS.cc | 9 ++++ src/uas/UASWaypointManager.cc | 97 ++++++++++++++++++++++------------- src/uas/UASWaypointManager.h | 6 ++- src/ui/WaypointList.cc | 10 ++-- src/ui/WaypointList.h | 23 +++++---- 5 files changed, 95 insertions(+), 50 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1417c97df..54e10b78d 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -379,6 +379,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) waypointManager.handleWaypoint(message.sysid, message.compid, &wp); } break; + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(&message, &wpr); + waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); + } + break; + case MAVLINK_MSG_ID_WAYPOINT_REACHED: { // mavlink_waypoint_reached_t wp; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 5a470397c..449439667 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -11,39 +11,6 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) { } -void UASWaypointManager::waypointChanged(Waypoint*) -{ -} - -void UASWaypointManager::currentWaypointChanged(int) -{ -} - -void UASWaypointManager::removeWaypointId(int) -{ -} - -void UASWaypointManager::requestWaypoints() -{ - if(current_state == WP_IDLE) - { - qDebug() << "handleWaypointCount"; - mavlink_message_t message; - mavlink_waypoint_request_list_t wprl; - - wprl.target_system = uas.getUASID(); - wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - - current_state = WP_GETLIST; - current_wp_id = 0; - current_partner_systemid = uas.getUASID(); - current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; - - mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); - uas.sendMessage(message); - } -} - void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count) { if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid) @@ -95,6 +62,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ } else { + // all waypoints retrieved, change state to idle current_state = WP_IDLE; current_count = 0; current_wp_id = 0; @@ -104,16 +72,75 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ } else { - //FIXME error handling + //TODO: error handling } } } -void UASWaypointManager::getWaypoint(quint16 seq) +void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr) { + if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_SENDLIST && wpr->seq == current_wp_id) + { + qDebug() << "handleWaypointRequest"; + if (wpr->seq < 0) + { + //TODO: send waypoint + } + else + { + //TODO: Error message or something + } + } } void UASWaypointManager::clearWaypointList() { + +} + +void UASWaypointManager::currentWaypointChanged(int) +{ + +} + +void UASWaypointManager::removeWaypointId(int) +{ + +} + +void UASWaypointManager::requestWaypoints() +{ + if(current_state == WP_IDLE) + { + qDebug() << "requestWaypoints"; + mavlink_message_t message; + mavlink_waypoint_request_list_t wprl; + + wprl.target_system = uas.getUASID(); + wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; + + current_state = WP_GETLIST; + current_wp_id = 0; + current_partner_systemid = uas.getUASID(); + current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; + + mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); + uas.sendMessage(message); + } +} + +void UASWaypointManager::sendWaypoints(void) +{ + +} + +void UASWaypointManager::getWaypoint(quint16 seq) +{ + +} + +void UASWaypointManager::waypointChanged(Waypoint*) +{ + } diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 22e9dfa6b..f370e1bd0 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -23,16 +23,18 @@ public: 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); private: void getWaypoint(quint16 seq); public slots: - void waypointChanged(Waypoint*); + void clearWaypointList(); void currentWaypointChanged(int); void removeWaypointId(int); void requestWaypoints(); - void clearWaypointList(); + void sendWaypoints(void); + void waypointChanged(Waypoint*); signals: void waypointUpdated(int,int,double,double,double,double,bool,bool); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index ddd141a6c..d151ecf38 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -87,11 +87,13 @@ void WaypointList::setUAS(UASInterface* uas) this->uas = uas; connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(int,int,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,int,double,double,double,double,bool,bool))); connect(&uas->getWaypointManager(), SIGNAL(waypointReached(UASInterface*,int)), this, SLOT(waypointReached(UASInterface*,int))); - connect(this, SIGNAL(waypointChanged(Waypoint*)), &uas->getWaypointManager(), SLOT(setWaypoint(Waypoint*))); - connect(this, SIGNAL(currentWaypointChanged(int)), &uas->getWaypointManager(), SLOT(setWaypointActive(int))); + connect(this, SIGNAL(waypointChanged(Waypoint*)), &uas->getWaypointManager(), SLOT(setWaypoint(Waypoint*))); + connect(this, SIGNAL(currentWaypointChanged(int)), &uas->getWaypointManager(), SLOT(setWaypointActive(int))); + connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); + connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); + // This slot is not implemented in UAS: connect(this, SIGNAL(removeWaypointId(int)), uas, SLOT(removeWaypoint(Waypoint*))); - connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); - connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); + qDebug() << "Requesting waypoints"; emit requestWaypoints(); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 226ac6f6a..9c0e59973 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -53,30 +53,35 @@ class WaypointList : public QWidget { virtual ~WaypointList(); public slots: - void transmit(); - void add(); - void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current); void setUAS(UASInterface* uas); - void addWaypoint(Waypoint* wp); - void removeWaypointAndName(Waypoint* wp); void redrawList(); + void reenableTransmit(); + + //UI Buttons + void saveWaypoints(); + void loadWaypoints(); + void transmit(); + void add(); void moveUp(Waypoint* wp); void moveDown(Waypoint* wp); void setCurrentWaypoint(Waypoint* wp); - void reenableTransmit(); - void saveWaypoints(); - void loadWaypoints(); + + //To be moved to UASWaypointManager (?) + void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current); + void addWaypoint(Waypoint* wp); + void removeWaypointAndName(Waypoint* wp); void waypointReached(UASInterface* uas, int waypointId); protected: virtual void changeEvent(QEvent *e); + void debugOutputWaypoints(); + QVector waypoints; QMap waypointNames; QMap wpViews; QVBoxLayout* listLayout; QTimer* transmitDelay; UASInterface* uas; - void debugOutputWaypoints(); private: Ui::WaypointList *m_ui; -- 2.22.0