diff --git a/.gitignore b/.gitignore index fe553de6c10f6a4a9b811a761564e2093f7564bb..ef46a13f4a61d32bab84e378d9ca59f4085e5951 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,7 @@ deploy/linux controller_log* user_config.pri *.app + +*.ncb +*.vcproj +*.sln \ No newline at end of file diff --git a/images/contrib/slugs.png b/images/contrib/slugs.png new file mode 100644 index 0000000000000000000000000000000000000000..4af15559a963287e696c51a6b00c6b5dd5163c6b Binary files /dev/null and b/images/contrib/slugs.png differ diff --git a/mavground.qrc b/mavground.qrc index 15e85568816bddcf4ae3ee54f9f3ef29a68a6854..3e7be65296d89971de3b5b32f06734814e32f1a5 100644 --- a/mavground.qrc +++ b/mavground.qrc @@ -84,6 +84,7 @@ images/mapproviders/yahoo.png images/earth.html images/mapproviders/googleearth.svg + images/contrib/slugs.png images/Vera.ttf diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 7d14491fdadad20309eb77fca83251b825b9fe6f..ef5e0fb2bf87dc03fc480536354d97dcf147b99d 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -125,10 +125,11 @@ macx { -framework GEOS \ -framework SQLite3 \ -framework osgFX \ - -framework osgTerrain \ + -framework osgTerrain DEFINES += QGC_OSGEARTH_ENABLED } + exists(/opt/local/include/libfreenect) { message("Building support for libfreenect") DEPENDENCIES_PRESENT += libfreenect @@ -324,7 +325,7 @@ exists($$BASEDIR/lib/osgEarth123) { BASEDIR_WIN = $$replace(BASEDIR,"/","\\") TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\debug\SDL.dll\" + QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\debug\\SDL.dll\" QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y @@ -365,15 +366,13 @@ win32-g++ { RC_FILE = $$BASEDIR/qgroundcontrol.rc # Copy dependencies - BASEDIR_WIN = $$replace(BASEDIR,"/","\\") - TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\debug\SDL.dll\" - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\debug\models\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\release\models\" /S /E /Y + QMAKE_PRE_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/debug/SDL.dll + QMAKE_PRE_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/release/SDL.dll + QMAKE_PRE_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/debug/audio + QMAKE_PRE_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/release/audio + QMAKE_PRE_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/debug/models + QMAKE_PRE_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/release/models # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index ee6deb127a12ca66bba572b8ba715b2f241fb433..f7cbd8f43d98af22e8a403dbac93becf2c066678 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -23,6 +23,7 @@ # Version from GIT repository is preferred # include ( "../qmapcontrol/QMapControl/QMapControl.pri" ) #{ # Include bundled version if necessary + include(lib/QMapControl/QMapControl.pri) # message("Including bundled QMapControl version as FALLBACK. This is fine on Linux and MacOS, but not the best choice in Windows") @@ -139,13 +140,18 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCPxImuFirmwareUpdate.ui \ src/ui/QGCDataPlot2D.ui \ src/ui/QGCRemoteControlView.ui \ + src/ui/WaypointGlobalView.ui \ src/ui/QMap3D.ui \ src/ui/QGCWebView.ui \ src/ui/map3D/QGCGoogleEarthView.ui \ src/ui/map3D/QGCGoogleEarthViewWin.ui \ - src/ui/map3D/QGCGoogleEarthControls.ui + src/ui/map3D/QGCGoogleEarthControls.ui \ + src/ui/SlugsDataSensorView.ui \ + src/ui/SlugsHilSim.ui \ + src/ui/SlugsPIDControl.ui \ + src/ui/SlugsVideoCamControl.ui \ + src/ui/SlugsPadCameraControl.ui -# src/ui/WaypointGlobalView.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -160,6 +166,7 @@ INCLUDEPATH += src \ src/ui/param \ src/ui/watchdog \ src/ui/map3D + HEADERS += src/MG.h \ src/Core.h \ src/uas/UASInterface.h \ @@ -236,8 +243,14 @@ HEADERS += src/MG.h \ src/ui/RadioCalibration/AbstractCalibrator.h \ src/comm/QGCMAVLink.h \ src/ui/QGCWebView.h \ - src/ui/map3D/QGCGoogleEarthView.h \ - src/ui/map3D/QGCWebPage.h + src/ui/map3D/QGCWebPage.h \ + src/ui/map3D/QGCGoogleEarthView.h\ + src/ui/SlugsDataSensorView.h \ + src/ui/SlugsHilSim.h \ + src/ui/SlugsPIDControl.h \ + src/ui/SlugsVideoCamControl.h \ + src/ui/SlugsPadCameraControl.h + contains(DEPENDENCIES_PRESENT, osg) { message("Including headers for OpenSceneGraph") @@ -269,6 +282,7 @@ contains(DEPENDENCIES_PRESENT, libfreenect) { # Enable only if libfreenect is available HEADERS += src/input/Freenect.h } + SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -340,7 +354,12 @@ SOURCES += src/main.cc \ src/ui/RadioCalibration/RadioCalibrationData.cc \ src/ui/QGCWebView.cc \ src/ui/map3D/QGCGoogleEarthView.cc \ - src/ui/map3D/QGCWebPage.cc + src/ui/map3D/QGCWebPage.cc \ + src/ui/SlugsDataSensorView.cc \ + src/ui/SlugsHilSim.cc \ + src/ui/SlugsPIDControl.cpp \ + src/ui/SlugsVideoCamControl.cpp \ + src/ui/SlugsPadCameraControl.cpp contains(DEPENDENCIES_PRESENT, osg) { message("Including sources for OpenSceneGraph") @@ -358,7 +377,9 @@ contains(DEPENDENCIES_PRESENT, osg) { src/ui/map3D/Texture.cc \ src/ui/map3D/Imagery.cc \ src/ui/map3D/HUDScaleGeode.cc \ - src/ui/map3D/WaypointGroupNode.cc + src/ui/map3D/WaypointGroupNode.cc \ + + contains(DEPENDENCIES_PRESENT, osgearth) { message("Including sources for osgEarth") diff --git a/src/Waypoint.cc b/src/Waypoint.cc index ed9207bc34d0f254469911284e487b95e46df854..bb351a4052818c54087fd21bcf9c9172d5d01e40 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -77,6 +77,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 216b60abc8a5013972bbbb7e6e1d6f07b24804b7..6e4f6ae8a9e94b959fcbcb60e11cfe5f426bbccf 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -63,6 +63,7 @@ public: void save(QTextStream &saveStream); bool load(QTextStream &loadStream); + protected: quint16 id; float x; diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index d41e946be612593889b4866babd0f105146558b7..d4b48cdc1143beda514fabbeb9dcd5e77eb693de 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -64,7 +64,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P // Set the port name if (porthandle == "") { - name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)"); +// name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)"); + name = tr("Serial Link ") + QString::number(getId()); } else { @@ -89,6 +90,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P #else // *nix (Linux, MacOS tested) serial port support port = new QextSerialPort(porthandle, QextSerialPort::Polling); + //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time port->setBaudRate(baudrate); port->setFlowControl(flow); diff --git a/src/configuration.h b/src/configuration.h index 16650c1e009f189bc4a2c18038ccf67d74ffc50e..3420e0f39a6bf6678f63ee613042781b1df6142f 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -1,8 +1,14 @@ #ifndef CONFIGURATION_H #define CONFIGURATION_H +#include "mavlink.h" + /** @brief Polling interval in ms */ -#define SERIAL_POLL_INTERVAL 2 +#ifdef MAVLINK_ENABLED_SLUGS + #define SERIAL_POLL_INTERVAL 7 +#else + #define SERIAL_POLL_INTERVAL 2 +#endif /** @brief Heartbeat emission rate, in Hertz (times per second) */ #define MAVLINK_HEARTBEAT_DEFAULT_RATE 1 diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 5fbf1c931041a58a3e25447ed2e40e872909515a..025087bf9ceee405e9cc6baecf1cb792d80f38e8 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -1,34 +1,52 @@ -/*===================================================================== +#include "SlugsMAV.h" -QGroundControl Open Source Ground Control Station +#include -(c) 2009, 2010 QGROUNDCONTROL PROJECT +SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : + UAS(mavlink, id)//, + // Place other initializers here +{ + widgetTimer = new QTimer (this); + widgetTimer->setInterval(SLUGS_UPDATE_RATE); -This file is part of the QGROUNDCONTROL project + connect (widgetTimer, SIGNAL(timeout()), + this, SLOT(emitSignals())); - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + widgetTimer->start(); - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + // clear all the state structures + memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t)); - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . +#ifdef MAVLINK_ENABLED_SLUGS -======================================================================*/ -#include "SlugsMAV.h" + memset(&mlGpsData, 0, sizeof(mavlink_gps_raw_t)); + memset(&mlCpuLoadData, 0, sizeof(mavlink_cpu_load_t)); + memset(&mlAirData, 0, sizeof(mavlink_air_data_t)); + memset(&mlSensorBiasData, 0, sizeof(mavlink_sensor_bias_t)); + memset(&mlDiagnosticData, 0, sizeof(mavlink_diagnostic_t)); + memset(&mlPilotConsoleData, 0, sizeof(mavlink_pilot_console_t)); + memset(&mlFilteredData ,0, sizeof(mavlink_filtered_data_t)); + memset(&mlBoot ,0, sizeof(mavlink_boot_t)); + memset(&mlGpsDateTime ,0, sizeof(mavlink_gps_date_time_t)); + memset(&mlApMode ,0, sizeof(mavlink_set_mode_t)); + memset(&mlPwmCommands ,0, sizeof(mavlink_pwm_commands_t)); + memset(&mlPidValues ,0, sizeof(mavlink_pid_values_t)); + memset(&mlSinglePid ,0, sizeof(mavlink_pid_t)); + memset(&mlNavigation ,0, sizeof(mavlink_slugs_navigation_t)); + memset(&mlDataLog ,0, sizeof(mavlink_data_log_t)); + memset(&mlPassthrough ,0, sizeof(mavlink_ctrl_srfc_pt_t)); + memset(&mlActionAck,0, sizeof(mavlink_action_ack_t)); + memset(&mlAction ,0, sizeof(mavlink_slugs_action_t)); -#include -SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : - UAS(mavlink, id)//, - // Place other initializers here -{ + updateRoundRobin = 0; + + + + uasId = id; + + #endif } /** @@ -42,106 +60,179 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { // Let UAS handle the default message set - UAS::receiveMessage(link, message); + UAS::receiveMessage(link, message); + // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: - { - qDebug() << "SLUGS RECEIVED HEARTBEAT"; - break; - } #ifdef MAVLINK_ENABLED_SLUGS - case MAVLINK_MSG_ID_CPU_LOAD: - { - mavlink_cpu_load_t cpu_load; - quint64 time = getUnixTime(0); - mavlink_msg_cpu_load_decode(&message,&cpu_load); + case MAVLINK_MSG_ID_BOOT: + mavlink_msg_boot_decode(&message,&mlBoot); + emit slugsBootMsg(uasId, mlBoot); + break; - emit valueChanged(uasId, tr("SensorDSC Load"), cpu_load.sensLoad, time); - emit valueChanged(uasId, tr("ControlDSC Load"), cpu_load.ctrlLoad, time); - emit valueChanged(uasId, tr("Battery Volt"), cpu_load.batVolt, time); + case MAVLINK_MSG_ID_ATTITUDE: + mavlink_msg_attitude_decode(&message, &mlAttitude); + break; + case MAVLINK_MSG_ID_GPS_RAW: + mavlink_msg_gps_raw_decode(&message, &mlGpsData); break; - } - case MAVLINK_MSG_ID_AIR_DATA: - { - mavlink_air_data_t air_data; - quint64 time = getUnixTime(0); - mavlink_msg_air_data_decode(&message,&air_data); - emit valueChanged(uasId, tr("Dynamic Pressure"), air_data.dynamicPressure,time); - emit valueChanged(uasId, tr("Static Pressure"),air_data.staticPressure, time); - emit valueChanged(uasId, tr("Temp"),air_data.temperature,time); + case MAVLINK_MSG_ID_ACTION_ACK: // 62 + mavlink_msg_action_ack_decode(&message,&mlActionAck); break; - } - case MAVLINK_MSG_ID_SENSOR_BIAS: - { - mavlink_sensor_bias_t sensor_bias; - quint64 time = getUnixTime(0); - mavlink_msg_sensor_bias_decode(&message,&sensor_bias); - emit valueChanged(uasId,tr("Ax Bias"),sensor_bias.axBias, time); - emit valueChanged(uasId,tr("Ay Bias"),sensor_bias.ayBias,time); - emit valueChanged(uasId,tr("Az Bias"),sensor_bias.azBias,time); - emit valueChanged(uasId,tr("Gx Bias"),sensor_bias.gxBias,time); - emit valueChanged(uasId,tr("Gy Bias"),sensor_bias.gyBias,time); - emit valueChanged(uasId,tr("Gz Bias"),sensor_bias.gzBias,time); + case MAVLINK_MSG_ID_CPU_LOAD: //170 + mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData); break; - } - case MAVLINK_MSG_ID_DIAGNOSTIC: - { - mavlink_diagnostic_t diagnostic; - quint64 time = getUnixTime(0); - mavlink_msg_diagnostic_decode(&message,&diagnostic); - emit valueChanged(uasId,tr("Diag F1"),diagnostic.diagFl1,time); - emit valueChanged(uasId,tr("Diag F2"),diagnostic.diagFl2,time); - emit valueChanged(uasId,tr("Diag F3"),diagnostic.diagFl3,time); - emit valueChanged(uasId,tr("Diag S1"),diagnostic.diagSh1,time); - emit valueChanged(uasId,tr("Diag S2"),diagnostic.diagSh2,time); - emit valueChanged(uasId,tr("Diag S3"),diagnostic.diagSh3,time); + case MAVLINK_MSG_ID_AIR_DATA: //171 + mavlink_msg_air_data_decode(&message,&mlAirData); break; - } - case MAVLINK_MSG_ID_PILOT_CONSOLE: - { - mavlink_pilot_console_t pilot; - quint64 time = getUnixTime(0); - mavlink_msg_pilot_console_decode(&message,&pilot); - emit valueChanged(uasId,"dt",pilot.dt,time); - emit valueChanged(uasId,"dla",pilot.dla,time); - emit valueChanged(uasId,"dra",pilot.dra,time); - emit valueChanged(uasId,"dr",pilot.dr,time); - emit valueChanged(uasId,"de",pilot.de,time); + case MAVLINK_MSG_ID_SENSOR_BIAS: //172 + mavlink_msg_sensor_bias_decode(&message,&mlSensorBiasData); + break; + + case MAVLINK_MSG_ID_DIAGNOSTIC: //173 + mavlink_msg_diagnostic_decode(&message,&mlDiagnosticData); + break; + + case MAVLINK_MSG_ID_PILOT_CONSOLE: //174 + mavlink_msg_pilot_console_decode(&message,&mlPilotConsoleData); break; - } - case MAVLINK_MSG_ID_PWM_COMMANDS: - { - mavlink_pwm_commands_t pwm; - quint64 time = getUnixTime(0); - mavlink_msg_pwm_commands_decode(&message,&pwm); - emit valueChanged(uasId,"dt_c",pwm.dt_c,time); - emit valueChanged(uasId,"dla_c",pwm.dla_c,time); - emit valueChanged(uasId,"dra_c",pwm.dra_c,time); - emit valueChanged(uasId,"dr_c",pwm.dr_c,time); - emit valueChanged(uasId,"dle_c",pwm.dle_c,time); - emit valueChanged(uasId,"dre_c",pwm.dre_c,time); - emit valueChanged(uasId,"dlf_c",pwm.dlf_c,time); - emit valueChanged(uasId,"drf_c",pwm.drf_c,time); - emit valueChanged(uasId,"da1_c",pwm.aux1,time); - emit valueChanged(uasId,"da2_c",pwm.aux2,time); + case MAVLINK_MSG_ID_PWM_COMMANDS: //175 + mavlink_msg_pwm_commands_decode(&message,&mlPwmCommands); + break; + + case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176 + mavlink_msg_slugs_navigation_decode(&message,&mlNavigation); + break; + + case MAVLINK_MSG_ID_DATA_LOG: //177 + mavlink_msg_data_log_decode(&message,&mlDataLog); + break; + + case MAVLINK_MSG_ID_FILTERED_DATA: //178 + mavlink_msg_filtered_data_decode(&message,&mlFilteredData); + break; + + case MAVLINK_MSG_ID_GPS_DATE_TIME: //179 + mavlink_msg_gps_date_time_decode(&message,&mlGpsDateTime); + break; + + case MAVLINK_MSG_ID_MID_LVL_CMDS: //180 + + break; + + case MAVLINK_MSG_ID_CTRL_SRFC_PT: //181 break; + + case MAVLINK_MSG_ID_PID: //182 + memset(&mlSinglePid,0,sizeof(mavlink_pid_t)); + mavlink_msg_pid_decode(&message, &mlSinglePid); + qDebug() << "\nSLUGS RECEIVED PID Message = "< 10? 1: updateRoundRobin + 1; + + +} + +#ifdef MAVLINK_ENABLED_SLUGS +void SlugsMAV::emitGpsSignals (void){ + + if (mlGpsData.fix_type > 0){ + emit globalPositionChanged(this, + mlGpsData.lon, + mlGpsData.lat, + mlGpsData.alt, + 0.0); + + // Smaller than threshold and not NaN + if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ + emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); + } else { + emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); } + } +} + +void SlugsMAV::emitPidSignal() +{ + qDebug() << "\nSLUGS RECEIVED PID Message"; + emit slugsPidValues(uasId, mlSinglePid); + } + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index e62866f3f0a015bf2ad537d41f927defa695bf29..295c8206530267bda6c4574215251824b628bf10 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -25,7 +25,10 @@ This file is part of the QGROUNDCONTROL project #define SLUGSMAV_H #include "UAS.h" +#include "mavlink.h" +#include +#define SLUGS_UPDATE_RATE 100 // in ms class SlugsMAV : public UAS { Q_OBJECT @@ -36,6 +39,88 @@ public: public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); + + void emitSignals (void); + + + +signals: + + void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData); + +#ifdef MAVLINK_ENABLED_SLUGS + + void slugsCPULoad(int systemId, const mavlink_cpu_load_t& cpuLoad); + void slugsAirData(int systemId, const mavlink_air_data_t& airData); + void slugsSensorBias(int systemId, const mavlink_sensor_bias_t& sensorBias); + void slugsDiagnostic(int systemId, const mavlink_diagnostic_t& diagnostic); + void slugsPilotConsolePWM(int systemId, const mavlink_pilot_console_t& pilotConsole); + void slugsPWM(int systemId, const mavlink_pwm_commands_t& pwmCommands); + void slugsNavegation(int systemId, const mavlink_slugs_navigation_t& slugsNavigation); + void slugsDataLog(int systemId, const mavlink_data_log_t& dataLog); + void slugsFilteredData(int systemId, const mavlink_filtered_data_t& filteredData); + void slugsGPSDateTime(int systemId, const mavlink_gps_date_time_t& gpsDateTime); + void slugsActionAck(int systemId, const mavlink_action_ack_t& actionAck); + + void slugsPidValues(int systemId, const mavlink_pid_t& pidValues); + + void slugsBootMsg(int uasId, mavlink_boot_t& boot); + void slugsAttitude(int uasId, mavlink_attitude_t& attitude); + +#endif + +protected: + + typedef struct _mavlink_pid_values_t { + float P[11]; + float I[11]; + float D[11]; + }mavlink_pid_values_t; + + unsigned char updateRoundRobin; + QTimer* widgetTimer; + + + mavlink_raw_imu_t mlRawImuData; + +#ifdef MAVLINK_ENABLED_SLUGS + mavlink_gps_raw_t mlGpsData; + mavlink_attitude_t mlAttitude; + mavlink_cpu_load_t mlCpuLoadData; + mavlink_air_data_t mlAirData; + mavlink_sensor_bias_t mlSensorBiasData; + mavlink_diagnostic_t mlDiagnosticData; + mavlink_pilot_console_t mlPilotConsoleData; + mavlink_filtered_data_t mlFilteredData; + mavlink_boot_t mlBoot; + mavlink_gps_date_time_t mlGpsDateTime; + mavlink_mid_lvl_cmds_t mlMidLevelCommands; + mavlink_set_mode_t mlApMode; + mavlink_pwm_commands_t mlPwmCommands; + mavlink_pid_values_t mlPidValues; + mavlink_pid_t mlSinglePid; + + mavlink_slugs_navigation_t mlNavigation; + mavlink_data_log_t mlDataLog; + mavlink_ctrl_srfc_pt_t mlPassthrough; + mavlink_action_ack_t mlActionAck; + + mavlink_slugs_action_t mlAction; + + + + + // Standart messages MAVLINK used by SLUGS +private: + + + void emitGpsSignals (void); + void emitPidSignal(void); + + int uasId; + +#endif // if SLUGS + }; #endif // SLUGSMAV_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index baac28f2d9f78f6cd24e37383a150ed47f92b4d4..60f0e043f9c7259b5a7e0182e54cfec2029384ab 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -637,12 +637,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) for (int i=0; i radioData = new RadioCalibrationData(aileron, - elevator, - rudder, - gyro, - pitch, - throttle); + QPointer radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); emit radioCalibrationReceived(radioData); delete radioData; } diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 9e840344063cabffe829e3e72d6b0e893bbe24b7..1aa7c88126d962cd5e976ee81b78f8f5f0af2c17 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -391,6 +391,11 @@ signals: * @param fix 0: No IR/Ultrasound sensor, N > 0: Found N active sensors */ void irUltraSoundLocalizationChanged(UASInterface* uas, int fix); + + + + + }; Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0"); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 6c7e177767d53e1d2dfd0f7879f2a887e07bb9d2..d1761bd3b093817d805cf63aedc4a1909b200d18 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -154,6 +154,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ current_partner_compid = 0; protocol_timer.stop(); + emit readGlobalWPFromUAS(false); emit updateStatusString("done."); qDebug() << "got all waypoints from ID " << systemId; @@ -295,6 +296,7 @@ void UASWaypointManager::addWaypoint(Waypoint *wp) { wp->setId(waypoints.size()); waypoints.insert(waypoints.size(), wp); + emit waypointListChanged(); } } @@ -384,7 +386,20 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) } file.close(); + emit loadWPFile(); emit waypointListChanged(); + +} + + +void UASWaypointManager::globalAddWaypoint(Waypoint *wp) +{ + +} + +int UASWaypointManager::globalRemoveWaypoint(quint16 seq) +{ + return 0; } void UASWaypointManager::clearWaypointList() @@ -405,6 +420,7 @@ void UASWaypointManager::clearWaypointList() void UASWaypointManager::readWaypoints() { + emit readGlobalWPFromUAS(true); if(current_state == WP_IDLE) { while(waypoints.size()>0) @@ -412,6 +428,7 @@ void UASWaypointManager::readWaypoints() Waypoint *t = waypoints.back(); delete t; waypoints.pop_back(); + } protocol_timer.start(PROTOCOL_TIMEOUT_MS); @@ -423,6 +440,7 @@ void UASWaypointManager::readWaypoints() current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; sendWaypointRequestList(); + } } @@ -532,6 +550,7 @@ void UASWaypointManager::sendWaypointCount() wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpc.count = current_count; + qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system; emit updateStatusString(QString("start transmitting waypoints...")); mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); @@ -556,6 +575,8 @@ void UASWaypointManager::sendWaypointRequestList() MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000); qDebug() << "sent waypoint list request to ID " << wprl.target_system; + + } void UASWaypointManager::sendWaypointRequest(quint16 seq) diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 85bd61421ce5d229df76a10496dec5c2a7642e7b..39c13194c6b7cea7bc3a2ac3fb44b9a592f3749e 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -91,6 +91,19 @@ public: void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile void loadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile + void localAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly + int localRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + 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 */ + /*@{*/ + const QVector &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list. + void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly + int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage /*@}*/ private: @@ -113,6 +126,9 @@ 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 + void readGlobalWPFromUAS(bool value); ///< emits signal when finish to read Global WP from UAS + 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 b053b35c4b00f17401ef424023b95615d7ae0246..9d149644ece5971a715abe4d5db231bca9f91bf8 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -170,6 +170,18 @@ void MainWindow::buildWidgets() //FIXME: free memory in destructor joystick = new JoystickInput(); + slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); + slugsDataWidget->setWidget( new SlugsDataSensorView(this)); + + slugsPIDControlWidget = new QDockWidget(tr("PID Control"), this); + slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); + + slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); + slugsHilSimWidget->setWidget( new SlugsHilSim(this)); + + slugsCamControlWidget = new QDockWidget(tr("Video Camera Control"), this); + slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); + } /** @@ -203,9 +215,25 @@ void MainWindow::connectWidgets() // it notifies that a waypoint global goes to do create and a map graphic too connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF))); - // it notifies that a waypoint global change it´s position by spinBox on Widget WaypointView + // it notifies that a waypoint global change itÂ¥s position by spinBox on Widget WaypointView //connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); + // connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); + + connect(slugsCamControlWidget->widget(),SIGNAL(viewCamBorderAtMap(bool)),mapWidget,SLOT(drawBorderCamAtMap(bool))); + connect(slugsCamControlWidget->widget(),SIGNAL(changeCamPosition(double,double,QString)),mapWidget,SLOT(updateCameraPosition(double,double, QString))); + } + + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); } + + if (slugsDataWidget && slugsDataWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); + } + + } void MainWindow::arrangeCenterStack() @@ -392,6 +420,11 @@ void MainWindow::connectActions() // Joystick configuration connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + // Slugs View + connect(ui.actionShow_Slugs_View, SIGNAL(triggered()), this, SLOT(loadSlugsView())); + + //GlobalOperatorView + // connect(ui.actionGlobalOperatorView,SIGNAL(triggered()),waypointsDockWidget->widget(),SLOT()) } @@ -549,9 +582,18 @@ void MainWindow::UASCreated(UASInterface* uas) // Check which type this UAS is of PxQuadMAV* mav = dynamic_cast(uas); if (mav) loadPixhawkView(); - SlugsMAV* mav2 = dynamic_cast(uas); - if (mav2) loadSlugsView(); + if (slugsDataWidget) { + SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); + if (dataWidget) { + SlugsMAV* mav2 = dynamic_cast(uas); + if (mav2) { + dataWidget->addUAS(uas); + //loadSlugsView(); + loadGlobalOperatorView(); + } + } + } } } @@ -638,17 +680,6 @@ void MainWindow::loadSlugsView() infoDockWidget->show(); } - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } // WAYPOINT LIST if (waypointsDockWidget) @@ -664,13 +695,19 @@ void MainWindow::loadSlugsView() debugConsoleDockWidget->show(); } - // ONBOARD PARAMETERS - if (parametersDockWidget) + // Slugs Data View + if (slugsDataWidget) { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); + addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); + slugsDataWidget->show(); } + // Slugs Data View + if (slugsHilSimWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget); + slugsHilSimWidget->show(); + } this->show(); } @@ -904,169 +941,193 @@ void MainWindow::loadGlobalOperatorView() } } - // UAS CONTROL - if (controlDockWidget) + // WAYPOINT LIST + if (waypointsDockWidget) { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } - // UAS LIST - if (listDockWidget) + // Slugs Data View + if (slugsDataWidget) { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); + addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); + slugsDataWidget->show(); } - // UAS STATUS - if (infoDockWidget) + // Slugs Data View + if (slugsPIDControlWidget) { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); + addDockWidget(Qt::LeftDockWidgetArea, slugsPIDControlWidget); + slugsPIDControlWidget->show(); } - // WAYPOINT LIST - if (waypointsDockWidget) + if (slugsCamControlWidget) { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); + addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); + slugsCamControlWidget->show(); } - // // HORIZONTAL SITUATION INDICATOR - // if (hsiDockWidget) - // { - // HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - // if (hsi) - // { - // addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); - // hsiDockWidget->show(); - // hsi->start(); - // } - // } + + +// // UAS CONTROL +// if (controlDockWidget) +// { +// addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); +// controlDockWidget->show(); +// } + +// // UAS LIST +// if (listDockWidget) +// { +// addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); +// listDockWidget->show(); +// } + +// // UAS STATUS +// if (infoDockWidget) +// { +// addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); +// infoDockWidget->show(); +// } + + +// // HORIZONTAL SITUATION INDICATOR +// if (hsiDockWidget) +// { +// HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); +// if (hsi) +// { +// addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); +// hsiDockWidget->show(); +// hsi->start(); +// } +// } // PROCESS CONTROL - if (watchdogControlDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); - watchdogControlDockWidget->show(); - } +// if (watchdogControlDockWidget) +// { +// addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); +// watchdogControlDockWidget->show(); +// } // HEAD UP DISPLAY - if (headUpDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, headUpDockWidget); - // FIXME Replace with default ->show() call - HUD* hud = dynamic_cast(headUpDockWidget->widget()); - - if (hud) - { - headUpDockWidget->show(); - hud->start(); - } - } +// if (headUpDockWidget) +// { +// addDockWidget(Qt::RightDockWidgetArea, headUpDockWidget); +// // FIXME Replace with default ->show() call +// HUD* hud = dynamic_cast(headUpDockWidget->widget()); + +// if (hud) +// { +// headUpDockWidget->show(); +// hud->start(); +// } +// } } void MainWindow::load3DMapView() { #ifdef QGC_OSGEARTH_ENABLED - clearView(); + clearView(); - // 3D map - if (_3DMapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DMapWidget); - } - } + // 3D map + if (_3DMapWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DMapWidget); + } + } - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } #endif - this->show(); + this->show(); } void MainWindow::loadGoogleEarthView() { #if (defined Q_OS_WIN) | (defined Q_OS_MAC) - clearView(); + clearView(); - // 3D map - if (gEarthWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(gEarthWidget); - } - } + // 3D map + if (gEarthWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + centerStack->setCurrentWidget(gEarthWidget); + } + } - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } - this->show(); + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } + this->show(); #endif } @@ -1075,53 +1136,53 @@ void MainWindow::loadGoogleEarthView() void MainWindow::load3DView() { #ifdef QGC_OSG_ENABLED - clearView(); + clearView(); - // 3D map - if (_3DWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } + // 3D map + if (_3DWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DWidget); + } + } - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } #endif - this->show(); + this->show(); } diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 38db7929181b952c0693c75267a607b5b92c3c4d..f423c60ed8dc87585bb832d4fabd6b2de844b1e3 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -64,8 +64,16 @@ This file is part of the QGROUNDCONTROL project #include "QGCDataPlot2D.h" #include "QGCRemoteControlView.h" #include "QGCGoogleEarthView.h" +//#include "QMap3DWidget.h" +#include "SlugsDataSensorView.h" #include "LogCompressor.h" +#include "SlugsPIDControl.h" + +#include "slugshilsim.h" + +#include "SlugsVideoCamControl.h" + /** * @brief Main Application Window @@ -188,6 +196,11 @@ protected: QPointer headUpDockWidget; QPointer hsiDockWidget; QPointer rcViewDockWidget; + QPointer slugsDataWidget; + QPointer slugsPIDControlWidget; + QPointer slugsHilSimWidget; + QPointer slugsCamControlWidget; + // Popup widgets JoystickWidget* joystickWidget; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 03960b620625a5611e3e29092ae425e1b8f2fd0a..323c51a5c2d34cf4a521cb06fc97b66470f1d7dc 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -84,6 +84,7 @@ + @@ -338,6 +339,15 @@ Google Earth View + + + + :/images/contrib/slugs.png:/images/contrib/slugs.png + + + Show Slugs View + + diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index f866f7fc7fdcb9c3cf6421cb613bc052652acc54..1f244dd5b732697c5431fdfc71d35fa78c541e3f 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project #include #include + #include "MapWidget.h" #include "ui_MapWidget.h" #include "UASInterface.h" @@ -42,6 +43,7 @@ This file is part of the QGROUNDCONTROL project #include "MG.h" + MapWidget::MapWidget(QWidget *parent) : QWidget(parent), zoomLevel(0), @@ -84,6 +86,8 @@ MapWidget::MapWidget(QWidget *parent) : geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); mc->addLayer(geomLayer); + + // // Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer); // mc->addLayer(gsatLayer); @@ -218,6 +222,19 @@ MapWidget::MapWidget(QWidget *parent) : path = new qmapcontrol::LineString (wps, "UAV Path", pointPen); mc->layer("Waypoints")->addGeometry(path); + //Camera Control + // CAMERA INDICATOR LAYER + // create a layer with the mapadapter and type GeometryLayer (for camera indicator) + camLayer = new qmapcontrol::GeometryLayer("Camera", mapadapter); + mc->addLayer(camLayer); + + //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen); + + drawCamBorder = false; + radioCamera = 10; + + + this->setVisible(false); } @@ -340,10 +357,19 @@ void MapWidget::createPathButtonClicked(bool checked) } +/** + * Captures a click on the map and if in create WP path mode, it adds the WP on MouseButtonRelease + * + * @param event The mouse event + * @param coordinate The coordinate in which it occured the mouse event + * @note This slot is connected to the mouseEventCoordinate of the QMapControl object + */ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate) { + qDebug() << mc->mouseMode(); + if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()) { // Create waypoint name @@ -381,13 +407,27 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) { + if (!wpExists(coordinate)){ // Create waypoint name QString str; + 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); @@ -395,14 +435,27 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) path->addPoint(tempPoint); wpIndex.insert(str,tempPoint); + qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude(); - // Refresh the screen + // Refresh the screen mc->updateRequestNew(); + } //// // emit signal mouse was clicked // emit captureMapCoordinateClick(coordinate); } +int MapWidget::wpExists(const QPointF coordinate){ + for (int i = 0; i < wps.size(); i++){ + if (wps.at(i)->latitude() == coordinate.y() && + wps.at(i)->longitude()== coordinate.x()){ + return 1; + } + } + return 0; +} + + void MapWidget::captureGeometryClick(Geometry* geom, QPoint point) { Q_UNUSED(geom); @@ -414,7 +467,7 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point) void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) { - Q_UNUSED(coordinate); + waypointIsDrag = true; @@ -445,13 +498,15 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) { - waypointIsDrag = false; + // TODO: Investigate why when creating the waypoint path this slot is being called + + // Only change the mouse mode back to panning when not creating a WP path + if (!createPath->isChecked()){ + waypointIsDrag = false; mc->setMouseMode(qmapcontrol::MapControl::Panning); + } -// qDebug() << geom->name(); -// qDebug() << geom->GeometryType; -// qDebug() << point; } MapWidget::~MapWidget() @@ -580,6 +635,10 @@ void MapWidget::wheelEvent(QWheelEvent *event) // Detail zoom level is the number of steps zoomed in further // after the bounding has taken effect detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom)); + + // visual field of camera + updateCameraPosition(20*newZoom,0,"no"); + } void MapWidget::keyPressEvent(QKeyEvent *event) @@ -669,3 +728,68 @@ void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, floa } +void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) +{ + //camPoints.clear(); + QPointF currentPos = mc->currentCoordinate(); +// QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance); + +// qmapcontrol::Point* tempPoint1 = new qmapcontrol::Point(currentPos.x(), currentPos.y(),"inicial",qmapcontrol::Point::Middle); +// qmapcontrol::Point* tempPoint2 = new qmapcontrol::Point(actualPos.x(), actualPos.y(),"final",qmapcontrol::Point::Middle); + +// camPoints.append(tempPoint1); +// camPoints.append(tempPoint2); + +// camLine->setPoints(camPoints); + + QPen* camBorderPen = new QPen(QColor(255,0,0)); + camBorderPen->setWidth(2); + + //radio = mc->currentZoom() + + if(drawCamBorder) + { + //clear camera borders + mc->layer("Camera")->clearGeometries(); + + //create a camera borders + qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen); + + //camBorder->setCoordinate(currentPos); + + mc->layer("Camera")->addGeometry(camBorder); + // mc->layer("Camera")->addGeometry(camLine); + mc->updateRequestNew(); + + } + else + { + //clear camera borders + mc->layer("Camera")->clearGeometries(); + mc->updateRequestNew(); + + } + + +} + +void MapWidget::drawBorderCamAtMap(bool status) +{ + drawCamBorder = status; + updateCameraPosition(20,0,"no"); + +} + +QPointF MapWidget::getPointxBearing_Range(double lat1, double lon1, double bearing, double distance) +{ + QPointF temp; + + double rad = M_PI/180; + + bearing = bearing*rad; + temp.setX((lon1 + ((distance/60) * (sin(bearing))))); + temp.setY((lat1 + ((distance/60) * (cos(bearing))))); + + return temp; +} + diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index 450ed255f1d7a0ad141d736254845d17447f81a2..66125a4ae33cbad8c550aff2007c3e2527a669fe 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -37,6 +37,11 @@ This file is part of the QGROUNDCONTROL project #include #include "qmapcontrol.h" #include "UASInterface.h" +#include "QPointF" + +#include + + class QMenu; @@ -63,10 +68,13 @@ public slots: void activeUASSet(UASInterface* uas); void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); void updatePosition(float time, double lat, double lon); + void updateCameraPosition(double distance, double bearing, QString dir); + QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance); //ROCA void clearPath(); void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon); + void drawBorderCamAtMap(bool status); protected: void changeEvent(QEvent* e); @@ -93,6 +101,10 @@ protected: qmapcontrol::Layer* l; ///< Current map layer (background) qmapcontrol::Layer* overlay; ///< Street overlay (foreground) qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints + + //only for experiment + qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator + int zoomLevel; int detailZoom; ///< Steps zoomed in further than qMapControl allows static const int scrollStep = 40; ///< Scroll n pixels per keypress @@ -107,13 +119,15 @@ protected: protected slots: void captureMapClick (const QMouseEvent* event, const QPointF coordinate); - void createWaypointGraphAtMap (const QPointF coordinate); - void createPathButtonClicked(bool checked); void captureGeometryClick(Geometry*, QPoint); - void mapproviderSelected(QAction* action); void captureGeometryDrag(Geometry* geom, QPointF coordinate); void captureGeometryEndDrag(Geometry* geom, QPointF coordinate); + void createPathButtonClicked(bool checked); + + void createWaypointGraphAtMap (const QPointF coordinate); + void mapproviderSelected(QAction* action); + @@ -130,7 +144,16 @@ private: QHash wpIndex; qmapcontrol::LineString* path; QPen* pointPen; + int wpExists(const QPointF coordinate); bool waypointIsDrag; + + + qmapcontrol::LineString* camLine; + QList camPoints; + QPointF lastCamBorderPos; + bool drawCamBorder; + int radioCamera; + }; #endif // MAPWIDGET_H diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc new file mode 100644 index 0000000000000000000000000000000000000000..749f2bd19506eb1e35ea26e0c2523616ffe51b0c --- /dev/null +++ b/src/ui/SlugsDataSensorView.cc @@ -0,0 +1,245 @@ +#include "SlugsDataSensorView.h" +#include "ui_SlugsDataSensorView.h" + +#include +#include "SlugsMAV.h" + +#include + +SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) : + QWidget(parent), + ui(new Ui::SlugsDataSensorView) +{ + ui->setupUi(this); + + activeUAS = NULL; + + this->setVisible(false); + + + + +} + +SlugsDataSensorView::~SlugsDataSensorView() +{ + delete ui; +} + +void SlugsDataSensorView::addUAS(UASInterface* uas) +{ + SlugsMAV* slugsMav = dynamic_cast(uas); + + if (slugsMav != NULL) { + + connect(slugsMav, SIGNAL(slugsRawImu(int, const mavlink_raw_imu_t&)), this, SLOT(slugRawDataChanged(int, const mavlink_raw_imu_t&))); + + #ifdef MAVLINK_ENABLED_SLUGS + + //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 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&))); + connect(slugsMav, SIGNAL(slugsNavegation(int,const mavlink_slugs_navigation_t&)),this,SLOT(slugsNavegationChanged(int,const mavlink_slugs_navigation_t&))); + connect(slugsMav, SIGNAL(slugsDataLog(int,const mavlink_data_log_t&)), this, SLOT(slugsDataLogChanged(int,const mavlink_data_log_t&))); + connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); + connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); + 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 + if(activeUAS == 0) { + activeUAS = uas; + } + + } +} + +void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t &rawData){ + Q_UNUSED(uasId); + + ui->m_Axr->setText(QString::number(rawData.xacc)); + ui->m_Ayr->setText(QString::number(rawData.yacc)); + ui->m_Azr->setText(QString::number(rawData.zacc)); +} + +void SlugsDataSensorView::setActiveUAS(UASInterface* uas){ + activeUAS = uas; +} + +#ifdef MAVLINK_ENABLED_SLUGS + +void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas, + double lat, + double lon, + double alt, + quint64 time) { + Q_UNUSED(uas); + Q_UNUSED(time); + + ui->m_GpsLatitude->setText(QString::number(lat)); + ui->m_GpsLongitude->setText(QString::number(lon)); + ui->m_GpsHeight->setText(QString::number(alt)); +} + + +void SlugsDataSensorView::slugLocalPositionChanged(UASInterface* uas, + double x, + double y, + double z, + quint64 time) { + Q_UNUSED(uas); + Q_UNUSED(time); + + ui->ed_x->setPlainText(QString::number(x)); + ui->ed_y->setPlainText(QString::number(y)); + ui->ed_z->setPlainText(QString::number(z)); + +} + +void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface* uas, + double vx, + double vy, + double vz, + quint64 time) { + Q_UNUSED( uas); + Q_UNUSED(time); + + ui->ed_vx->setPlainText(QString::number(vx)); + ui->ed_vy->setPlainText(QString::number(vy)); + ui->ed_vz->setPlainText(QString::number(vz)); + +} + +void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas, + double slugroll, + double slugpitch, + double slugyaw, + quint64 time) +{ + Q_UNUSED( uas); + Q_UNUSED(time); + + ui->m_Roll->setPlainText(QString::number(slugroll)); + ui->m_Pitch->setPlainText(QString::number(slugpitch)); + ui->m_Yaw->setPlainText(QString::number(slugyaw)); + +} + + +void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, + const mavlink_sensor_bias_t& sensorBias){ + Q_UNUSED( systemId); + + ui->m_AxBiases->setText(QString::number(sensorBias.axBias)); + ui->m_AyBiases->setText(QString::number(sensorBias.ayBias)); + ui->m_AzBiases->setText(QString::number(sensorBias.azBias)); + ui->m_GxBiases->setText(QString::number(sensorBias.gxBias)); + ui->m_GyBiases->setText(QString::number(sensorBias.gyBias)); + ui->m_GzBiases->setText(QString::number(sensorBias.gzBias)); + +} + +void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId, + const mavlink_diagnostic_t& diagnostic){ + 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)); +} + + +void SlugsDataSensorView::slugsCpuLoadChanged(int systemId, + const mavlink_cpu_load_t& cpuLoad){ + 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)); +} + +void SlugsDataSensorView::slugsNavegationChanged(int systemId, + const mavlink_slugs_navigation_t& slugsNavigation){ + 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)); + ui->m_PsidC->setText(QString::number(slugsNavigation.psiDot_c)); + ui->m_AyBody->setText(QString::number(slugsNavigation.ay_body)); + ui->m_TotRun->setText(QString::number(slugsNavigation.totalDist)); + ui->m_DistToGo->setText(QString::number(slugsNavigation.dist2Go)); + ui->m_FromWP->setText(QString::number(slugsNavigation.fromWP)); + ui->m_ToWP->setText(QString::number(slugsNavigation.toWP)); +} + + + +void SlugsDataSensorView::slugsDataLogChanged(int systemId, + const mavlink_data_log_t& dataLog){ + 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)); + ui->m_logFl4->setText(QString::number(dataLog.fl_4)); + ui->m_logFl5->setText(QString::number(dataLog.fl_5)); + ui->m_logFl6->setText(QString::number(dataLog.fl_6)); +} + +void SlugsDataSensorView::slugsPWMChanged(int systemId, + const mavlink_pwm_commands_t& pwmCommands){ + 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)); + ui->m_pwmRudd->setText(QString::number(pwmCommands.dr_c)); + + ui->m_pwmThroTrim->setText(QString::number(pwmCommands.dre_c)); + ui->m_pwmAileTrim->setText(QString::number(pwmCommands.dlf_c)); + ui->m_pwmElevTrim->setText(QString::number(pwmCommands.drf_c)); + ui->m_pwmRuddTrim->setText(QString::number(pwmCommands.aux1)); + +} + +void SlugsDataSensorView::slugsFilteredDataChanged(int systemId, + const mavlink_filtered_data_t& filteredData){ + 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)); + ui->m_Gxf->setText(QString::number(filteredData.gX)); + ui->m_Gyf->setText(QString::number(filteredData.gY)); + ui->m_Gzf->setText(QString::number(filteredData.gZ)); + ui->m_Mxf->setText(QString::number(filteredData.mX)); + ui->m_Myf->setText(QString::number(filteredData.mY)); + ui->m_Mzf->setText(QString::number(filteredData.mZ)); +} + +void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, + const mavlink_gps_date_time_t& gpsDateTime){ + Q_UNUSED(systemId); + ui->m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" + + QString::number(gpsDateTime.day) + "/" + + QString::number(gpsDateTime.year)); + + ui->m_GpsTime->setText(QString::number(gpsDateTime.hour) + ":" + + QString::number(gpsDateTime.min) + ":" + + QString::number(gpsDateTime.sec)); + + ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); +} + + + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h new file mode 100644 index 0000000000000000000000000000000000000000..ed0b9f07c7dd2195ddca02c739a7b1e795635fc7 --- /dev/null +++ b/src/ui/SlugsDataSensorView.h @@ -0,0 +1,185 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Grpahical presentation of SLUGS generated data + * + * @author Juan F. Robles + * + */ + +#ifndef SLUGSDATASENSORVIEW_H +#define SLUGSDATASENSORVIEW_H + +#include + +#include "UASInterface.h" +#include "SlugsMAV.h" +#include "mavlink.h" + + +namespace Ui { + class SlugsDataSensorView; +} + +class SlugsDataSensorView : public QWidget +{ + Q_OBJECT + +public: + explicit SlugsDataSensorView(QWidget *parent = 0); + ~SlugsDataSensorView(); + +public slots: + /** + * @brief Adds the UAS for data display + * + * Adds the UAS and makes all the correct connections for data display on the Widgets. If + * there is no current UAS active, it sets it as active. + + * @param uas The UAS being added + */ + void addUAS(UASInterface* uas); + + /** + * @brief Sets the UAS as active + * + * @param uas The UAS being set as active + */ + void setActiveUAS(UASInterface* uas); + + + void slugRawDataChanged (int uasId, const mavlink_raw_imu_t& rawData); + +#ifdef MAVLINK_ENABLED_SLUGS + /** + * @brief Adds the UAS for data display + * + * Adds the UAS and makes all the correct connections for data display on the Widgets + */ + void slugLocalPositionChanged(UASInterface* uas, + double x, + double y, + double z, + quint64 time); + /** + * @brief Adds the UAS for data display + * + * Adds the UAS and makes all the correct connections for data display on the Widgets + */ + void slugSpeedLocalPositionChanged(UASInterface* uas, + double vx, + double vy, + double vz, + quint64 time); + /** + * @brief Adds the UAS for data display + * + * Adds the UAS and makes all the correct connections for data display on the Widgets + */ + void slugAttitudeChanged(UASInterface* uas, + double slugroll, + double slugpitch, + double slugyaw, + quint64 time); + + /** + * @brief Adds the UAS for data display + * + * Adds the UAS and makes all the correct connections for data display on the Widgets + */ + void slugsGlobalPositionChanged(UASInterface* uas, + double lat, + double lon, + double alt, + quint64 time); + /** + * @brief Updates the sensor bias widget + */ + void slugsSensorBiasChanged(int systemId, + const mavlink_sensor_bias_t& sensorBias); + + /** + * @brief Updates the diagnostic widget + */ + void slugsDiagnosticMessageChanged(int systemId, + const mavlink_diagnostic_t& diagnostic); + + /** + * @brief Updates the CPU load widget + */ + void slugsCpuLoadChanged(int systemId, + const mavlink_cpu_load_t& cpuLoad); + + + /** + * @brief Updates the Navigation widget + */ + void slugsNavegationChanged(int systemId, + const mavlink_slugs_navigation_t& slugsNavigation); + + /** + * @brief Updates the Data Log widget + */ + void slugsDataLogChanged(int systemId, + const mavlink_data_log_t& dataLog); + + /** + * @brief Updates the PWM Commands widget + */ + void slugsPWMChanged(int systemId, + const mavlink_pwm_commands_t& pwmCommands); + + /** + * @brief Updates the filtered sensor measurements widget + */ + void slugsFilteredDataChanged(int systemId, + const mavlink_filtered_data_t& filteredData); + + + /** + * @brief Updates the gps Date Time widget + */ + void slugsGPSDateTimeChanged(int systemId, + const mavlink_gps_date_time_t& gpsDateTime); + + + +#endif // MAVLINK_ENABLED_SLUGS + +protected: + UASInterface* activeUAS; + +private: + Ui::SlugsDataSensorView *ui; + + + + + + + +}; + +#endif // SLUGSDATASENSORVIEW_H diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui new file mode 100644 index 0000000000000000000000000000000000000000..4a6e2de6bc0a1e9004a25ffdd440a16eb4c1a06a --- /dev/null +++ b/src/ui/SlugsDataSensorView.ui @@ -0,0 +1,4497 @@ + + + SlugsDataSensorView + + + + 0 + 0 + 399 + 667 + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Form + + + + + + 0 + + + + Tab 1 + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Position + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + QFrame::NoFrame + + + X + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Y + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + QFrame::NoFrame + + + Z + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vx + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vy + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vz + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Attitude + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Roll + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Pitch + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Yaw + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Navigation + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + U_m + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + From WP + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Pitch C + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + To WP + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Psi_d C + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + Qt::Vertical + + + + 20 + 13 + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Phi C + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Tot Run + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay body + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Dist to G + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 10 + 75 + true + + + + Diagnostic Messages + + + + + + + + + 10 + 75 + true + + + + Fl1 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Sh1 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl2 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Sh2 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl3 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Sh3 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + + + + + + 10 + 75 + true + + + + Log Messages + + + + + + + + + 10 + 75 + true + + + + Fl1 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl4 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl2 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl5 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl3 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + 10 + 75 + true + + + + Fl6 + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + + + + + + + + + + + Tab 2 + + + + + + + + GPS Data + + + + + + + + + + + 10 + 75 + true + + + + Date + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Time + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + # Sats + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + COG + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + SOG + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + + + + + 10 + 75 + true + + + + Latitude + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Longitude + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Height + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + + 311 + 111 + + + + + 311 + 111 + + + + Sensor Biases + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Axb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gxb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ayb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gyb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Azb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gzb + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 311 + 171 + + + + + 311 + 171 + + + + PWM Commands + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Thro + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Thro Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Aile + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Aile Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Elev + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Elev Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Rudd + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Rudd Trim + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 60 + 18 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + + + + + + + + Tab 3 + + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + CPU Load + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Sensor + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Control + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Batt Volt + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + Air Data + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Dynamic + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Static + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Temperature + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + + + 311 + 213 + + + + + 371 + 213 + + + + Filtered Data + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + 311 + 213 + + + + + 361 + 213 + + + + Raw Data + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Tab 4 + + + + + + + + + diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eec5dd58ffb82a1c1b15eec11bdeb89531df6bb2 --- /dev/null +++ b/src/ui/SlugsPIDControl.cpp @@ -0,0 +1,731 @@ +#include "SlugsPIDControl.h" +#include "ui_SlugsPIDControl.h" + + +#include +#include +#include +#include +#include +#include "LinkManager.h" + +SlugsPIDControl::SlugsPIDControl(QWidget *parent) : + QWidget(parent), + ui(new Ui::SlugsPIDControl) +{ + ui->setupUi(this); + + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUasSet(UASInterface*))); + + activeUAS = NULL; + + setRedColorStyle(); + setGreenColorStyle(); + + refreshTimerGet = new QTimer(this); + refreshTimerGet->setInterval(100); // 20 Hz + connect(refreshTimerGet, SIGNAL(timeout()), this, SLOT(slugsGetGeneral())); + + + refreshTimerSet = new QTimer(this); + refreshTimerSet->setInterval(100); // 20 Hz + connect(refreshTimerSet, SIGNAL(timeout()), this, SLOT(slugsSetGeneral())); + + + counterRefreshGet = 1; + counterRefreshSet = 1; + +} + +/** + * @brief Called when the a new UAS is set to active. + * + * Called when the a new UAS is set to active. + * + * @param uas The new active UAS + */ +void SlugsPIDControl::activeUasSet(UASInterface* uas) +{ + #ifdef MAVLINK_ENABLED_SLUGS + SlugsMAV* slugsMav = dynamic_cast(uas); + + if (slugsMav != NULL) + { + connect(slugsMav,SIGNAL(slugsActionAck(int,const mavlink_action_ack_t&)),this,SLOT(recibeMensaje(int,mavlink_action_ack_t))); + connect(slugsMav,SIGNAL(slugsPidValues(int,mavlink_pid_t)),this, SLOT(receivePidValues(int,mavlink_pid_t)) ); + + connect(ui->setGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartSet())); + } + +#endif // MAVLINK_ENABLED_SLUG + // Set this UAS as active if it is the first one + if(activeUAS == 0) + { + activeUAS = uas; + systemId = activeUAS->getUASID(); + connect_editLinesPDIvalues(); + + //qDebug()<<"------------------->Active UAS ID: "<getUASID(); + } + +} + +/** + * @brief Connect Edition Lines for PID Values + * + * @param + */ +void SlugsPIDControl::connect_editLinesPDIvalues() +{ + if(activeUAS) + { + connect_set_pushButtons(); + connect_get_pushButtons(); + connect_AirSpeed_LineEdit(); + connect_PitchFollowei_LineEdit(); + connect_RollControl_LineEdit(); + connect_HeigthError_LineEdit(); + connect_YawDamper_LineEdit(); + connect_Pitch2dT_LineEdit(); + } +} + +SlugsPIDControl::~SlugsPIDControl() +{ + delete ui; +} + +/** + *@brief Set the background color RED style for the GroupBox PID when change lineEdit information + * + */ +void SlugsPIDControl::setRedColorStyle() +{ + // GroupBox Color + QColor groupColor = QColor(231,72,28); + + QString borderColor = "#FA4A4F"; + + groupColor = groupColor.darker(475); + + + REDcolorStyle = REDcolorStyle.sprintf("QGroupBox {background-color: #%02X%02X%02X; border: 5px solid %s; }", + groupColor.red(), groupColor.green(), groupColor.blue(), borderColor.toStdString().c_str()); + +} + +/** + *@brief Set the background color GREEN style for the GroupBox PID when change lineEdit information + * + */ +void SlugsPIDControl::setGreenColorStyle() +{ + // create Green color style + QColor groupColor = QColor(30,124,16); + QString borderColor = "#24AC23"; + + groupColor = groupColor.darker(475); + + + GREENcolorStyle = GREENcolorStyle.sprintf("QGroupBox {background-color: #%02X%02X%02X; border: 5px solid %s; }", + groupColor.red(), groupColor.green(), groupColor.blue(), borderColor.toStdString().c_str()); + +} + +/** + *@brief Connection Signal and Slot of the set buttons on the widget + */ +void SlugsPIDControl::connect_set_pushButtons() +{ + //ToDo connect buttons set and get. Before create the slots + connect(ui->dT_PID_set_pushButton, SIGNAL(clicked()),this,SLOT(changeColor_GREEN_AirSpeed_groupBox())); + connect(ui->dE_PID_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_PitchFollowei_groupBox())); + connect(ui->dA_PID_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_RollControl_groupBox())); + connect(ui->HELPComm_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_HeigthError_groupBox())); + connect(ui->dR_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_YawDamper_groupBox())); + connect(ui->Pitch2dT_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_Pitch2dT_groupBox())); + + +} + +/** + *@brief Connection Signal and Slot of the set buttons on the widget + */ +void SlugsPIDControl::connect_get_pushButtons() +{ + connect(ui->dT_PID_get_pushButton, SIGNAL(clicked()),this,SLOT(get_AirSpeed_PID())); + connect(ui->dE_PID_get_pushButton,SIGNAL(clicked()),this,SLOT(get_PitchFollowei_PID())); + connect(ui->dR_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_YawDamper_PID())); + connect(ui->dA_PID_get_pushButton,SIGNAL(clicked()),this,SLOT(get_RollControl_PID())); + connect(ui->Pitch2dT_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_Pitch2dT_PID())); + connect(ui->HELPComm_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_HeigthError_PID())); + +} + +// Functions for Air Speed GroupBox +void SlugsPIDControl::connect_AirSpeed_LineEdit() +{ + connect(ui->dT_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString))); + connect(ui->dT_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString))); + connect(ui->dT_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString))); +} + +void SlugsPIDControl::changeColor_RED_AirSpeed_groupBox(QString text) +{ + Q_UNUSED(text); + ui->AirSpeedHold_groupBox->setStyleSheet(REDcolorStyle); + +} + +void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() +{ + + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + + //create the packet +#ifdef MAVLINK_ENABLED_SLUGS + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 0; + pidMessage.pVal = ui->dT_P_set->text().toFloat(); + pidMessage.iVal = ui->dT_I_set->text().toFloat(); + pidMessage.dVal = ui->dT_D_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + + ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +void SlugsPIDControl::get_AirSpeed_PID() +{ + qDebug() << "\nSend Message = Air Speed "; + sendMessagePIDStatus(0); + +} + + + +// Functions for PitchFollowei GroupBox +void SlugsPIDControl::connect_PitchFollowei_LineEdit() +{ + connect(ui->dE_P_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString))); + connect(ui->dE_I_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString))); + connect(ui->dE_D_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString))); +} + +void SlugsPIDControl::changeColor_RED_PitchFollowei_groupBox(QString text) +{ + Q_UNUSED(text); + ui->PitchFlowei_groupBox->setStyleSheet(REDcolorStyle); + +} + +void SlugsPIDControl::changeColor_GREEN_PitchFollowei_groupBox() +{ + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + #ifdef MAVLINK_ENABLED_SLUGS + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 2; + pidMessage.pVal = ui->dE_P_set->text().toFloat(); + pidMessage.iVal = ui->dE_I_set->text().toFloat(); + pidMessage.dVal = ui->dE_D_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + + ui->PitchFlowei_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +void SlugsPIDControl::get_PitchFollowei_PID() +{ + qDebug() << "\nSend Message = Pitch Followei "; + sendMessagePIDStatus(2); + +} + + +// Functions for Roll Control GroupBox +/** + * @brief Change color style to red when PID values of Roll Control are edited + * + * + * @param + */ +void SlugsPIDControl::changeColor_RED_RollControl_groupBox(QString text) +{ + Q_UNUSED(text); + ui->RollControl_groupBox->setStyleSheet(REDcolorStyle); +} + +/** + * @brief Change color style to green when PID values of Roll Control are send to UAS + * + * @param + */ +void SlugsPIDControl::changeColor_GREEN_RollControl_groupBox() +{ + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + #ifdef MAVLINK_ENABLED_SLUGS + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 4; + pidMessage.pVal = ui->dA_P_set->text().toFloat(); + pidMessage.iVal = ui->dA_I_set->text().toFloat(); + pidMessage.dVal = ui->dA_D_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + + ui->RollControl_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +/** + * @brief Connects the SIGNALS from the editline to SLOT RollControl_groupBox + * + * @param + */ +void SlugsPIDControl::connect_RollControl_LineEdit() +{ + connect(ui->dA_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString))); + connect(ui->dA_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString))); + connect(ui->dA_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString))); +} + +void SlugsPIDControl::get_RollControl_PID() +{ + qDebug() << "\nSend Message = Roll Control "; + sendMessagePIDStatus(4); +} + + + +// Functions for Heigth Error GroupBox +/** + * @brief Change color style to red when PID values of Heigth Error are edited + * + * + * @param + */ +void SlugsPIDControl::changeColor_RED_HeigthError_groupBox(QString text) +{ + Q_UNUSED(text); + ui->HeightErrorLoPitch_groupBox->setStyleSheet(REDcolorStyle); +} + +/** + * @brief Change color style to green when PID values of Heigth Error are send to UAS + * + * @param + */ +void SlugsPIDControl::changeColor_GREEN_HeigthError_groupBox() +{ + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + #ifdef MAVLINK_ENABLED_SLUGS + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 1; + pidMessage.pVal = ui->HELPComm_P_set->text().toFloat(); + pidMessage.iVal = ui->HELPComm_I_set->text().toFloat(); + pidMessage.dVal = ui->HELPComm_FF_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + + ui->HeightErrorLoPitch_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +/** + * @brief Connects the SIGNALS from the editline to SLOT HeigthError_groupBox + * + * @param + */ +void SlugsPIDControl::connect_HeigthError_LineEdit() +{ + connect(ui->HELPComm_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString))); + connect(ui->HELPComm_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString))); + connect(ui->HELPComm_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString))); +} + +void SlugsPIDControl::get_HeigthError_PID() +{ + qDebug() << "\nSend Message = Heigth Error "; + sendMessagePIDStatus(1); +} + + +// Functions for Yaw Damper GroupBox +/** + * @brief Change color style to red when PID values of Yaw Damper are edited + * + * + * @param + */ +void SlugsPIDControl::changeColor_RED_YawDamper_groupBox(QString text) +{ + Q_UNUSED(text); + ui->YawDamper_groupBox->setStyleSheet(REDcolorStyle); +} + +/** + * @brief Change color style to green when PID values of Yaw Damper are send to UAS + * + * @param + */ +void SlugsPIDControl::changeColor_GREEN_YawDamper_groupBox() +{ + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + #ifdef MAVLINK_ENABLED_SLUGS + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 3; + pidMessage.pVal = ui->dR_P_set->text().toFloat(); + pidMessage.iVal = ui->dR_I_set->text().toFloat(); + pidMessage.dVal = ui->dR_D_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + + ui->YawDamper_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +/** + * @brief Connects the SIGNALS from the editline to SLOT YawDamper_groupBox + * + * @param + */ +void SlugsPIDControl::connect_YawDamper_LineEdit() +{ + connect(ui->dR_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString))); + connect(ui->dR_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString))); + connect(ui->dR_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString))); + + +} + +void SlugsPIDControl::get_YawDamper_PID() +{ + qDebug() << "\nSend Message = Yaw Damper "; + sendMessagePIDStatus(3); +} + + +// Functions for Pitch to dT GroupBox +/** + * @brief Change color style to red when PID values of Pitch to dT are edited + * + * + * @param + */ +void SlugsPIDControl::changeColor_RED_Pitch2dT_groupBox(QString text) +{ + Q_UNUSED(text); + ui->Pitch2dTFFterm_groupBox->setStyleSheet(REDcolorStyle); +} + +/** + * @brief Change color style to green when PID values of Pitch to dT are send to UAS + * + * @param + */ +void SlugsPIDControl::changeColor_GREEN_Pitch2dT_groupBox() +{ + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { +#ifdef MAVLINK_ENABLED_SLUGS + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 8; + pidMessage.pVal = ui->dR_P_set->text().toFloat(); + pidMessage.iVal = ui->dR_I_set->text().toFloat(); + pidMessage.dVal = ui->dR_D_set->text().toFloat(); + + mavlink_message_t msg; + + mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); + slugsMav->sendMessage(msg); +#endif + ui->Pitch2dTFFterm_groupBox->setStyleSheet(GREENcolorStyle); + } +} + +/** + * @brief Connects the SIGNALS from the editline to SLOT Pitch2dT_groupBox + * + * @param + */ +void SlugsPIDControl::connect_Pitch2dT_LineEdit() +{ + connect(ui->P2dT_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_Pitch2dT_groupBox(QString))); +} + +void SlugsPIDControl::get_Pitch2dT_PID() +{ + qDebug() << "\nSend Message = Pitch to dT "; + sendMessagePIDStatus(8); +} + +#ifdef MAVLINK_ENABLED_SLUGS + +void SlugsPIDControl::recibeMensaje(int systemId, const mavlink_action_ack_t& action) +{ + ui->recepcion_label->setText(QString::number(action.action) + ":" + QString::number(action.result)); +} + +void SlugsPIDControl::receivePidValues(int systemId, const mavlink_pid_t &pidValues) +{ + Q_UNUSED(systemId); + + qDebug() << "\nACTUALIZANDO GUI = " << pidValues.idx; + switch(pidValues.idx) + { + case 0: + ui->dT_P_get->setText(QString::number(pidValues.pVal)); + ui->dT_I_get->setText(QString::number(pidValues.iVal)); + ui->dT_D_get->setText(QString::number(pidValues.dVal)); + break; + case 1: + ui->HELPComm_P_get->setText(QString::number(pidValues.pVal)); + ui->HELPComm_I_get->setText(QString::number(pidValues.iVal)); + ui->HELPComm_FF_get->setText(QString::number(pidValues.dVal)); + break; + case 2: + ui->dE_P_get->setText(QString::number(pidValues.pVal)); + ui->dE_I_get->setText(QString::number(pidValues.iVal)); + ui->dE_D_get->setText(QString::number(pidValues.dVal)); + break; + case 3: + ui->dR_P_get->setText(QString::number(pidValues.pVal)); + ui->dR_I_get->setText(QString::number(pidValues.iVal)); + ui->dR_D_get->setText(QString::number(pidValues.dVal)); + break; + case 4: + ui->dA_P_get->setText(QString::number(pidValues.pVal)); + ui->dA_I_get->setText(QString::number(pidValues.iVal)); + ui->dA_D_get->setText(QString::number(pidValues.dVal)); + break; + case 8: + ui->P2dT_FF_get->setText(QString::number(pidValues.pVal)); + break; + + default: + qDebug() << "\nSLUGS RECEIVED AND SHOW PID type ID = " << pidValues.idx; + break; + + } +} +#endif // MAVLINK_ENABLED_SLUG + + +void SlugsPIDControl::sendMessagePIDStatus(int PIDtype) +{ +#ifdef MAVLINK_ENABLED_SLUGS + //ToDo remplace actionId values + + + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + mavlink_message_t msg; + qDebug() << "\n Send Message SLUGS PID with loop index = " << PIDtype; + + switch(PIDtype) + { + case 0: //Air Speed PID values Request + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 0; + + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + case 1: //Heigth Error lo Pitch Comm PID values request + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 1; + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + case 2://Pitch Followei PID values Request + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 2; + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + case 3:// Yaw Damper PID values request + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 3; + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + case 4: // Roll Control PID values request + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 4; + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + case 8: // Pitch to dT FF term + actionSlugs.target = activeUAS->getUASID(); + actionSlugs.actionId = 9; + actionSlugs.actionVal = 8; + + + mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs); + slugsMav->sendMessage(msg); + break; + + + default: + qDebug() << "\nSLUGS RECEIVED PID type ID = " << PIDtype; + break; + + + } + } + #endif // MAVLINK_ENABLED_SLUG +} + +void SlugsPIDControl::slugsGetGeneral() +{ + valuesMutex.lock(); + switch(counterRefreshGet) + { + case 1: + ui->dT_PID_get_pushButton->click(); + break; + case 2: + ui->HELPComm_PDI_get_pushButton->click(); + break; + case 3: + ui->dE_PID_get_pushButton->click(); + break; + case 4: + ui->dR_PDI_get_pushButton->click(); + break; + case 5: + ui->dA_PID_get_pushButton->click(); + break; + case 6: + ui->Pitch2dT_PDI_get_pushButton->click(); + break; + default: + refreshTimerGet->stop(); + break; + + + } + + counterRefreshGet++; + valuesMutex.unlock(); + +} + +void SlugsPIDControl::slugsSetGeneral() +{ + valuesMutex.lock(); + switch(counterRefreshSet) + { + case 1: + ui->dT_PID_set_pushButton->click(); + break; + case 2: + ui->HELPComm_PDI_set_pushButton->click(); + break; + case 3: + ui->dE_PID_set_pushButton->click(); + break; + case 4: + ui->dR_PDI_set_pushButton->click(); + break; + case 5: + ui->dA_PID_set_pushButton->click(); + break; + case 6: + ui->Pitch2dT_PDI_set_pushButton->click(); + break; + default: + refreshTimerSet->stop(); + break; + + } + + counterRefreshSet++; + valuesMutex.unlock(); +} + + +void SlugsPIDControl::slugsTimerStartSet() +{ + counterRefreshSet = 1; + refreshTimerSet->start(); + +} + +void SlugsPIDControl::slugsTimerStartGet() +{ + counterRefreshGet = 1; + refreshTimerGet->start(); + +} +void SlugsPIDControl::slugsTimerStop() +{ +// refreshTimerGet->stop(); +// counterRefresh = 1; + +} diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h new file mode 100644 index 0000000000000000000000000000000000000000..051dc3f22278cd97a27363e268dfd29eaec70412 --- /dev/null +++ b/src/ui/SlugsPIDControl.h @@ -0,0 +1,292 @@ +#ifndef SLUGSPIDCONTROL_H +#define SLUGSPIDCONTROL_H + +#include +#include +#include "UASInterface.h" +#include "QGCMAVLink.h" +#include "SlugsMAV.h" +#include "mavlink.h" +#include +#include + +namespace Ui { + class SlugsPIDControl; +} + +class SlugsPIDControl : public QWidget +{ + Q_OBJECT + +public: + explicit SlugsPIDControl(QWidget *parent = 0); + ~SlugsPIDControl(); + +public slots: + + /** + * @brief Called when the a new UAS is set to active. + * + * Called when the a new UAS is set to active. + * + * @param uas The new active UAS + */ + void activeUasSet(UASInterface* uas); + + /** + */ + void setRedColorStyle(); + /** + * @brief Set color StyleSheet GREEN + * + * @param + */ + void setGreenColorStyle(); + + /** + * @brief Connect Set pushButtons to change the color GroupBox + * + * @param + */ + void connect_set_pushButtons(); + + /** + * @brief Connect Set pushButtons to change the color GroupBox + * + * @param + */ + void connect_get_pushButtons(); + + /** + * @brief Connect Edition Lines for PID Values + * + * @param + */ + void connect_editLinesPDIvalues(); + + /** + * @brief send a PDI request message to UAS + * + * @param + */ + void sendMessagePIDStatus(int PIDtype); + +// Fuctions for Air Speed GroupBox + /** + * @brief Change color style to red when PID values of Air Speed are edited + * + * + * @param + */ + void changeColor_RED_AirSpeed_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Air Speed are send to UAS + * + * @param + */ + void changeColor_GREEN_AirSpeed_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT changeColor_RED_AirSpeed_groupBox() + * + * @param + */ + void connect_AirSpeed_LineEdit(); + /** + * @brief get message PID Air Speed(loop index = 0) from UAS + * + * @param + */ + void get_AirSpeed_PID(); + + +// Functions for Pitch Followei GroupBox + /** + * @brief Change color style to red when PID values of Pitch Followei are edited + * + * + * @param + */ + void changeColor_RED_PitchFollowei_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Pitch Followei are send to UAS + * + * @param + */ + void changeColor_GREEN_PitchFollowei_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT PitchFlowei_groupBox + * + * @param + */ + void connect_PitchFollowei_LineEdit(); + /** + * @brief get message PID Pitch Followei(loop index = 2) from UAS + * + * @param + */ + void get_PitchFollowei_PID(); + + + // Functions for Roll Control GroupBox + /** + * @brief Change color style to red when PID values of Roll Control are edited + * + * + * @param + */ + void changeColor_RED_RollControl_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Roll Control are send to UAS + * + * @param + */ + void changeColor_GREEN_RollControl_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT RollControl_groupBox + * + * @param + */ + void connect_RollControl_LineEdit(); + /** + * @brief get message PID Roll Control(loop index = 4) from UAS + * + * @param + */ + void get_RollControl_PID(); + + + // Functions for Heigth Error GroupBox + /** + * @brief Change color style to red when PID values of Heigth Error are edited + * + * + * @param + */ + void changeColor_RED_HeigthError_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Heigth Error are send to UAS + * + * @param + */ + void changeColor_GREEN_HeigthError_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT HeigthError_groupBox + * + * @param + */ + void connect_HeigthError_LineEdit(); + /** + * @brief get message PID Heigth Error(loop index = 1) from UAS + * + * @param + */ + void get_HeigthError_PID(); + + // Functions for Yaw Damper GroupBox + /** + * @brief Change color style to red when PID values of Yaw Damper are edited + * + * + * @param + */ + void changeColor_RED_YawDamper_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Yaw Damper are send to UAS + * + * @param + */ + void changeColor_GREEN_YawDamper_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT YawDamper_groupBox + * + * @param + */ + void connect_YawDamper_LineEdit(); + /** + * @brief get message PID Yaw Damper(loop index = 3) from UAS + * + * @param + */ + void get_YawDamper_PID(); + + + + // Functions for Pitch to dT GroupBox + /** + * @brief Change color style to red when PID values of Pitch to dT are edited + * + * + * @param + */ + void changeColor_RED_Pitch2dT_groupBox(QString text); + /** + * @brief Change color style to green when PID values of Pitch to dT are send to UAS + * + * @param + */ + void changeColor_GREEN_Pitch2dT_groupBox(); + /** + * @brief Connects the SIGNALS from the editline to SLOT Pitch2dT_groupBox + * + * @param + */ + void connect_Pitch2dT_LineEdit(); + /** + * @brief get message PID Pitch2dT(loop index = 8) from UAS + * + * @param + */ + void get_Pitch2dT_PID(); + + /** + * @brief get and updates the values on widget + */ + void slugsGetGeneral(); + /** + * @brief Sent all values to UAS + */ + void slugsSetGeneral(); + + void slugsTimerStartSet(); + void slugsTimerStartGet(); + void slugsTimerStop(); + + + + //Create, send and get Messages PID + // void createMessagePID(); +#ifdef MAVLINK_ENABLED_SLUGS + + void recibeMensaje(int systemId, const mavlink_action_ack_t& action); + void receivePidValues(int systemId, const mavlink_pid_t& pidValues); + +#endif // MAVLINK_ENABLED_SLUG + +private: + Ui::SlugsPIDControl *ui; + + UASInterface* activeUAS; + int systemId; + + bool change_dT; + + + //Color Styles + QString REDcolorStyle; + QString GREENcolorStyle; + QString ORIGINcolorStyle; + + //SlugsMav Message + #ifdef MAVLINK_ENABLED_SLUGS + mavlink_pid_t pidMessage; + mavlink_slugs_action_t actionSlugs; +#endif + + QTimer* refreshTimerSet; ///< The main timer, controls the update view + QTimer* refreshTimerGet; ///< The main timer, controls the update view + int counterRefreshSet; + int counterRefreshGet; + QMutex valuesMutex; +}; + +#endif // SLUGSPIDCONTROL_H diff --git a/src/ui/SlugsPIDControl.ui b/src/ui/SlugsPIDControl.ui new file mode 100644 index 0000000000000000000000000000000000000000..042f3d92c3fe31622011b0295bb50fa622ff0a7b --- /dev/null +++ b/src/ui/SlugsPIDControl.ui @@ -0,0 +1,1061 @@ + + + SlugsPIDControl + + + + 0 + 0 + 429 + 579 + + + + Form + + + + + + + + + + Air Speed Hold (dT) + + + + + + + + + 10 + 75 + true + + + + P + + + + + + + + 10 + 75 + true + + + + I + + + + + + + + 10 + 75 + true + + + + D + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Recepcion + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Pitch Followei (dE) + + + + + + + + + 10 + 75 + true + + + + P + + + + + + + + 10 + 75 + true + + + + I + + + + + + + + 10 + 75 + true + + + + D + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Roll Control (dA) + + + + + + + + + 10 + 75 + true + + + + P + + + + + + + + 10 + 75 + true + + + + I + + + + + + + + 10 + 75 + true + + + + D + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + + + Heigth Error lo Pitch Comm + + + + + + + + + 10 + 75 + true + + + + P + + + + + + + + 10 + 75 + true + + + + I + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + 10 + 75 + true + + + + QFrame::Sunken + + + 5 + + + 1 + + + Qt::Horizontal + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + + 10 + 75 + true + + + + FF + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Yaw Damper (dR) + + + + + + + + + 10 + 75 + true + + + + P + + + + + + + + 10 + 75 + true + + + + I + + + + + + + + 10 + 75 + true + + + + D + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Pitch to dT FF term + + + + + + + + + 10 + 75 + true + + + + FF + + + + + + + 0.0 + + + + + + + 0.0 + + + true + + + + + + + + + + + SET + + + + + + + GET + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Set General + + + + + + + Get General + + + + + + + verticalSpacer_13 + + + dT_P_set + dT_I_set + dT_D_set + dT_PID_set_pushButton + dT_PID_get_pushButton + HELPComm_P_set + HELPComm_I_set + HELPComm_FF_set + HELPComm_PDI_set_pushButton + HELPComm_PDI_get_pushButton + dE_P_set + dE_I_set + dE_D_set + dE_PID_set_pushButton + dE_PID_get_pushButton + dR_P_set + dR_I_set + dR_D_set + dR_PDI_set_pushButton + dR_PDI_get_pushButton + dA_P_set + dA_I_set + dA_D_set + dA_PID_set_pushButton + dA_PID_get_pushButton + P2dT_FF_set + Pitch2dT_PDI_set_pushButton + Pitch2dT_PDI_get_pushButton + dT_P_get + dT_I_get + dT_D_get + HELPComm_P_get + HELPComm_I_get + HELPComm_FF_get + dE_P_get + dE_I_get + dE_D_get + dR_P_get + dR_I_get + dR_D_get + dA_P_get + dA_I_get + dA_D_get + P2dT_FF_get + + + + diff --git a/src/ui/SlugsPadCameraControl.cpp b/src/ui/SlugsPadCameraControl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..40b40c072ec83feb6c385387f04c696cccac969e --- /dev/null +++ b/src/ui/SlugsPadCameraControl.cpp @@ -0,0 +1,231 @@ +#include "SlugsPadCameraControl.h" +#include "ui_SlugsPadCameraControl.h" +#include +#include +#include +#include + +SlugsPadCameraControl::SlugsPadCameraControl(QWidget *parent) : + QWidget(parent), //QGraphicsView(parent), + ui(new Ui::SlugsPadCameraControl), + dragging(0) +{ + ui->setupUi(this); + x1= 0; + y1 = 0; + +} + +SlugsPadCameraControl::~SlugsPadCameraControl() +{ + delete ui; +} + +void SlugsPadCameraControl::mouseMoveEvent(QMouseEvent *event) +{ + emit mouseMoveCoord(event->x(),event->y()); + +} + +void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) +{ + emit mousePressCoord(event->x(),event->y()); + x1 = event->x(); + y1 = event->y(); + +} + +void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) +{ + emit mouseReleaseCoord(event->x(),event->y()); + getDeltaPositionPad(event->x(), event->y()); + +} + +void SlugsPadCameraControl::paintEvent(QPaintEvent *pe) +{ + Q_UNUSED(pe); + QPainter painter(this); + painter.setPen(Qt::blue); + painter.setFont(QFont("Arial", 30)); + +// QRectF rectangle(tL.x(), tL.y(), ui->padCamContro_frame->width(), ui->padCamContro_frame->height()); +// int startAngle = 30 * 16; +// int spanAngle = 120 * 16; + + painter.drawLine(QPoint(ui->frame->width()/2,ui->frame->geometry().topLeft().y()), + QPoint(ui->frame->width()/2,ui->frame->geometry().bottomRight().y())); + + painter.drawLine(QPoint(ui->frame->geometry().topLeft().x(),ui->frame->height()/2), + QPoint(ui->frame->geometry().bottomRight().x(),ui->frame->height()/2)); + + + // painter.drawLine(QPoint()); + //painter.drawLines(padLines); + + + // painter.drawPie(rectangle, startAngle, spanAngle); + + //painter.drawText(rect(), Qt::AlignCenter, "Qt"); +} + +void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) +{ + + QString dir = "nd"; + QPointF localMeasures = ObtenerMarcacionDistanciaPixel(y1,x1,y2,x2); + + double bearing = localMeasures.x(); + double dist = getDistPixel(y1,x1,y2,x2); + + + + if(((bearing > 330)&&(bearing < 360)) || ((bearing >= 0)&&(bearing <= 30))) + { + emit dirCursorText("up"); + //bearing = 315; + dir = "up"; + } + else + { + if((bearing > 30)&&(bearing <= 60) ) + { + emit dirCursorText("right up"); + //bearing = 315; + dir = "riht up"; + } + else + { + if((bearing > 60)&&(bearing <= 105) ) + { + emit dirCursorText("right"); + //bearing = 315; + dir = "riht"; + } + else + { + if((bearing > 105)&&(bearing <= 150) ) + { + emit dirCursorText("right down"); + //bearing = 315; + dir = "riht down"; + } + else + { + if((bearing > 150)&&(bearing <= 195) ) + { + emit dirCursorText("down"); + //bearing = 315; + dir = "down"; + } + else + { + if((bearing > 195)&&(bearing <= 240) ) + { + emit dirCursorText("left down"); + //bearing = 315; + dir = "left down"; + } + else + { + if((bearing > 240)&&(bearing <= 300) ) + { + emit dirCursorText("left"); + //bearing = 315; + dir = "left"; + } + else + { + if((bearing > 300)&&(bearing <= 330) ) + { + emit dirCursorText("left up"); + //bearing = 315; + dir = "left up"; + } + + } + + } + + } + + } + + } + + } + + } + + + emit changeCursorPosition(bearing, dist, dir); + +} + +double SlugsPadCameraControl::getDistPixel(int x1, int y1, int x2, int y2) +{ + double cateto_opuesto,cateto_adyacente; + //latitud y longitud del primer punto + + + cateto_opuesto = abs((x1-x2)); //diferencia de latitudes entre PCR1 y PCR2 + cateto_adyacente = abs((y1-y2));//diferencia de longitudes entre PCR1 y PCR2 + + return sqrt(pow(cateto_opuesto,2) + pow(cateto_adyacente,2)); + + // distancia = (float) hipotenusa; +} + +/** + * Esta función xxxxxxxxx + * @param double lat1 --> + * @param double lon1 --> + * @param double lat2 --> + * @param double lon2 --> + * @param ref double rumbo --> + * @param ref double distancia --> + */ +QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2) +{ + double cateto_opuesto,cateto_adyacente, hipotenusa, distancia, marcacion; + + //latitude and longitude first point + + if(lat1<0) lat1= lat1*(-1); + if(lat2<0) lat2= lat2*(-1); + if(lon1<0) lon1= lon1*(-1); + if(lon2<0) lon1= lon1*(-1); + + cateto_opuesto = abs((lat1-lat2)); + cateto_adyacente = abs((lon1-lon2)); + + hipotenusa = sqrt(pow(cateto_opuesto,2) + pow(cateto_adyacente,2)); + distancia = hipotenusa*60; + + + if ((lat1 < lat2) && (lon1 > lon2)) //primer cuadrante + marcacion = 360 -((asin(cateto_adyacente/hipotenusa))/ 0.017453292); + else if ((lat1 < lat2) && (lon1 < lon2)) //segundo cuadrante + marcacion = (asin(cateto_adyacente/hipotenusa))/ 0.017453292; + else if((lat1 > lat2) && (lon1 < lon2)) //tercer cuadrante + marcacion = 180 -((asin(cateto_adyacente/hipotenusa))/ 0.017453292); + else if((lat1 > lat2) && (lon1 > lon2)) //cuarto cuadrante + marcacion = 180 +((asin(cateto_adyacente/hipotenusa))/ 0.017453292); + else if((lat1 < lat2) && (lon1 == lon2)) //360 + marcacion = 360; + else if((lat1 == lat2) && (lon1 > lon2)) //270 + marcacion = 270; + else if((lat1 > lat2) && (lon1 == lon2)) //180 + marcacion = 180; + else if((lat1 == lat2) && (lon1 < lon2)) //90 + marcacion =90; + else if((lat1 == lat2) && (lon1 == lon2)) //0 + marcacion = 0.0; + + // this only convert real bearing to frame widget bearing + marcacion = marcacion +90; + if(marcacion>= 360) marcacion = marcacion - 360; + + return QPointF(marcacion,distancia); + +} diff --git a/src/ui/SlugsPadCameraControl.h b/src/ui/SlugsPadCameraControl.h new file mode 100644 index 0000000000000000000000000000000000000000..06a923328c5c12042a81d197072254fec56df9c4 --- /dev/null +++ b/src/ui/SlugsPadCameraControl.h @@ -0,0 +1,47 @@ +#ifndef SLUGSPADCAMERACONTROL_H +#define SLUGSPADCAMERACONTROL_H + +#include +#include + +namespace Ui { + class SlugsPadCameraControl; +} + +class SlugsPadCameraControl : public QWidget //QGraphicsView// +{ + Q_OBJECT + +public: + explicit SlugsPadCameraControl(QWidget *parent = 0); + + ~SlugsPadCameraControl(); + +public slots: + void getDeltaPositionPad(int x, int y); + double getDistPixel(int x1, int y1, int x2, int y2); + QPointF ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2); + +signals: + void mouseMoveCoord(int x, int y); + void mousePressCoord(int x, int y); + void mouseReleaseCoord(int x, int y); + void dirCursorText(QString dir); + void distance_Bearing(double dist, double bearing); + void changeCursorPosition(double bearing, double distance, QString textDir); + +protected: + void mousePressEvent(QMouseEvent* event); + void mouseReleaseEvent(QMouseEvent* event); + void mouseMoveEvent(QMouseEvent* event); + void paintEvent(QPaintEvent *pe); + +private: + Ui::SlugsPadCameraControl *ui; + bool dragging; + int x1; + int y1; + +}; + +#endif // SLUGSPADCAMERACONTROL_H diff --git a/src/ui/SlugsPadCameraControl.ui b/src/ui/SlugsPadCameraControl.ui new file mode 100644 index 0000000000000000000000000000000000000000..f96e02a006d094f0e8a0fcab16ebfe8506c07bb8 --- /dev/null +++ b/src/ui/SlugsPadCameraControl.ui @@ -0,0 +1,52 @@ + + + SlugsPadCameraControl + + + + 0 + 0 + 183 + 127 + + + + Form + + + background-color: rgb(255, 170, 0); + + + + 1 + + + 1 + + + + + + 181 + 125 + + + + true + + + background-color: rgba(255, 0, 0,50%); + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + diff --git a/src/ui/SlugsVideoCamControl.cpp b/src/ui/SlugsVideoCamControl.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1eb0b5264e29a990e0e11e6a56933242f4ae0f46 --- /dev/null +++ b/src/ui/SlugsVideoCamControl.cpp @@ -0,0 +1,134 @@ +#include "SlugsVideoCamControl.h" +#include "ui_SlugsVideoCamControl.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SlugsPadCameraControl.h" + + +SlugsVideoCamControl::SlugsVideoCamControl(QWidget *parent) : + QWidget(parent), + ui(new Ui::SlugsVideoCamControl) + //dragging(0) + +{ + ui->setupUi(this); +// x1= 0; +// y1 = 0; + connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); +// tL = ui->padCamContro_frame->frameGeometry().topLeft(); +// bR = ui->padCamContro_frame->frameGeometry().bottomRight(); + //ui->padCamContro_frame->setVisible(true); + +// // create a layout for camera pad +// QGridLayout* padCameraLayout = new QGridLayout(this); +// padCameraLayout->setSpacing(2); +// padCameraLayout->setMargin(0); +// padCameraLayout->setAlignment(Qt::AlignTop); + + //ui->padCamContro_frame->setLayout(padCameraLayout); + // create a camera pad widget + //test = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); + //test->setMaximumWidth(50); + //ui->gridLayout->addWidget(test, 0,0); + + padCamera = new SlugsPadCameraControl(this); + + ui->gridLayout->addWidget(padCamera); + + connect(padCamera,SIGNAL(mouseMoveCoord(int,int)),this,SLOT(mousePadMoveEvent(int,int))); + connect(padCamera,SIGNAL(mousePressCoord(int,int)),this,SLOT(mousePadPressEvent(int,int))); + connect(padCamera,SIGNAL(mouseReleaseCoord(int,int)),this,SLOT(mousePadReleaseEvent(int,int))); + connect(padCamera,SIGNAL(changeCursorPosition(double,double,QString)),this,SLOT(getDeltaPositionPad(double,double,QString))); + + + + //padCamera->setVisible(true); + + + + // padCameraLayout->addWidget(padCamera); + + + +// QGraphicsScene *scene = new QGraphicsScene(ui->CamControlPanel_graphicsView); +// scene->setItemIndexMethod(QGraphicsScene::NoIndex); +// scene->setSceneRect(-200, -200, 400, 400); +// setScene(scene); +// setCacheMode(CacheBackground); +// setViewportUpdateMode(BoundingRectViewportUpdate); +// setRenderHint(QPainter::Antialiasing); +// setTransformationAnchor(AnchorUnderMouse); +// setResizeAnchor(AnchorViewCenter); + +// ui->CamControlPanel_graphicsView->installEventFilter(this); +// ui->label_x->installEventFilter(this); + +} + +SlugsVideoCamControl::~SlugsVideoCamControl() +{ + delete ui; +} + +void SlugsVideoCamControl::mouseMoveEvent(QMouseEvent *event) +{ + Q_UNUSED(event); + +} + + +void SlugsVideoCamControl::mousePressEvent(QMouseEvent *evnt) +{ + Q_UNUSED(evnt); + +} + +void SlugsVideoCamControl::mouseReleaseEvent(QMouseEvent *evnt) +{ + Q_UNUSED(evnt); + +} + + +void SlugsVideoCamControl::mousePadMoveEvent(int x, int y) +{ + +} + +void SlugsVideoCamControl::mousePadPressEvent(int x, int y) +{ + +} + +void SlugsVideoCamControl::mousePadReleaseEvent(int x, int y) +{ + + +} + +void SlugsVideoCamControl::changeViewCamBorderAtMapStatus(bool status) +{ + emit viewCamBorderAtMap(status); +} + +void SlugsVideoCamControl::getDeltaPositionPad(double bearing, double distance, QString dirText) +{ + ui->label_dir->setText(dirText); + ui->label_x->setText("Distancia= " + QString::number(distance)); + ui->label_y->setText("Bearing= " + QString::number(bearing)); + + emit changeCamPosition(20, bearing, dirText); +} + diff --git a/src/ui/SlugsVideoCamControl.h b/src/ui/SlugsVideoCamControl.h new file mode 100644 index 0000000000000000000000000000000000000000..2580033a5e1dee3883ea60008707112b6f11f674 --- /dev/null +++ b/src/ui/SlugsVideoCamControl.h @@ -0,0 +1,57 @@ +#ifndef SLUGSVIDEOCAMCONTROL_H +#define SLUGSVIDEOCAMCONTROL_H + +#include +#include +#include +#include +#include +#include "SlugsPadCameraControl.h" +#include + + + +#define DELTA 1000 + +namespace Ui { + class SlugsVideoCamControl; +} + +class SlugsVideoCamControl : public QWidget + +{ + Q_OBJECT + +public: + explicit SlugsVideoCamControl(QWidget *parent = 0); + ~SlugsVideoCamControl(); + +public slots: + void changeViewCamBorderAtMapStatus(bool status); + void getDeltaPositionPad(double dir, double dist, QString dirText); + + + void mousePadPressEvent(int x, int y); + void mousePadReleaseEvent(int x, int y); + void mousePadMoveEvent(int x, int y); + +signals: + void changeCamPosition(double dist, double dir, QString textDir); + void viewCamBorderAtMap(bool status); + +protected: + void mousePressEvent(QMouseEvent* event); + void mouseReleaseEvent(QMouseEvent* event); + void mouseMoveEvent(QMouseEvent* event); + + + + +private: + Ui::SlugsVideoCamControl *ui; + + SlugsPadCameraControl* padCamera; + +}; + +#endif // SLUGSVIDEOCAMCONTROL_H diff --git a/src/ui/SlugsVideoCamControl.ui b/src/ui/SlugsVideoCamControl.ui new file mode 100644 index 0000000000000000000000000000000000000000..c89c2e7ef26e6c05ecb9e103c616c02506d1934b --- /dev/null +++ b/src/ui/SlugsVideoCamControl.ui @@ -0,0 +1,72 @@ + + + SlugsVideoCamControl + + + + 0 + 0 + 165 + 191 + + + + + 0 + 0 + + + + true + + + Form + + + + + + + + true + + + Coord_X + + + + + + + true + + + Coord_Y + + + + + + + Camera Pos + + + + + + + + + + + Camera at Map + + + + + + + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 2eff2aad87c3b4c152ed92cddb2a779c0bc389a0..3b5b96786435d4abe87c9bad83df39e91ed820b0 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -88,6 +88,8 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : updateStatusLabel(""); this->setVisible(false); + loadFileGlobalWP = false; + readGlobalWP = false; centerMapCoordinate.setX(0.0); centerMapCoordinate.setY(0.0); @@ -128,6 +130,9 @@ 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())); + connect(&uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); + } } @@ -144,6 +149,8 @@ void WaypointList::loadWaypoints() { if (uas) { + + QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); uas->getWaypointManager().loadWaypoints(fileName); } @@ -242,6 +249,8 @@ void WaypointList::addCurrentPositonWaypoint() void WaypointList::updateStatusLabel(const QString &string) { m_ui->statusLabel->setText(string); + + } void WaypointList::changeCurrentWaypoint(quint16 seq) @@ -388,7 +397,7 @@ void WaypointList::waypointListChanged() } - + loadFileGlobalWP = false; } @@ -462,15 +471,23 @@ void WaypointList::changeEvent(QEvent *e) void WaypointList::on_clearWPListButton_clicked() { + if (uas) { - emit clearPathclicked(); - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[0]).value(); - widget->remove(); + emit clearPathclicked(); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[0]).value(); + widget->remove(); + } } + else + { +// if(isGlobalWP) +// { +// emit clearPathclicked(); +// } } } @@ -534,14 +551,24 @@ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) void WaypointList::clearWPWidget() { - if (uas) - { - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[0]).value(); - widget->remove(); - } - } + if (uas) + { + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[0]).value(); + widget->remove(); + } + } //emit changePositionWPBySpinBox(wp->getId(), wp->getY(), wp->getX()); } + +void WaypointList::setIsLoadFileWP() +{ + loadFileGlobalWP = true; +} + +void WaypointList::setIsReadGlobalWP(bool value) +{ + // readGlobalWP = value; +} diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index ab5d760484d416cdb21b025a1ec00e3a616ed4b4..cb42b70db39eb79ff40c32d2edcd666153f8bf28 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,9 @@ public slots: void moveDown(Waypoint* wp); void removeWaypoint(Waypoint* wp); + void setIsLoadFileWP(); + void setIsReadGlobalWP(bool value); + @@ -126,6 +129,8 @@ protected: double mavZ; double mavYaw; QPointF centerMapCoordinate; + bool loadFileGlobalWP; + bool readGlobalWP; private: Ui::WaypointList *m_ui; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 004242451f1d7bdaa369adcf1225b4967007e926..2cfaf83ad5fd35daa9bba71d17a5000e86ee8675 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -54,7 +54,9 @@ void Linecharts::addSystem(UASInterface* uas) LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this); addWidget(widget); plots.insert(uas->getUASID(), widget); +#ifndef MAVLINK_ENABLED_SLUGS connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); +#endif connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system if (active) diff --git a/src/ui/slugshilsim.cc b/src/ui/slugshilsim.cc new file mode 100644 index 0000000000000000000000000000000000000000..03911c370910175bb0a8fb0467a2b5dc0bd4e22f --- /dev/null +++ b/src/ui/slugshilsim.cc @@ -0,0 +1,208 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Configuration Window for Slugs' HIL Simulator + * @author Mariano Lizarraga + */ + + +#include "slugshilsim.h" +#include "ui_slugshilsim.h" +#include "LinkManager.h" + +SlugsHilSim::SlugsHilSim(QWidget *parent) : + QWidget(parent), + ui(new Ui::SlugsHilSim) +{ + ui->setupUi(this); + + rxSocket = new QUdpSocket(this); + txSocket = new QUdpSocket(this); + + hilLink = NULL; + + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*))); + connect(ui->cb_mavlinkLinks, SIGNAL(currentIndexChanged(int)), this, SLOT(linkSelected(int))); + connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode())); + connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram())); + + linksAvailable.clear(); +} + +SlugsHilSim::~SlugsHilSim() +{ + rxSocket->disconnectFromHost(); + delete ui; +} + +void SlugsHilSim::addToCombo(LinkInterface* theLink){ + + ui->cb_mavlinkLinks->addItem(theLink->getName()); + linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink); + + if (hilLink == NULL){ + hilLink = theLink; + } + +} + +void SlugsHilSim::putInHilMode(void){ + + bool sw_enableControls = !(ui->bt_startHil->isChecked()); + QString buttonCaption= ui->bt_startHil->isChecked()? "Stop Slugs HIL Mode": "Set Slugs in HIL Mode"; + + if (ui->bt_startHil->isChecked()){ + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("You are about to put SLUGS in HIL Mode."); + msgBox.setInformativeText("It will stop reading the actual sensor readings. Do you wish to continue?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); + msgBox.setDefaultButton(QMessageBox::No); + + if(msgBox.exec() == QMessageBox::Yes) { + rxSocket->disconnectFromHost(); + rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt()); + //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt()); + + ui->ed_ipAdress->setEnabled(sw_enableControls); + ui->ed_rxPort->setEnabled(sw_enableControls); + ui->ed_txPort->setEnabled(sw_enableControls); + ui->cb_mavlinkLinks->setEnabled(sw_enableControls); + + ui->bt_startHil->setText(buttonCaption); + + } else { + ui->bt_startHil->setChecked(false); + } + } else { + ui->ed_ipAdress->setEnabled(sw_enableControls); + ui->ed_rxPort->setEnabled(sw_enableControls); + ui->ed_txPort->setEnabled(sw_enableControls); + ui->cb_mavlinkLinks->setEnabled(sw_enableControls); + + ui->bt_startHil->setText(buttonCaption); + + rxSocket->disconnectFromHost(); + } +} + +void SlugsHilSim::readDatagram(void){ + static int count = 0; + while (rxSocket->hasPendingDatagrams()) { + QByteArray datagram; + datagram.resize(rxSocket->pendingDatagramSize()); + QHostAddress sender; + quint16 senderPort; + + rxSocket->readDatagram(datagram.data(), datagram.size(), + &sender, &senderPort); + + if (datagram.size() == 113) { + processHilDatagram(&datagram); + } + + ui->ed_count->setText(QString::number(count++)); + } +} + + +void SlugsHilSim::activeUasSet(UASInterface* uas){ + + if (uas != NULL) { + activeUas = static_cast (uas); + } +} + + +void SlugsHilSim::processHilDatagram(const QByteArray* datagram){ + unsigned char i = 0; + + mavlink_message_t msg; + + // GPS + mavlink_gps_raw_t tmpGpsRaw; + +#ifdef MAVLINK_ENABLED_SLUGS + mavlink_gps_date_time_t tmpGpsTime; + + tmpGpsTime.year = datagram->at(i++); + tmpGpsTime.month = datagram->at(i++); + tmpGpsTime.day = datagram->at(i++); + tmpGpsTime.hour = datagram->at(i++); + tmpGpsTime.min = datagram->at(i++); + tmpGpsTime.sec = datagram->at(i++); + + tmpGpsRaw.lat = getFloatFromDatagram(datagram, &i); + tmpGpsRaw.lon = getFloatFromDatagram(datagram, &i); + tmpGpsRaw.alt = getFloatFromDatagram(datagram, &i); + + tmpGpsRaw.hdg = getUint16FromDatagram(datagram, &i); + tmpGpsRaw.v = getUint16FromDatagram(datagram, &i); + tmpGpsRaw.eph = getUint16FromDatagram(datagram, &i); + + tmpGpsRaw.fix_type = datagram->at(i++); + tmpGpsTime.visSat = datagram->at(i++); + + mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime); + activeUas->sendMessage(hilLink, msg); +#endif + + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw); + activeUas->sendMessage(hilLink,msg); + + // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model + i++; + + ui->ed_1->setText(QString::number(tmpGpsRaw.hdg)); + ui->ed_2->setText(QString::number(tmpGpsRaw.v)); + ui->ed_3->setText(QString::number(tmpGpsRaw.eph)); +} + +float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned char * i){ + tFloatToChar tmpF2C; + + tmpF2C.chData[0] = datagram->at((*i)++); + tmpF2C.chData[1] = datagram->at((*i)++); + tmpF2C.chData[2] = datagram->at((*i)++); + tmpF2C.chData[3] = datagram->at((*i)++); + + return tmpF2C.flData; +} + +uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigned char * i){ + tUint16ToChar tmpU2C; + + tmpU2C.chData[0] = datagram->at((*i)++); + tmpU2C.chData[1] = datagram->at((*i)++); + + return tmpU2C.uiData; + +} + +void SlugsHilSim::linkSelected(int cbIndex){ + //hilLink = linksAvailable +} diff --git a/src/ui/slugshilsim.h b/src/ui/slugshilsim.h new file mode 100644 index 0000000000000000000000000000000000000000..dc0d8d1d7a90909656c29145de053c7231470cb3 --- /dev/null +++ b/src/ui/slugshilsim.h @@ -0,0 +1,132 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of the configuration Window for Slugs' HIL Simulator + * @author Mariano Lizarraga + */ + +#ifndef SLUGSHILSIM_H +#define SLUGSHILSIM_H + +#include +#include +#include +#include + +#include "LinkInterface.h" +#include "UAS.h" +#include + + +namespace Ui { + class SlugsHilSim; +} + +class SlugsHilSim : public QWidget +{ + Q_OBJECT + +public: + explicit SlugsHilSim(QWidget *parent = 0); + ~SlugsHilSim(); + +protected: + LinkInterface* hilLink; + QHostAddress* simulinkIp; + QUdpSocket* txSocket; + QUdpSocket* rxSocket; + UAS* activeUas; + +public slots: + + /** + * @brief Adds a link to the combo box listing so the user can select a link + * + * Populates the Combo box that allows the user to select the link with which Slugs will + * receive the simulated sensor data from Simulink + * + * @param theLink the link that is being added to the combo box + */ + void addToCombo(LinkInterface* theLink); + + /** + * @brief Puts Slugs in HIL Mode + * + * Sends the required messages through the main communication link to set Slugs in HIL Mode + * + */ + void putInHilMode(void); + + /** + * @brief Receives a datagram from Simulink containing the sensor data. + * + * Receives a datagram from Simulink containing the simulated sensor data. This data is then + * forwarded to Slugs to use as input to the attitude estimation and navigation algorithms. + * + */ + void readDatagram(void); + + /** + * @brief Called when the a new UAS is set to active. + * + * Called when the a new UAS is set to active. + * + * @param uas The new active UAS + */ + void activeUasSet(UASInterface* uas); + + /** + * @brief Called when the Link combobox selects a new link. + * + * @param uas The new index of the selected link + */ + void linkSelected (int cbIndex); + +public slots: + + + +private: + + typedef union _tFloatToChar { + unsigned char chData[4]; + float flData; + } tFloatToChar; + + typedef union _tUint16ToChar { + unsigned char chData[2]; + uint16_t uiData; + } tUint16ToChar; + + Ui::SlugsHilSim *ui; + QHash linksAvailable; + + void processHilDatagram (const QByteArray* datagram); + float getFloatFromDatagram (const QByteArray* datagram, unsigned char * i); + uint16_t getUint16FromDatagram (const QByteArray* datagram, unsigned char * i); + +}; + +#endif // SLUGSHILSIM_H diff --git a/src/ui/slugshilsim.ui b/src/ui/slugshilsim.ui new file mode 100644 index 0000000000000000000000000000000000000000..6ed975ca47e28fd913cb87f1326346dac1ba5d51 --- /dev/null +++ b/src/ui/slugshilsim.ui @@ -0,0 +1,356 @@ + + + SlugsHilSim + + + + 0 + 0 + 325 + 278 + + + + + 320 + 252 + + + + + 450 + 278 + + + + + 10 + + + + Form + + + + + + + + + + + 10 + 75 + true + + + + Qt::LeftToRight + + + IP Address + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + + + + 10 + 75 + true + + + + Qt::LeftToRight + + + Receive Port + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + 10 + 75 + true + + + + Qt::LeftToRight + + + Send Port + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 10 + 75 + true + + + + Qt::RightToLeft + + + Slugs HIL Link + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 171 + 26 + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Set Slugs in HIL Mode + + + true + + + + + + + + + + + + 10 + 75 + true + + + + Qt::LeftToRight + + + Count + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + true + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + true + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + true + + + + + + + + + +