Browse Source

add global wp draw on a map when load file

QGC4.4
tecnosapiens 14 years ago
parent
commit
6d43e99e25
  1. 1
      src/Waypoint.cc
  2. 1
      src/Waypoint.h
  3. 2
      src/uas/SlugsMAV.cc
  4. 2
      src/uas/SlugsMAV.h
  5. 3
      src/uas/UASWaypointManager.cc
  6. 3
      src/uas/UASWaypointManager.h
  7. 4
      src/ui/MainWindow.cc
  8. 12
      src/ui/MapWidget.cc
  9. 10
      src/ui/SlugsDataSensorView.cc
  10. 2
      src/ui/SlugsPIDControl.cpp
  11. 19
      src/ui/WaypointList.cc
  12. 5
      src/ui/WaypointList.h

1
src/Waypoint.cc

@ -75,6 +75,7 @@ bool Waypoint::load(QTextStream &loadStream) @@ -75,6 +75,7 @@ bool Waypoint::load(QTextStream &loadStream)
return false;
}
void Waypoint::setId(quint16 id)
{
this->id = id;

1
src/Waypoint.h

@ -59,6 +59,7 @@ public: @@ -59,6 +59,7 @@ public:
bool load(QTextStream &loadStream);
protected:
quint16 id;
float x;

2
src/uas/SlugsMAV.cc

@ -348,7 +348,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -348,7 +348,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
#endif
default:
qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
//qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
break;
}
}

2
src/uas/SlugsMAV.h

@ -135,6 +135,8 @@ signals: @@ -135,6 +135,8 @@ signals:
uint8_t gpsvisSat,
quint64 time);
// Standart messages MAVLINK used by SLUGS
void slugsActionAck(int systemId,
uint8_t action,
uint8_t result);

3
src/uas/UASWaypointManager.cc

@ -383,9 +383,12 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile) @@ -383,9 +383,12 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile)
}
file.close();
emit loadWPFile();
emit waypointListChanged();
}
void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
{

3
src/uas/UASWaypointManager.h

@ -91,6 +91,7 @@ public: @@ -91,6 +91,7 @@ public:
void localMoveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq
void localSaveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile
void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile
/*@}*/
/** @name Global waypoint list operations */
@ -120,6 +121,8 @@ signals: @@ -120,6 +121,8 @@ signals:
void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number
void updateStatusString(const QString &); ///< emits the current status string
void loadWPFile(); ///< emits signal that a file wp has been load
private:
UAS &uas; ///< Reference to the corresponding UAS
quint32 current_retries; ///< The current number of retries left

4
src/ui/MainWindow.cc

@ -395,6 +395,9 @@ void MainWindow::connectActions() @@ -395,6 +395,9 @@ void MainWindow::connectActions()
// Slugs View
connect(ui.actionShow_Slugs_View, SIGNAL(triggered()), this, SLOT(loadSlugsView()));
//GlobalOperatorView
// connect(ui.actionGlobalOperatorView,SIGNAL(triggered()),waypointsDockWidget->widget(),SLOT())
}
void MainWindow::showHelp()
@ -912,6 +915,7 @@ void MainWindow::loadGlobalOperatorView() @@ -912,6 +915,7 @@ void MainWindow::loadGlobalOperatorView()
{
addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget);
waypointsDockWidget->show();
}
// Slugs Data View

12
src/ui/MapWidget.cc

@ -396,7 +396,17 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) @@ -396,7 +396,17 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
str = QString("%1").arg(path->numberOfPoints());
// create the WP and set everything in the LineString to display the path
CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
//CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
Waypoint2DIcon* tempCirclePoint;
if (mav)
{
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
}
else
{
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
}
mc->layer("Waypoints")->addGeometry(tempCirclePoint);
Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);

10
src/ui/SlugsDataSensorView.cc

@ -167,10 +167,6 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) @@ -167,10 +167,6 @@ void SlugsDataSensorView::addUAS(UASInterface* uas)
connect(slugsMav, SIGNAL(slugsGPSDateTime(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64)),this,SLOT(slugsGPSDateTimeChanged(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64)));
// Set this UAS as active if it is the first one
if(activeUAS == 0)
{
@ -267,12 +263,6 @@ void SlugsDataSensorView::refresh() @@ -267,12 +263,6 @@ void SlugsDataSensorView::refresh()
ui->m_pwmAvailable->setText("No Data");
}
}

2
src/ui/SlugsPIDControl.cpp

@ -337,5 +337,5 @@ void SlugsPIDControl::connect_Pitch2dT_LineEdit() @@ -337,5 +337,5 @@ void SlugsPIDControl::connect_Pitch2dT_LineEdit()
void SlugsPIDControl::recibeMensaje(int systemId, uint8_t action, uint8_t result)
{
ui->recepcion_label->setText(QString::number(action));
ui->recepcion_label->setText("Mensaje Recibido: " + QString::number(action));
}

19
src/ui/WaypointList.cc

@ -91,6 +91,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : @@ -91,6 +91,7 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
this->setVisible(false);
isGlobalWP = false;
isLocalWP = false;
loadFileGlobalWP = false;
centerMapCoordinate.setX(0.0);
centerMapCoordinate.setY(0.0);
@ -131,6 +132,7 @@ void WaypointList::setUAS(UASInterface* uas) @@ -131,6 +132,7 @@ void WaypointList::setUAS(UASInterface* uas)
connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void)));
connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
connect(&uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
}
}
@ -147,9 +149,12 @@ void WaypointList::loadWaypoints() @@ -147,9 +149,12 @@ void WaypointList::loadWaypoints()
{
if (uas)
{
QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
uas->getWaypointManager().localLoadWaypoints(fileName);
}
}
}
void WaypointList::transmit()
@ -336,6 +341,11 @@ void WaypointList::waypointListChanged() @@ -336,6 +341,11 @@ void WaypointList::waypointListChanged()
WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
wpgv->updateValues();
listLayout->addWidget(wpgv);
if(loadFileGlobalWP)
{
emit createWaypointAtMap(QPointF(wp->getX(),wp->getY()));
qDebug()<<"Emitiendo Pos: "<<wp->getX()<<" - "<<wp->getY();
}
}
}
@ -391,7 +401,7 @@ void WaypointList::waypointListChanged() @@ -391,7 +401,7 @@ void WaypointList::waypointListChanged()
}
loadFileGlobalWP = false;
}
@ -622,3 +632,8 @@ void WaypointList::changeWPPositionBySpinBox(Waypoint *wp) @@ -622,3 +632,8 @@ void WaypointList::changeWPPositionBySpinBox(Waypoint *wp)
emit changePositionWPGlobalBySpinBox(wp->getId(), wp->getY(), wp->getX());
}
void WaypointList::setIsLoadFileWP()
{
loadFileGlobalWP = true;
}

5
src/ui/WaypointList.h

@ -65,7 +65,7 @@ public slots: @@ -65,7 +65,7 @@ public slots:
/** @brief Save the local waypoint list to a file */
void saveWaypoints();
/** @brief Load a waypoint list from a file */
void loadWaypoints();
void loadWaypoints();
/** @brief Transmit the local waypoint list to the UAS */
void transmit();
/** @brief Read the remote waypoint list */
@ -102,6 +102,8 @@ public slots: @@ -102,6 +102,8 @@ public slots:
void moveDown(Waypoint* wp);
void removeWaypoint(Waypoint* wp);
void setIsLoadFileWP();
@ -128,6 +130,7 @@ protected: @@ -128,6 +130,7 @@ protected:
bool isGlobalWP;
bool isLocalWP;
QPointF centerMapCoordinate;
bool loadFileGlobalWP;
private:
Ui::WaypointList *m_ui;

Loading…
Cancel
Save