diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 7901330b9a561a8350596491f2db95f510ccb9c5..8dada49c43ef2a37c53ae270c73f7241fe3c91b8 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -75,6 +75,7 @@ bool Waypoint::load(QTextStream &loadStream) return false; } + void Waypoint::setId(quint16 id) { this->id = id; diff --git a/src/Waypoint.h b/src/Waypoint.h index 8e8f40025aa81d307354fc2e07fd439611b37966..7e1a6022955045b1348c050a96c31360b63cb6a2 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -59,6 +59,7 @@ public: bool load(QTextStream &loadStream); + protected: quint16 id; float x; diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 62fdd6d8bf8219a8ceb39ee047ea7819d292e786..19beb486adb624593f8518e69d669d2b3624d4d5 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -80,7 +80,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_RAW_IMU: mavlink_msg_raw_imu_decode(&message, &mlRawImuData); - break; + break; #ifdef MAVLINK_ENABLED_SLUGS @@ -97,21 +97,21 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_GPS_RAW: mavlink_msg_gps_raw_decode(&message, &mlGpsData); - break; + break; case MAVLINK_MSG_ID_ACTION_ACK: // 62 mavlink_msg_action_ack_decode(&message,&mlActionAck); - break; + break; case MAVLINK_MSG_ID_CPU_LOAD: //170 mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData); - break; + break; case MAVLINK_MSG_ID_AIR_DATA: //171 mavlink_msg_air_data_decode(&message,&mlAirData); - break; + break; case MAVLINK_MSG_ID_SENSOR_BIAS: //172 mavlink_msg_sensor_bias_decode(&message,&mlSensorBiasData); @@ -123,7 +123,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_PILOT_CONSOLE: //174 mavlink_msg_pilot_console_decode(&message,&mlPilotConsoleData); - break; + break; case MAVLINK_MSG_ID_PWM_COMMANDS: //175 mavlink_msg_pwm_commands_decode(&message,&mlPwmCommands); @@ -131,7 +131,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176 mavlink_msg_slugs_navigation_decode(&message,&mlNavigation); - break; + break; case MAVLINK_MSG_ID_DATA_LOG: //177 mavlink_msg_data_log_decode(&message,&mlDataLog); @@ -143,7 +143,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_GPS_DATE_TIME: //179 mavlink_msg_gps_date_time_decode(&message,&mlGpsDateTime); - break; + break; case MAVLINK_MSG_ID_MID_LVL_CMDS: //180 @@ -151,7 +151,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_CTRL_SRFC_PT: //181 - break; + break; case MAVLINK_MSG_ID_PID: //182 @@ -165,8 +165,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) default: // qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; - break; - } + break; + } } @@ -197,13 +197,13 @@ void SlugsMAV::emitSignals (void){ case 5: emit slugsFilteredData(uasId,mlFilteredData); emit slugsGPSDateTime(uasId, mlGpsDateTime); - break; + break; case 6: emit slugsActionAck(uasId,mlActionAck); emit emitGpsSignals(); break; - } + } emit slugsAttitude(uasId, mlAttitude); emit attitudeChanged(this, @@ -238,7 +238,7 @@ void SlugsMAV::emitGpsSignals (void){ } else { emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); } - } + } } #endif // MAVLINK_ENABLED_SLUGS diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index d7669d48cb6d3bb30830950bf868bbce2d0a40f4..4fc690f92b614c720951dcda0c500d9d42fb9da0 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -105,6 +105,8 @@ protected: mavlink_slugs_action_t mlAction; + + // Standart messages MAVLINK used by SLUGS private: diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index fb03c520431ee630ada5f868433b4a92abf37209..ef3719dab63de49e778173f4c46f6920e31f4503 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -383,9 +383,12 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile) } file.close(); + emit loadWPFile(); emit waypointListChanged(); + } + void UASWaypointManager::globalAddWaypoint(Waypoint *wp) { diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index a16c8d3fe9a433a2af487b62f1da8751f042ab7d..f7fb7eace9b32573fb3e507f842fad2b7da1f37f 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -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: 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 diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 9cb6d36c835cf7bdda3c229f983733c15f331684..981d7fe484a68d3c16c506df801e8eac403158c0 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -401,6 +401,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() @@ -918,6 +921,7 @@ void MainWindow::loadGlobalOperatorView() { addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); waypointsDockWidget->show(); + } // Slugs Data View diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 14120f79bddb668833ee6ca4dc0ed40c39bd777d..ece5a391a342f8a9c3726481ac7964366dcf8d42 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -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); diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index 3939c8da7a3c7219c545e29aaf41b47707f6a477..5aed013188f71a37f2c630418e57a6e934c55677 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -10,21 +10,21 @@ SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) : QWidget(parent), ui(new Ui::SlugsDataSensorView) { - ui->setupUi(this); + ui->setupUi(this); - activeUAS = NULL; + activeUAS = NULL; - this->setVisible(false); + this->setVisible(false); } SlugsDataSensorView::~SlugsDataSensorView() { - delete ui; + delete ui; } void SlugsDataSensorView::addUAS(UASInterface* uas) { - SlugsMAV* slugsMav = dynamic_cast(uas); + SlugsMAV* slugsMav = dynamic_cast(uas); if (slugsMav != NULL) { @@ -32,15 +32,15 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) #ifdef MAVLINK_ENABLED_SLUGS - //connect standar messages + //connect standar messages connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); - //connect slugs especial messages + //connect slugs especial messages connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&))); connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&))); connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&))); @@ -51,13 +51,12 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); #endif // MAVLINK_ENABLED_SLUGS - - // Set this UAS as active if it is the first one + // Set this UAS as active if it is the first one if(activeUAS == 0) { - activeUAS = uas; - } + activeUAS = uas; + } - } + } } void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t &rawData){ @@ -107,7 +106,7 @@ void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface* uas, double vy, double vz, quint64 time) { - Q_UNUSED( uas); + Q_UNUSED( uas); Q_UNUSED(time); ui->ed_vx->setPlainText(QString::number(vx)); @@ -134,7 +133,7 @@ void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas, void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, const mavlink_sensor_bias_t& sensorBias){ - Q_UNUSED( systemId); + Q_UNUSED( systemId); ui->m_AxBiases->setText(QString::number(sensorBias.axBias)); ui->m_AyBiases->setText(QString::number(sensorBias.ayBias)); @@ -147,12 +146,12 @@ void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId, const mavlink_diagnostic_t& diagnostic){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_Fl1->setText(QString::number(diagnostic.diagFl1)); ui->m_Fl2->setText(QString::number(diagnostic.diagFl2)); ui->m_Fl3->setText(QString::number(diagnostic.diagFl2)); - + ui->m_Sh1->setText(QString::number(diagnostic.diagSh1)); ui->m_Sh2->setText(QString::number(diagnostic.diagSh2)); ui->m_Sh3->setText(QString::number(diagnostic.diagSh3)); @@ -161,7 +160,7 @@ void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId, void SlugsDataSensorView::slugsCpuLoadChanged(int systemId, const mavlink_cpu_load_t& cpuLoad){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->ed_sens->setText(QString::number(cpuLoad.sensLoad)); ui->ed_control->setText(QString::number(cpuLoad.ctrlLoad)); ui->ed_batvolt->setText(QString::number(cpuLoad.batVolt)); @@ -169,7 +168,7 @@ void SlugsDataSensorView::slugsCpuLoadChanged(int systemId, void SlugsDataSensorView::slugsNavegationChanged(int systemId, const mavlink_slugs_navigation_t& slugsNavigation){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_Um->setText(QString::number(slugsNavigation.u_m)); ui->m_PhiC->setText(QString::number(slugsNavigation.phi_c)); ui->m_PitchC->setText(QString::number(slugsNavigation.theta_c)); @@ -185,7 +184,7 @@ void SlugsDataSensorView::slugsNavegationChanged(int systemId, void SlugsDataSensorView::slugsDataLogChanged(int systemId, const mavlink_data_log_t& dataLog){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_logFl1->setText(QString::number(dataLog.fl_1)); ui->m_logFl2->setText(QString::number(dataLog.fl_2)); ui->m_logFl3->setText(QString::number(dataLog.fl_3)); @@ -196,7 +195,7 @@ void SlugsDataSensorView::slugsDataLogChanged(int systemId, void SlugsDataSensorView::slugsPWMChanged(int systemId, const mavlink_pwm_commands_t& pwmCommands){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_pwmThro->setText(QString::number(pwmCommands.dt_c)); ui->m_pwmAile->setText(QString::number(pwmCommands.dla_c)); ui->m_pwmElev->setText(QString::number(pwmCommands.dle_c)); @@ -211,7 +210,7 @@ void SlugsDataSensorView::slugsPWMChanged(int systemId, void SlugsDataSensorView::slugsFilteredDataChanged(int systemId, const mavlink_filtered_data_t& filteredData){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_Axf->setText(QString::number(filteredData.aX)); ui->m_Ayf->setText(QString::number(filteredData.aY)); ui->m_Azf->setText(QString::number(filteredData.aZ)); @@ -225,7 +224,7 @@ void SlugsDataSensorView::slugsFilteredDataChanged(int systemId, void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, const mavlink_gps_date_time_t& gpsDateTime){ - Q_UNUSED(systemId); + Q_UNUSED(systemId); ui->m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" + QString::number(gpsDateTime.day) + "/" + QString::number(gpsDateTime.year)); diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index 63d9250cfac99e089200dd61d393c2952006709b..001a9e734d4b9d61c6ddade2c2729e15491b16b6 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -162,7 +162,7 @@ void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() if (slugsMav != NULL) { - //create the packet + //create the packet pidMessage.target = activeUAS->getUASID(); pidMessage.idx = 0; pidMessage.pVal = ui->dT_P_set->text().toFloat(); @@ -174,7 +174,7 @@ void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); slugsMav->sendMessage(msg); - ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle); + ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle); } } diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 1f7f1332338fc7a834f93607dc78a43eebf55d4a..bf75abf33e777cceb3d895d8c91ec7935f2b7871 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -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) 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() { 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() WaypointGlobalView *wpgv = wpGlobalViews.value(wp); wpgv->updateValues(); listLayout->addWidget(wpgv); + if(loadFileGlobalWP) + { + emit createWaypointAtMap(QPointF(wp->getX(),wp->getY())); + qDebug()<<"Emitiendo Pos: "<getX()<<" - "<getY(); + } } } @@ -391,7 +401,7 @@ void WaypointList::waypointListChanged() } - + loadFileGlobalWP = false; } @@ -622,3 +632,8 @@ void WaypointList::changeWPPositionBySpinBox(Waypoint *wp) emit changePositionWPGlobalBySpinBox(wp->getId(), wp->getY(), wp->getX()); } + +void WaypointList::setIsLoadFileWP() +{ + loadFileGlobalWP = true; +} diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 24325046f241622707310c749abdc34d2f58153c..027a46927dcef5739fa96f4a69c38ae326c74175 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -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: void moveDown(Waypoint* wp); void removeWaypoint(Waypoint* wp); + void setIsLoadFileWP(); + @@ -128,6 +130,7 @@ protected: bool isGlobalWP; bool isLocalWP; QPointF centerMapCoordinate; + bool loadFileGlobalWP; private: Ui::WaypointList *m_ui;