From c3c390c2e54684aa51455708d0ee335a942dc068 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 24 Jun 2010 10:47:35 +0200 Subject: [PATCH] new waypoint fields included --- src/uas/UASWaypointManager.cc | 5 +++-- src/uas/UASWaypointManager.h | 2 +- src/ui/HSIDisplay.cc | 2 +- src/ui/WaypointList.cc | 16 ++++++++-------- src/ui/WaypointList.h | 2 +- src/ui/WaypointView.cc | 7 ++++++- src/ui/WaypointView.ui | 2 +- 7 files changed, 21 insertions(+), 15 deletions(-) diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 4f5b9d94f..05523810c 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -94,7 +94,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { //update the UI FIXME - emit waypointUpdated(uas.getUASID(), wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current); + emit waypointUpdated(uas.getUASID(), wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); //get next waypoint current_wp_id++; @@ -237,7 +237,8 @@ void UASWaypointManager::sendWaypoints(const QVector &list) cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent(); - cur_d->orbit = 0.1f; //FIXME + cur_d->orbit = cur_s->getOrbit(); + cur_d->hold_time = cur_s->getHoldTime(); cur_d->type = 1; //FIXME cur_d->seq = i; cur_d->x = cur_s->getX(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 247feacc1..8e7e74584 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -71,7 +71,7 @@ public slots: void sendWaypoints(const QVector &list); signals: - void waypointUpdated(int,quint16,double,double,double,double,bool,bool); ///< Adds a waypoint to the waypoint list widget + void waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number void updateStatusString(const QString &); ///< emits the current status string diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 34c7963dc..7c5468c64 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -450,7 +450,7 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir bodyYawSet = yawDesired; mavInitialized = true; - qDebug() << "Received setpoint at x: " << x << "metric y:" << y; +// qDebug() << "Received setpoint at x: " << x << "metric y:" << y; // posXSet = xDesired; // posYSet = yDesired; // posZSet = zDesired; diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 622b67549..39fe08554 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -96,7 +96,7 @@ void WaypointList::setUAS(UASInterface* uas) this->uas = uas; connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(int,quint16,double,double,double,double,bool,bool)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool))); + connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(int,quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool,double,int))); connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); connect(this, SIGNAL(sendWaypoints(const QVector &)), &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector &))); @@ -105,11 +105,11 @@ void WaypointList::setUAS(UASInterface* uas) } } -void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current) +void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) { if (uasId == this->uas->getUASID()) { - Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current); + Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime); addWaypoint(wp); } } @@ -168,11 +168,11 @@ void WaypointList::add() { if (waypoints.size() > 0) { - addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, false)); + addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, false, 0.1, 2000)); } else { - addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.0, -0.0, 0.0, true, true)); + addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.6, 0.0, true, true, 0.1, 2000)); } } } @@ -302,7 +302,7 @@ void WaypointList::saveWaypoints() for (int i = 0; i < waypoints.size(); i++) { Waypoint* wp = waypoints[i]; - in << "~" << wp->getId() << "~" << wp->getX() << "~" << wp->getY() << "~" << wp->getZ() << "~" << wp->getYaw() << "~" << wp->getAutoContinue() << "~" << wp->getCurrent() << "\n"; + in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY() << "\t" << wp->getZ() << "\t" << wp->getYaw() << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n"; in.flush(); } file.close(); @@ -323,9 +323,9 @@ void WaypointList::loadWaypoints() QTextStream in(&file); while (!in.atEnd()) { - QStringList wpParams = in.readLine().split("~"); + QStringList wpParams = in.readLine().split("\t"); if (wpParams.size() == 8) - addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false))); + addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false), wpParams[8].toDouble(), wpParams[9].toInt())); } file.close(); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index fecd118e9..1164b9f00 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -72,7 +72,7 @@ public slots: void currentWaypointChanged(quint16 seq); //To be moved to UASWaypointManager (?) - void setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current); + void setWaypoint(int uasId, 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); void waypointReached(UASInterface* uas, quint16 waypointId); diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 17e091970..bb79f74d4 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -54,7 +54,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); m_ui->selectedBox->setChecked(wp->getCurrent()); m_ui->autoContinue->setChecked(wp->getAutoContinue()); - m_ui->idLabel->setText(QString("%1").arg(wp->getId())); + m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\ + m_ui->orbitSpinBox->setValue(wp->getOrbit()); + m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); @@ -70,6 +72,9 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int))); connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int))); + + connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double))); + connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int))); } void WaypointView::setYaw(int yawDegree) diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index 123485c5a..97f8013a6 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -323,7 +323,7 @@ QProgressBar::chunk#thrustBar { 500 - 2000 + 0 -- 2.22.0