diff --git a/lib/QMapControl/src/mapcontrol.cpp b/lib/QMapControl/src/mapcontrol.cpp index 3a08f663e25c6b7fd5ce9f55cd0fff38f9dc7557..12a70a5a41f5733bdab747159ea8599661819f00 100644 --- a/lib/QMapControl/src/mapcontrol.cpp +++ b/lib/QMapControl/src/mapcontrol.cpp @@ -368,11 +368,13 @@ namespace qmapcontrol layermanager->zoomIn(); update(); } + void MapControl::zoomOut() { layermanager->zoomOut(); update(); } + void MapControl::setZoom(int zoomlevel) { layermanager->setZoom(zoomlevel); diff --git a/qgcunittest/SlugsMavUnitTest.cc b/qgcunittest/SlugsMavUnitTest.cc index 0e15497019615cf68a0819f8898f5b85f842a80c..349903e074bd2647bd20bfa071b6d53ede50c3b6 100644 --- a/qgcunittest/SlugsMavUnitTest.cc +++ b/qgcunittest/SlugsMavUnitTest.cc @@ -1,20 +1,34 @@ -#include "SlugsMavUnitTest.h" - -SlugsMavUnitTest::SlugsMavUnitTest() -{ -} - -void SlugsMavUnitTest::initTestCase() -{ - -} - -void SlugsMavUnitTest::cleanupTestCase() -{ - -} - -void SlugsMavUnitTest::first_test() -{ - QCOMPARE(1,2); -} +#include "SlugsMavUnitTest.h" + +SlugsMavUnitTest::SlugsMavUnitTest() +{ +} + +void SlugsMavUnitTest::initTestCase() +{ + mav = new MAVLinkProtocol(); + slugsMav = new SlugsMAV(mav, UASID); +} + +void SlugsMavUnitTest::cleanupTestCase() +{ + delete slugsMav; + delete mav; +} + +void SlugsMavUnitTest::first_test() +{ + QCOMPARE(1,1); +} + +void SlugsMavUnitTest::getPwmCommands_test() +{ + mavlink_pwm_commands_t* k = slugsMav->getPwmCommands(); + k->aux1=80; + + mavlink_pwm_commands_t* k2 = slugsMav->getPwmCommands(); + k2->aux1=81; + + QCOMPARE(k->aux1, k2->aux1); +} + diff --git a/qgcunittest/SlugsMavUnitTest.h b/qgcunittest/SlugsMavUnitTest.h index 493e4a4c55a8f251b82d114e7e516f5b2eb65b76..51fb52959b64a36afbca8608e3a1f5e6464cb792 100644 --- a/qgcunittest/SlugsMavUnitTest.h +++ b/qgcunittest/SlugsMavUnitTest.h @@ -1,27 +1,35 @@ -#ifndef SLUGSMAVUNITTEST_H -#define SLUGSMAVUNITTEST_H - -#include -#include -#include -#include "UAS.h" -#include "MAVLinkProtocol.h" -#include "UASInterface.h" -#include "AutoTest.h" - -class SlugsMavUnitTest : public QObject -{ - Q_OBJECT -public: - SlugsMavUnitTest(); -signals: - -private slots: - void initTestCase(); - void cleanupTestCase(); - void first_test(); -}; - -DECLARE_TEST(SlugsMavUnitTest) - -#endif // SLUGSMAVUNITTEST_H +#ifndef SLUGSMAVUNITTEST_H +#define SLUGSMAVUNITTEST_H + +#include +#include +#include + +#include "UAS.h" +#include "MAVLinkProtocol.h" +#include "UASInterface.h" +#include "AutoTest.h" +#include "SlugsMAV.h" + +class SlugsMavUnitTest : public QObject +{ + Q_OBJECT +public: +#define UASID 5 + MAVLinkProtocol* mav; + SlugsMAV* slugsMav; + SlugsMavUnitTest(); +signals: + +private slots: + void initTestCase(); + void cleanupTestCase(); + void first_test(); + + void getPwmCommands_test(); + +}; + +DECLARE_TEST(SlugsMavUnitTest) + +#endif // SLUGSMAVUNITTEST_H diff --git a/qgcunittest/UASUnitTest.cc b/qgcunittest/UASUnitTest.cc index 8b4c6c1b795a756737bd261b98af3d6ba1ac7660..5711347b065242a6c5337ae878304ef945a2353d 100644 --- a/qgcunittest/UASUnitTest.cc +++ b/qgcunittest/UASUnitTest.cc @@ -13,9 +13,8 @@ void UASUnitTest::initTestCase() void UASUnitTest::cleanupTestCase() { - delete uas; - delete mav; - + delete uas; + delete mav; } void UASUnitTest::getUASID_test() @@ -31,12 +30,15 @@ void UASUnitTest::getUASID_test() // Make sure that no other ID was sert QEXPECT_FAIL("", "When you set an ID it does not use the default ID of 0", Continue); QCOMPARE(uas->getUASID(), 0); + + // Make sure that ID >= 0 + QCOMPARE(uas->getUASID(), 100); } void UASUnitTest::getUASName_test() { - // Test that the name is build as MAV + ID - QCOMPARE(uas->getUASName(), "MAV 0" + QString::number(UASID)); + // Test that the name is build as MAV + ID + QCOMPARE(uas->getUASName(), "MAV " + QString::number(UASID)); } @@ -119,7 +121,6 @@ void UASUnitTest::getStatusForCode_test() QVERIFY(state == "UNKNOWN"); } - void UASUnitTest::getLocalX_test() { QCOMPARE(uas->getLocalX(), 0.0); @@ -155,124 +156,180 @@ void UASUnitTest::getYaw_test() { QCOMPARE(uas->getYaw(), 0.0); } -void UASUnitTest::attitudeLimitsZero_test() + +void UASUnitTest::getSelected_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - - // Set zero values and encode them - att.roll = 0.0f; - att.pitch = 0.0f; - att.yaw = 0.0f; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QCOMPARE(uas->getRoll(), 0.0); - QCOMPARE(uas->getPitch(), 0.0); - QCOMPARE(uas->getYaw(), 0.0); + QCOMPARE(uas->getSelected(), false); } -void UASUnitTest::attitudeLimitsPi_test() +void UASUnitTest::getSystemType_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set PI values and encode them - att.roll = M_PI; - att.pitch = M_PI; - att.yaw = M_PI; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() < M_PI+0.000001) && (uas->getRoll() > M_PI+-0.000001)); - QVERIFY((uas->getPitch() < M_PI+0.000001) && (uas->getPitch() > M_PI+-0.000001)); - QVERIFY((uas->getYaw() < M_PI+0.000001) && (uas->getYaw() > M_PI+-0.000001)); + QCOMPARE(uas->getSystemType(), 13); } -void UASUnitTest::attitudeLimitsMinusPi_test() +void UASUnitTest::getAirframe_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set -PI values and encode them - att.roll = -M_PI; - att.pitch = -M_PI; - att.yaw = -M_PI; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() > -M_PI-0.000001) && (uas->getRoll() < -M_PI+0.000001)); - QVERIFY((uas->getPitch() > -M_PI-0.000001) && (uas->getPitch() < -M_PI+0.000001)); - QVERIFY((uas->getYaw() > -M_PI-0.000001) && (uas->getYaw() < -M_PI+0.000001)); + QCOMPARE(uas->getAirframe(), 0); + + uas->setAirframe(25); + QVERIFY(uas->getAirframe() == 25); } -void UASUnitTest::attitudeLimitsTwoPi_test() +void UASUnitTest::getWaypointList_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set off-limit values and check - // correct wrapping - // Set 2PI values and encode them - att.roll = 2*M_PI; - att.pitch = 2*M_PI; - att.yaw = 2*M_PI; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); - QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); - QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); + QVector kk = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(kk.count(), 0); + + Waypoint* wp = new Waypoint(0,0,0,0,0,false, false, 0,0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); + uas->getWaypointManager()->addWaypoint(wp, true); + + kk = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(kk.count(), 1); + + wp = new Waypoint(); + uas->getWaypointManager()->addWaypoint(wp, false); + + kk = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(kk.count(), 2); + + uas->getWaypointManager()->removeWaypoint(1); + kk = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(kk.count(), 1); + + uas->getWaypointManager()->removeWaypoint(0); + kk = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(kk.count(), 0); + + qDebug()<<"disconnect SIGNAL waypointListChanged"; +} + +void UASUnitTest::getWaypoint_test() +{ + Waypoint* wp = new Waypoint(0,5.6,0,0,0,false, false, 0,0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); + + uas->getWaypointManager()->addWaypoint(wp, true); + + QVector wpList = uas->getWaypointManager()->getWaypointList(); + + QCOMPARE(wpList.count(), 1); + QCOMPARE(static_cast(0), static_cast(wpList.at(0))->getId()); + + wp = new Waypoint(0, 5.6, 2, 3); + uas->getWaypointManager()->addWaypoint(wp, true); + Waypoint* wp2 = static_cast(wpList.at(0)); + + QCOMPARE(wp->getX(), wp2->getX()); + QCOMPARE(wp->getFrame(), MAV_FRAME_GLOBAL); + QCOMPARE(wp->getFrame(), wp2->getFrame()); } -void UASUnitTest::attitudeLimitsMinusTwoPi_test() +void UASUnitTest::signalWayPoint_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set -2PI values and encode them - att.roll = -2*M_PI; - att.pitch = -2*M_PI; - att.yaw = -2*M_PI; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); - QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); - QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); + QSignalSpy spy(uas->getWaypointManager(), SIGNAL(waypointListChanged())); + + Waypoint* wp = new Waypoint(0,5.6,0,0,0,false, false, 0,0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); + uas->getWaypointManager()->addWaypoint(wp, true); + + QCOMPARE(spy.count(), 1); // 1 listChanged for add wayPoint + uas->getWaypointManager()->removeWaypoint(0); + QCOMPARE(spy.count(), 2); // 2 listChanged for remove wayPoint + + QSignalSpy spyDestroyed(uas->getWaypointManager(), SIGNAL(destroyed())); + QVERIFY(spyDestroyed.isValid()); + QCOMPARE( spyDestroyed.count(), 0 ); + + delete uas;// delete(destroyed) uas for validating + QCOMPARE(spyDestroyed.count(), 1);// count destroyed uas should are 1 + + uas = new UAS(mav,UASID); + QSignalSpy spy2(uas->getWaypointManager(), SIGNAL(waypointListChanged())); + QCOMPARE(spy2.count(), 0); + + uas->getWaypointManager()->addWaypoint(wp, true); + QCOMPARE(spy2.count(), 1); + + uas->getWaypointManager()->clearWaypointList(); + QVector wpList = uas->getWaypointManager()->getWaypointList(); + QCOMPARE(wpList.count(), 1); } -void UASUnitTest::attitudeLimitsTwoPiOne_test() +void UASUnitTest::signalUASLink_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set over 2 PI values and encode them - att.roll = 2*M_PI+1.0f; - att.pitch = 2*M_PI+1.0f; - att.yaw = 2*M_PI+1.0f; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() < 1.000001) && (uas->getRoll() > 0.999999)); - QVERIFY((uas->getPitch() < 1.000001) && (uas->getPitch() > 0.999999)); - QVERIFY((uas->getYaw() < 1.000001) && (uas->getYaw() > 0.999999)); + QSignalSpy spy(uas, SIGNAL(modeChanged(int,QString,QString))); + uas->setMode(2); + QCOMPARE(spy.count(), 0);// not solve for UAS not receiving message from UAS + + QSignalSpy spyS(LinkManager::instance(), SIGNAL(newLink(LinkInterface*))); + SerialLink* link = new SerialLink(); + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mav); + QCOMPARE(spyS.count(), 1); + + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mav); + QCOMPARE(spyS.count(), 1);// not add SerialLink, exist in list + + SerialLink* link2 = new SerialLink(); + LinkManager::instance()->add(link2); + LinkManager::instance()->addProtocol(link2, mav); + QCOMPARE(spyS.count(), 2);// add SerialLink, not exist in list + + QList links = LinkManager::instance()->getLinks(); + foreach(LinkInterface* link, links) + { + qDebug()<< link->getName(); + qDebug()<< QString::number(link->getId()); + qDebug()<< QString::number(link->getNominalDataRate()); + QVERIFY(link != NULL); + uas->addLink(link); + } + + SerialLink* ff = static_cast(uas->getLinks()->at(0)); + + QCOMPARE(ff->isConnected(), false); + + QCOMPARE(ff->isRunning(), false); + + QCOMPARE(ff->isFinished(), false); + + QCOMPARE(links.count(), uas->getLinks()->count()); + QCOMPARE(uas->getLinks()->count(), 2); + + LinkInterface* ff99 = static_cast(links.at(1)); + LinkManager::instance()->removeLink(ff99); + + QCOMPARE(LinkManager::instance()->getLinks().count(), 1); + QCOMPARE(uas->getLinks()->count(), 2); + + + QCOMPARE(static_cast(LinkManager::instance()->getLinks().at(0))->getId(), + static_cast(uas->getLinks()->at(0))->getId()); + + link = new SerialLink(); + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mav); + QCOMPARE(spyS.count(), 3); } -void UASUnitTest::attitudeLimitsMinusTwoPiOne_test() +void UASUnitTest::signalIdUASLink_test() { - mavlink_message_t msg; - mavlink_attitude_t att; - // Set 3PI values and encode them - att.roll = -2*M_PI-1.0f; - att.pitch = -2*M_PI-1.0f; - att.yaw = -2*M_PI-1.0f; - mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); - // Let UAS decode message - uas->receiveMessage(link, msg); - // Check result - QVERIFY((uas->getRoll() > -1.000001) && (uas->getRoll() < -0.999999)); - QVERIFY((uas->getPitch() > -1.000001) && (uas->getPitch() < -0.999999)); - QVERIFY((uas->getYaw() > -1.000001) && (uas->getYaw() < -0.999999)); + SerialLink* myLink = new SerialLink(); + myLink->setPortName("COM 17"); + LinkManager::instance()->add(myLink); + LinkManager::instance()->addProtocol(myLink, mav); + + myLink = new SerialLink(); + myLink->setPortName("COM 18"); + LinkManager::instance()->add(myLink); + LinkManager::instance()->addProtocol(myLink, mav); + + QCOMPARE(LinkManager::instance()->getLinks().count(), 4); + + QList links = LinkManager::instance()->getLinks(); + + LinkInterface* a = static_cast(links.at(2)); + LinkInterface* b = static_cast(links.at(3)); + + QCOMPARE(a->getName(), QString("serial port COM 17")); + QCOMPARE(b->getName(), QString("serial port COM 18")); } diff --git a/qgcunittest/UASUnitTest.h b/qgcunittest/UASUnitTest.h index 7bd98bd23ed0db1be3e38460e01970aad85d68b2..4fcb10a935be5fed6371f0d8e0173356c828f7c7 100644 --- a/qgcunittest/UASUnitTest.h +++ b/qgcunittest/UASUnitTest.h @@ -4,51 +4,63 @@ #include #include #include +#include + #include "UAS.h" #include "MAVLinkProtocol.h" #include "SerialLink.h" #include "UASInterface.h" #include "AutoTest.h" +#include "LinkManager.h" +#include "UASWaypointManager.h" +#include "SerialLink.h" +#include "LinkInterface.h" class UASUnitTest : public QObject { Q_OBJECT public: -#define UASID 50 - MAVLinkProtocol* mav; - UAS* uas; - SerialLink* link; - UASUnitTest(); + #define UASID 100 + MAVLinkProtocol* mav; + UAS* uas; + UASUnitTest(); signals: private slots: - void initTestCase(); - void cleanupTestCase(); - void getUASID_test(); - void getUASName_test(); - void getUpTime_test(); - void getCommunicationStatus_test(); - void filterVoltage_test(); - void getAutopilotType_test(); - void setAutopilotType_test(); - void getStatusForCode_test(); - void getLocalX_test(); - void getLocalY_test(); - void getLocalZ_test(); - void getLatitude_test(); - void getLongitude_test(); - void getAltitude_test(); - void getRoll_test(); - void getPitch_test(); - void getYaw_test(); - void attitudeLimitsZero_test(); - void attitudeLimitsPi_test(); - void attitudeLimitsMinusPi_test(); - void attitudeLimitsTwoPi_test(); - void attitudeLimitsMinusTwoPi_test(); - void attitudeLimitsTwoPiOne_test(); - void attitudeLimitsMinusTwoPiOne_test(); + void initTestCase(); + void cleanupTestCase(); + + + void getUASID_test(); + void getUASName_test(); + void getUpTime_test(); + void getCommunicationStatus_test(); + void filterVoltage_test(); + void getAutopilotType_test(); + void setAutopilotType_test(); + void getStatusForCode_test(); + void getLocalX_test(); + void getLocalY_test(); + void getLocalZ_test(); + void getLatitude_test(); + void getLongitude_test(); + void getAltitude_test(); + void getRoll_test(); + void getPitch_test(); + void getYaw_test(); + + void getSelected_test(); + void getSystemType_test(); + void getAirframe_test(); + + + void getWaypointList_test(); + void getWaypoint_test(); + + void signalWayPoint_test(); + void signalUASLink_test(); + void signalIdUASLink_test(); protected: UAS *prueba; diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index a5bd6af35fcc58c415996bf62cddbdadece3b1b9..36c57a1bb6634cf7e7522ca536bce6d3690f42ca 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -395,7 +395,8 @@ win32-g++ { message(Building for Windows Platform (32bit)) # Special settings for debug - #CONFIG += CONSOLE + CONFIG += CONSOLE + OUTPUT += CONSOLE INCLUDEPATH += $$BASEDIR/lib/sdl/include \ $$BASEDIR/lib/opal/include #\ #\ diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d6f42d825d3160e53976441b5a8079390c8e47f6..19cc41a7e23adc4eba58f202b204464c20f888bc 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -146,8 +146,6 @@ FORMS += src/ui/MainWindow.ui \ src/ui/map3D/QGCGoogleEarthView.ui \ src/ui/SlugsDataSensorView.ui \ src/ui/SlugsHilSim.ui \ - src/ui/SlugsPIDControl.ui \ - src/ui/SlugsVideoCamControl.ui \ src/ui/SlugsPadCameraControl.ui \ src/ui/uas/QGCUnconnectedInfoWidget.ui \ src/ui/designer/QGCToolWidget.ui \ @@ -159,6 +157,7 @@ FORMS += src/ui/MainWindow.ui \ src/ui/mission/QGCCustomWaypointAction.ui \ src/ui/QGCUDPLinkConfiguration.ui \ src/ui/QGCSettingsWidget.ui \ + src/ui/UASControlParameters.ui \ src/ui/mission/QGCMissionDoWidget.ui \ src/ui/mission/QGCMissionConditionWidget.ui @@ -256,8 +255,6 @@ HEADERS += src/MG.h \ src/ui/map3D/QGCWebPage.h \ src/ui/SlugsDataSensorView.h \ src/ui/SlugsHilSim.h \ - src/ui/SlugsPIDControl.h \ - src/ui/SlugsVideoCamControl.h \ src/ui/SlugsPadCameraControl.h \ src/ui/QGCMainWindowAPConfigurator.h \ src/comm/MAVLinkSwarmSimulationLink.h \ @@ -274,6 +271,7 @@ HEADERS += src/MG.h \ src/ui/QGCWaypointListMulti.h \ src/ui/QGCUDPLinkConfiguration.h \ src/ui/QGCSettingsWidget.h \ + src/ui/uas/UASControlParameters.h \ src/ui/mission/QGCMissionDoWidget.h \ src/ui/mission/QGCMissionConditionWidget.h \ src/uas/QGCUASParamManager.h @@ -387,8 +385,6 @@ SOURCES += src/main.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 \ src/ui/QGCMainWindowAPConfigurator.cc \ src/comm/MAVLinkSwarmSimulationLink.cc \ @@ -405,6 +401,7 @@ SOURCES += src/main.cc \ src/ui/QGCWaypointListMulti.cc \ src/ui/QGCUDPLinkConfiguration.cc \ src/ui/QGCSettingsWidget.cc \ + src/ui/uas/UASControlParameters.cpp \ src/ui/mission/QGCMissionDoWidget.cc \ src/ui/mission/QGCMissionConditionWidget.cc \ src/uas/QGCUASParamManager.cc @@ -469,3 +466,6 @@ win32:exists(src/lib/opalrt/OpalApi.h):exists(C:/OPAL-RT/RT-LAB7.2.4/Common/bin) FORMS += src/ui/OpalLinkSettings.ui DEFINES += OPAL_RT } + +TRANSLATIONS += es-MX.ts \ + en-US.ts diff --git a/src/Core.cc b/src/Core.cc index d5846985218c373f6b75a72cdc0730b284a81f47..38a37b93bb6ca8d4d9c2c3869b261d9f5aa062d5 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -65,8 +65,11 @@ This file is part of the QGROUNDCONTROL project * @param argv The string array of parameters **/ + Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) { + + // Set application name this->setApplicationName(QGC_APPLICATION_NAME); this->setApplicationVersion(QGC_APPLICATION_VERSION); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 822e1da3109ca6567801489cb4b9096f35698efa..983eabbb4ecb9a2a37d0ec2a7a0861e228bd4803 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -324,6 +324,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) currLossCounter = 0; currReceiveCounter = 0; emit receiveLossChanged(message.sysid, receiveLoss); + //qDebug() << "LOSSCHANGED" << message.sysid<<" "<writeDatagram(data, size, currentHost, currentPort); } diff --git a/src/lib/qextserialport/win_qextserialport.cpp b/src/lib/qextserialport/win_qextserialport.cpp index 39768baf45a54702020ba8f15df4ab5dd0cdb4bb..7fca3621ba5f1e48c8890c975afd676a5cfdfeb9 100644 --- a/src/lib/qextserialport/win_qextserialport.cpp +++ b/src/lib/qextserialport/win_qextserialport.cpp @@ -1,1070 +1,1072 @@ -//#include -//#include -//#include -//#include -#include -#include "win_qextserialport.h" - - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort() -Default constructor. Note that the name of the device used by a Win_QextSerialPort constructed -with this constructor will be determined by #defined constants, or lack thereof - the default -behavior is the same as _TTY_LINUX_. Possible naming conventions and their associated constants -are: - -\verbatim - -Constant Used By Naming Convention ----------- ------------- ------------------------ -_TTY_WIN_ Windows COM1, COM2 -_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 -_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 -_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb -_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 -_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 -_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 - Linux /dev/ttyS0, /dev/ttyS1 -\endverbatim - -This constructor associates the object with the first port on the system, e.g. COM1 for Windows -platforms. See the other constructor if you need a port other than the first. -*/ -Win_QextSerialPort::Win_QextSerialPort(QextSerialBase::QueryMode mode): - QextSerialBase() -{ - Win_Handle=INVALID_HANDLE_VALUE; - setQueryMode(mode); - init(); -} - -/*!Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort&) -Copy constructor. -*/ -Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort& s): - QextSerialBase(s.port) -{ - Win_Handle=INVALID_HANDLE_VALUE; - _queryMode = s._queryMode; - _bytesToWrite = s._bytesToWrite; - bytesToWriteLock = new QReadWriteLock; - overlapThread = new Win_QextSerialThread(this); - memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); - memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); - setOpenMode(s.openMode()); - lastErr=s.lastErr; - port = s.port; - Settings.FlowControl=s.Settings.FlowControl; - Settings.Parity=s.Settings.Parity; - Settings.DataBits=s.Settings.DataBits; - Settings.StopBits=s.Settings.StopBits; - Settings.BaudRate=s.Settings.BaudRate; - Win_Handle=s.Win_Handle; - memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); - memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); - if (s.overlapThread->isRunning()) - overlapThread->start(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name) -Constructs a serial port attached to the port specified by devName. -devName is the name of the device, which is windowsystem-specific, -e.g."COM2" or "/dev/ttyS0". -*/ -Win_QextSerialPort::Win_QextSerialPort(const QString & name, QextSerialBase::QueryMode mode): - QextSerialBase(name) -{ - Win_Handle=INVALID_HANDLE_VALUE; - setQueryMode(mode); - init(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings) -Constructs a port with default name and specified settings. -*/ -Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings, QextSerialBase::QueryMode mode): - QextSerialBase() -{ - Win_Handle=INVALID_HANDLE_VALUE; - setBaudRate(settings.BaudRate); - setDataBits(settings.DataBits); - setStopBits(settings.StopBits); - setParity(settings.Parity); - setFlowControl(settings.FlowControl); - setTimeout(settings.Timeout_Millisec); - setQueryMode(mode); - init(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings) -Constructs a port with specified name and settings. -*/ -Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings, QextSerialBase::QueryMode mode): - QextSerialBase(name) -{ - Win_Handle=INVALID_HANDLE_VALUE; - setPortName(name); - setBaudRate(settings.BaudRate); - setDataBits(settings.DataBits); - setStopBits(settings.StopBits); - setParity(settings.Parity); - setFlowControl(settings.FlowControl); - setTimeout(settings.Timeout_Millisec); - setQueryMode(mode); - init(); -} - -void Win_QextSerialPort::init() -{ - _bytesToWrite = 0; - overlap.Internal = 0; - overlap.InternalHigh = 0; - overlap.Offset = 0; - overlap.OffsetHigh = 0; - overlap.hEvent = CreateEvent(NULL, true, false, NULL); - overlapThread = new Win_QextSerialThread(this); - bytesToWriteLock = new QReadWriteLock; -} - -/*! -\fn Win_QextSerialPort::~Win_QextSerialPort() -Standard destructor. -*/ -Win_QextSerialPort::~Win_QextSerialPort() { - if (isOpen()) { - close(); - } - CloseHandle(overlap.hEvent); - delete overlapThread; - delete bytesToWriteLock; -} - -/*! -\fn Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) -overrides the = operator -*/ -Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) { - setOpenMode(s.openMode()); - _queryMode = s._queryMode; - _bytesToWrite = s._bytesToWrite; - bytesToWriteLock = new QReadWriteLock; - overlapThread = new Win_QextSerialThread(this); - memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); - memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); - lastErr=s.lastErr; - port = s.port; - Settings.FlowControl=s.Settings.FlowControl; - Settings.Parity=s.Settings.Parity; - Settings.DataBits=s.Settings.DataBits; - Settings.StopBits=s.Settings.StopBits; - Settings.BaudRate=s.Settings.BaudRate; - Win_Handle=s.Win_Handle; - memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); - memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); - if (s.overlapThread->isRunning()) - overlapThread->start(); - return *this; -} - - -/*! -\fn bool Win_QextSerialPort::open(OpenMode mode) -Opens a serial port. Note that this function does not specify which device to open. If you need -to open a device by name, see Win_QextSerialPort::open(const char*). This function has no effect -if the port associated with the class is already open. The port is also configured to the current -settings, as stored in the Settings structure. -*/ -bool Win_QextSerialPort::open(OpenMode mode) { - unsigned long confSize = sizeof(COMMCONFIG); - Win_CommConfig.dwSize = confSize; - DWORD dwFlagsAndAttributes = 0; - if (queryMode() == QextSerialBase::EventDriven) - dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; - - LOCK_MUTEX(); - if (mode == QIODevice::NotOpen) - return isOpen(); - if (!isOpen()) { - /*open the port*/ - Win_Handle=CreateFileA(port.toAscii(), GENERIC_READ|GENERIC_WRITE, - FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); - if (Win_Handle!=INVALID_HANDLE_VALUE) { - /*configure port settings*/ - GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); - GetCommState(Win_Handle, &(Win_CommConfig.dcb)); - - /*set up parameters*/ - Win_CommConfig.dcb.fBinary=TRUE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - Win_CommConfig.dcb.fAbortOnError=FALSE; - Win_CommConfig.dcb.fNull=FALSE; - setBaudRate(Settings.BaudRate); - setDataBits(Settings.DataBits); - setStopBits(Settings.StopBits); - setParity(Settings.Parity); - setFlowControl(Settings.FlowControl); - setTimeout(Settings.Timeout_Millisec); - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - - //init event driven approach - if (queryMode() == QextSerialBase::EventDriven) { - Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; - Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - SetCommTimeouts(Win_Handle, &Win_CommTimeouts); - if (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { - qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); - UNLOCK_MUTEX(); - return false; - } - overlapThread->start(); - } - QIODevice::open(mode); - } - } else { - UNLOCK_MUTEX(); - return false; - } - UNLOCK_MUTEX(); - return isOpen(); -} - -/*! -\fn void Win_QextSerialPort::close() -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ -void Win_QextSerialPort::close() -{ - LOCK_MUTEX(); - - if (isOpen()) { - flush(); - if (overlapThread->isRunning()) { - overlapThread->stop(); - if (QThread::currentThread() != overlapThread) - overlapThread->wait(); - } - if (CloseHandle(Win_Handle)) - Win_Handle = INVALID_HANDLE_VALUE; - _bytesToWrite = 0; - QIODevice::close(); - } - - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::flush() -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ -void Win_QextSerialPort::flush() { - LOCK_MUTEX(); - if (isOpen()) { - FlushFileBuffers(Win_Handle); - } - UNLOCK_MUTEX(); -} - -/*! -\fn qint64 Win_QextSerialPort::size() const -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use Win_QextSerialPort::bytesAvailable() instead. -*/ -qint64 Win_QextSerialPort::size() const { - int availBytes; - COMSTAT Win_ComStat; - DWORD Win_ErrorMask=0; - ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); - availBytes = Win_ComStat.cbInQue; - return (qint64)availBytes; -} - -/*! -\fn qint64 Win_QextSerialPort::bytesAvailable() -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ -qint64 Win_QextSerialPort::bytesAvailable() const { - LOCK_MUTEX(); - if (isOpen()) { - DWORD Errors; - COMSTAT Status; - if (ClearCommError(Win_Handle, &Errors, &Status)) { - UNLOCK_MUTEX(); - return Status.cbInQue + QIODevice::bytesAvailable(); - } - UNLOCK_MUTEX(); - return (qint64)-1; - } - UNLOCK_MUTEX(); - return 0; -} - -/*! -\fn void Win_QextSerialPort::translateError(ulong error) -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ -void Win_QextSerialPort::translateError(ulong error) { - if (error&CE_BREAK) { - lastErr=E_BREAK_CONDITION; - } - else if (error&CE_FRAME) { - lastErr=E_FRAMING_ERROR; - } - else if (error&CE_IOE) { - lastErr=E_IO_ERROR; - } - else if (error&CE_MODE) { - lastErr=E_INVALID_FD; - } - else if (error&CE_OVERRUN) { - lastErr=E_BUFFER_OVERRUN; - } - else if (error&CE_RXPARITY) { - lastErr=E_RECEIVE_PARITY_ERROR; - } - else if (error&CE_RXOVER) { - lastErr=E_RECEIVE_OVERFLOW; - } - else if (error&CE_TXFULL) { - lastErr=E_TRANSMIT_OVERFLOW; - } -} - -/*! -\fn qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) -Reads a block of data from the serial port. This function will read at most maxlen bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. - -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) -{ - DWORD retVal; - - LOCK_MUTEX(); - - retVal = 0; - if (queryMode() == QextSerialBase::EventDriven) { - OVERLAPPED overlapRead; - overlapRead.Internal = 0; - overlapRead.InternalHigh = 0; - overlapRead.Offset = 0; - overlapRead.OffsetHigh = 0; - overlapRead.hEvent = CreateEvent(NULL, true, false, NULL); - if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { - if (GetLastError() == ERROR_IO_PENDING) - GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); - else { - lastErr = E_READ_FAILED; - retVal = (DWORD)-1; - } - } - CloseHandle(overlapRead.hEvent); - } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { - lastErr = E_READ_FAILED; - retVal = (DWORD)-1; - } - - UNLOCK_MUTEX(); - - return (qint64)retVal; -} - -/*! -\fn qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) -Writes a block of data to the serial port. This function will write len bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. - -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) -{ - DWORD retVal; - - LOCK_MUTEX(); - - retVal = 0; - if (queryMode() == QextSerialBase::EventDriven) { - bytesToWriteLock->lockForWrite(); - _bytesToWrite += maxSize; - bytesToWriteLock->unlock(); - overlapWrite.Internal = 0; - overlapWrite.InternalHigh = 0; - overlapWrite.Offset = 0; - overlapWrite.OffsetHigh = 0; - overlapWrite.hEvent = CreateEvent(NULL, true, false, NULL); - if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapWrite)) { - lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - } else - retVal = maxSize; - } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { - lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - } - - UNLOCK_MUTEX(); - - return (qint64)retVal; -} - -/*! -\fn void Win_QextSerialPort::ungetChar(char c) -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ -void Win_QextSerialPort::ungetChar(char c) { - - /*meaningless on unbuffered sequential device - return error and print a warning*/ - TTY_WARNING("Win_QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); -} - -/*! -\fn void Win_QextSerialPort::setFlowControl(FlowType flow) -Sets the flow control used by the port. Possible values of flow are: -\verbatim - FLOW_OFF No flow control - FLOW_HARDWARE Hardware (RTS/CTS) flow control - FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -*/ -void Win_QextSerialPort::setFlowControl(FlowType flow) { - LOCK_MUTEX(); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; - } - if (isOpen()) { - switch(flow) { - - /*no flow control*/ - case FLOW_OFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - - /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=TRUE; - Win_CommConfig.dcb.fOutX=TRUE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - - case FLOW_HARDWARE: - Win_CommConfig.dcb.fOutxCtsFlow=TRUE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setParity(ParityType parity) -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim - PAR_SPACE Space Parity - PAR_MARK Mark Parity - PAR_NONE No Parity - PAR_EVEN Even Parity - PAR_ODD Odd Parity -\endverbatim -*/ -void Win_QextSerialPort::setParity(ParityType parity) { - LOCK_MUTEX(); - if (Settings.Parity!=parity) { - Settings.Parity=parity; - } - if (isOpen()) { - Win_CommConfig.dcb.Parity=(unsigned char)parity; - switch (parity) { - - /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); - } - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*mark parity - WINDOWS ONLY*/ - case PAR_MARK: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*no parity*/ - case PAR_NONE: - Win_CommConfig.dcb.fParity=FALSE; - break; - - /*even parity*/ - case PAR_EVEN: - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*odd parity*/ - case PAR_ODD: - Win_CommConfig.dcb.fParity=TRUE; - break; - } - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setDataBits(DataBitsType dataBits) -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim - DATA_5 5 data bits - DATA_6 6 data bits - DATA_7 7 data bits - DATA_8 8 data bits -\endverbatim - -\note -This function is subject to the following restrictions: -\par - 5 data bits cannot be used with 2 stop bits. -\par - 1.5 stop bits can only be used with 5 data bits. -\par - 8 data bits cannot be used with space parity on POSIX systems. - -*/ -void Win_QextSerialPort::setDataBits(DataBitsType dataBits) { - LOCK_MUTEX(); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { - } - else { - Settings.DataBits=dataBits; - } - } - if (isOpen()) { - switch(dataBits) { - - /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("Win_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=5; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*6 data bits*/ - case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=6; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*7 data bits*/ - case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=7; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*8 data bits*/ - case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=8; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setStopBits(StopBitsType stopBits) -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim - STOP_1 1 stop bit - STOP_1_5 1.5 stop bits - STOP_2 2 stop bits -\endverbatim - -\note -This function is subject to the following restrictions: -\par - 2 stop bits cannot be used with 5 data bits. -\par - 1.5 stop bits cannot be used with 6 or more data bits. -\par - POSIX does not support 1.5 stop bits. -*/ -void Win_QextSerialPort::setStopBits(StopBitsType stopBits) { - LOCK_MUTEX(); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || - (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { - } - else { - Settings.StopBits=stopBits; - } - } - if (isOpen()) { - switch (stopBits) { - - /*one stop bit*/ - case STOP_1: - Win_CommConfig.dcb.StopBits=ONESTOPBIT; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; - - /*1.5 stop bits*/ - case STOP_1_5: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); - if (Settings.DataBits!=DATA_5) { - TTY_WARNING("Win_QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=ONE5STOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - - /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("Win_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=TWOSTOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. -\verbatim - - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- - BAUD50 110 50 - BAUD75 110 75 - *BAUD110 110 110 - BAUD134 110 134.5 - BAUD150 110 150 - BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 - BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 - BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 - BAUD56000 56000 38400 - *BAUD57600 57600 57600 - BAUD76800 57600 76800 - *BAUD115200 115200 115200 - BAUD128000 128000 115200 - BAUD256000 256000 115200 - BAUD230400 230400 115200 - BAUD256000 256000 115200 - BAUD460800 460800 115200 - BAUD921600 921600 115200 -\endverbatim -*/ -void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) { - LOCK_MUTEX(); - if (Settings.BaudRate!=baudRate) { - switch (baudRate) { - case BAUD50: - case BAUD75: - case BAUD134: - case BAUD150: - case BAUD200: - Settings.BaudRate=BAUD110; - break; - - case BAUD1800: - Settings.BaudRate=BAUD1200; - break; - - case BAUD76800: - Settings.BaudRate=BAUD57600; - break; - - default: - Settings.BaudRate=baudRate; - break; - } - } - if (isOpen()) { - switch (baudRate) { - - /*50 baud*/ - case BAUD50: - TTY_WARNING("Win_QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*75 baud*/ - case BAUD75: - TTY_WARNING("Win_QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*110 baud*/ - case BAUD110: - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*134.5 baud*/ - case BAUD134: - TTY_WARNING("Win_QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*150 baud*/ - case BAUD150: - TTY_WARNING("Win_QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*200 baud*/ - case BAUD200: - TTY_WARNING("Win_QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; - - /*300 baud*/ - case BAUD300: - Win_CommConfig.dcb.BaudRate=CBR_300; - break; - - /*600 baud*/ - case BAUD600: - Win_CommConfig.dcb.BaudRate=CBR_600; - break; - - /*1200 baud*/ - case BAUD1200: - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; - - /*1800 baud*/ - case BAUD1800: - TTY_WARNING("Win_QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; - - /*2400 baud*/ - case BAUD2400: - Win_CommConfig.dcb.BaudRate=CBR_2400; - break; - - /*4800 baud*/ - case BAUD4800: - Win_CommConfig.dcb.BaudRate=CBR_4800; - break; - - /*9600 baud*/ - case BAUD9600: - Win_CommConfig.dcb.BaudRate=CBR_9600; - break; - - /*14400 baud*/ - case BAUD14400: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_14400; - break; - - /*19200 baud*/ - case BAUD19200: - Win_CommConfig.dcb.BaudRate=CBR_19200; - break; - - /*38400 baud*/ - case BAUD38400: - Win_CommConfig.dcb.BaudRate=CBR_38400; - break; - - /*56000 baud*/ - case BAUD56000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_56000; - break; - - /*57600 baud*/ - case BAUD57600: - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; - - /*76800 baud*/ - case BAUD76800: - TTY_WARNING("Win_QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; - - /*115200 baud*/ - case BAUD115200: - Win_CommConfig.dcb.BaudRate=CBR_115200; - break; - - /*128000 baud*/ - case BAUD128000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_128000; - break; - - /*256000 baud*/ - case BAUD256000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_256000; - break; - /*230400 baud*/ - case BAUD230400: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 230400 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_230400; - break; - /*460800 baud*/ - case BAUD460800: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 460800 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_460800; - break; - /*921600 baud*/ - case BAUD921600: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 921600 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_921600; - break; - } - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setDtr(bool set) -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void Win_QextSerialPort::setDtr(bool set) { - LOCK_MUTEX(); - if (isOpen()) { - if (set) { - EscapeCommFunction(Win_Handle, SETDTR); - } - else { - EscapeCommFunction(Win_Handle, CLRDTR); - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setRts(bool set) -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void Win_QextSerialPort::setRts(bool set) { - LOCK_MUTEX(); - if (isOpen()) { - if (set) { - EscapeCommFunction(Win_Handle, SETRTS); - } - else { - EscapeCommFunction(Win_Handle, CLRRTS); - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn ulong Win_QextSerialPort::lineStatus(void) -returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: - -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -\endverbatim - -This function will return 0 if the port associated with the class is not currently open. -*/ -ulong Win_QextSerialPort::lineStatus(void) { - unsigned long Status=0, Temp=0; - LOCK_MUTEX(); - if (isOpen()) { - GetCommModemStatus(Win_Handle, &Temp); - if (Temp&MS_CTS_ON) { - Status|=LS_CTS; - } - if (Temp&MS_DSR_ON) { - Status|=LS_DSR; - } - if (Temp&MS_RING_ON) { - Status|=LS_RI; - } - if (Temp&MS_RLSD_ON) { - Status|=LS_DCD; - } - } - UNLOCK_MUTEX(); - return Status; -} - -bool Win_QextSerialPort::waitForReadyRead(int msecs) -{ - //@todo implement - return false; -} - -qint64 Win_QextSerialPort::bytesToWrite() const -{ - return _bytesToWrite; -} - -void Win_QextSerialPort::monitorCommEvent() -{ - DWORD eventMask = 0; - - ResetEvent(overlap.hEvent); - if (!WaitCommEvent(Win_Handle, & eventMask, & overlap)) - if (GetLastError() != ERROR_IO_PENDING) - qCritical("WaitCommEvent error %ld\n", GetLastError()); - - if (WaitForSingleObject(overlap.hEvent, INFINITE) == WAIT_OBJECT_0) { - //overlap event occured - DWORD undefined; - if (!GetOverlappedResult(Win_Handle, & overlap, & undefined, false)) { - qWarning("CommEvent overlapped error %ld", GetLastError()); - return; - } - if (eventMask & EV_RXCHAR) { - if (sender() != this) - emit readyRead(); - } - if (eventMask & EV_TXEMPTY) { - DWORD numBytes; - GetOverlappedResult(Win_Handle, & overlapWrite, & numBytes, true); - bytesToWriteLock->lockForWrite(); - if (sender() != this) - emit bytesWritten(bytesToWrite()); - _bytesToWrite = 0; - bytesToWriteLock->unlock(); - } - if (eventMask & EV_DSR) - if (lineStatus() & LS_DSR) - emit dsrChanged(true); - else - emit dsrChanged(false); - } -} - -void Win_QextSerialPort::terminateCommWait() -{ - SetCommMask(Win_Handle, 0); -} - - -/*! -\fn void Win_QextSerialPort::setTimeout(ulong millisec); -Sets the read and write timeouts for the port to millisec milliseconds. -Setting 0 indicates that timeouts are not used for read nor write operations; -however read() and write() functions will still block. Set -1 to provide -non-blocking behaviour (read() and write() will return immediately). - -\note this function does nothing in event driven mode. -*/ -void Win_QextSerialPort::setTimeout(long millisec) { - LOCK_MUTEX(); - Settings.Timeout_Millisec = millisec; - - if (millisec == -1) { - Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; - } else { - Win_CommTimeouts.ReadIntervalTimeout = millisec; - Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; - } - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - if (queryMode() != QextSerialBase::EventDriven) - SetCommTimeouts(Win_Handle, &Win_CommTimeouts); - - UNLOCK_MUTEX(); -} - - -Win_QextSerialThread::Win_QextSerialThread(Win_QextSerialPort * qesp): - QThread() -{ - this->qesp = qesp; - terminate = false; -} - -void Win_QextSerialThread::stop() -{ - terminate = true; - qesp->terminateCommWait(); -} - -void Win_QextSerialThread::run() -{ - while (!terminate) - qesp->monitorCommEvent(); - terminate = false; -} +//#include +//#include +//#include +//#include +#include +#include "win_qextserialport.h" + + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort() +Default constructor. Note that the name of the device used by a Win_QextSerialPort constructed +with this constructor will be determined by #defined constants, or lack thereof - the default +behavior is the same as _TTY_LINUX_. Possible naming conventions and their associated constants +are: + +\verbatim + +Constant Used By Naming Convention +---------- ------------- ------------------------ +_TTY_WIN_ Windows COM1, COM2 +_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 +_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 +_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb +_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 +_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 +_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 + Linux /dev/ttyS0, /dev/ttyS1 +\endverbatim + +This constructor associates the object with the first port on the system, e.g. COM1 for Windows +platforms. See the other constructor if you need a port other than the first. +*/ +Win_QextSerialPort::Win_QextSerialPort(QextSerialBase::QueryMode mode): + QextSerialBase() +{ + Win_Handle=INVALID_HANDLE_VALUE; + setQueryMode(mode); + init(); +} + +/*!Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort&) +Copy constructor. +*/ +Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort& s): + QextSerialBase(s.port) +{ + Win_Handle=INVALID_HANDLE_VALUE; + _queryMode = s._queryMode; + _bytesToWrite = s._bytesToWrite; + bytesToWriteLock = new QReadWriteLock; + overlapThread = new Win_QextSerialThread(this); + memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); + memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); + setOpenMode(s.openMode()); + lastErr=s.lastErr; + port = s.port; + Settings.FlowControl=s.Settings.FlowControl; + Settings.Parity=s.Settings.Parity; + Settings.DataBits=s.Settings.DataBits; + Settings.StopBits=s.Settings.StopBits; + Settings.BaudRate=s.Settings.BaudRate; + Win_Handle=s.Win_Handle; + memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); + memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); + if (s.overlapThread->isRunning()) + overlapThread->start(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name) +Constructs a serial port attached to the port specified by devName. +devName is the name of the device, which is windowsystem-specific, +e.g."COM2" or "/dev/ttyS0". +*/ +Win_QextSerialPort::Win_QextSerialPort(const QString & name, QextSerialBase::QueryMode mode): + QextSerialBase(name) +{ + Win_Handle=INVALID_HANDLE_VALUE; + setQueryMode(mode); + init(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings) +Constructs a port with default name and specified settings. +*/ +Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings, QextSerialBase::QueryMode mode): + QextSerialBase() +{ + Win_Handle=INVALID_HANDLE_VALUE; + setBaudRate(settings.BaudRate); + setDataBits(settings.DataBits); + setStopBits(settings.StopBits); + setParity(settings.Parity); + setFlowControl(settings.FlowControl); + setTimeout(settings.Timeout_Millisec); + setQueryMode(mode); + init(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings) +Constructs a port with specified name and settings. +*/ +Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings, QextSerialBase::QueryMode mode): + QextSerialBase(name) +{ + Win_Handle=INVALID_HANDLE_VALUE; + setPortName(name); + setBaudRate(settings.BaudRate); + setDataBits(settings.DataBits); + setStopBits(settings.StopBits); + setParity(settings.Parity); + setFlowControl(settings.FlowControl); + setTimeout(settings.Timeout_Millisec); + setQueryMode(mode); + init(); +} + +void Win_QextSerialPort::init() +{ + _bytesToWrite = 0; + overlap.Internal = 0; + overlap.InternalHigh = 0; + overlap.Offset = 0; + overlap.OffsetHigh = 0; + overlap.hEvent = CreateEvent(NULL, true, false, NULL); + overlapThread = new Win_QextSerialThread(this); + bytesToWriteLock = new QReadWriteLock; +} + +/*! +\fn Win_QextSerialPort::~Win_QextSerialPort() +Standard destructor. +*/ +Win_QextSerialPort::~Win_QextSerialPort() { + if (isOpen()) { + close(); + } + CloseHandle(overlap.hEvent); + delete overlapThread; + delete bytesToWriteLock; +} + +/*! +\fn Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) +overrides the = operator +*/ +Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) { + setOpenMode(s.openMode()); + _queryMode = s._queryMode; + _bytesToWrite = s._bytesToWrite; + bytesToWriteLock = new QReadWriteLock; + overlapThread = new Win_QextSerialThread(this); + memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); + memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); + lastErr=s.lastErr; + port = s.port; + Settings.FlowControl=s.Settings.FlowControl; + Settings.Parity=s.Settings.Parity; + Settings.DataBits=s.Settings.DataBits; + Settings.StopBits=s.Settings.StopBits; + Settings.BaudRate=s.Settings.BaudRate; + Win_Handle=s.Win_Handle; + memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); + memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); + if (s.overlapThread->isRunning()) + overlapThread->start(); + return *this; +} + + +/*! +\fn bool Win_QextSerialPort::open(OpenMode mode) +Opens a serial port. Note that this function does not specify which device to open. If you need +to open a device by name, see Win_QextSerialPort::open(const char*). This function has no effect +if the port associated with the class is already open. The port is also configured to the current +settings, as stored in the Settings structure. +*/ +bool Win_QextSerialPort::open(OpenMode mode) { + unsigned long confSize = sizeof(COMMCONFIG); + Win_CommConfig.dwSize = confSize; + DWORD dwFlagsAndAttributes = 0; + if (queryMode() == QextSerialBase::EventDriven) + dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; + + LOCK_MUTEX(); + if (mode == QIODevice::NotOpen) + return isOpen(); + if (!isOpen()) { + /*open the port*/ + Win_Handle=CreateFileA(port.toAscii(), GENERIC_READ|GENERIC_WRITE, + FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); + if (Win_Handle!=INVALID_HANDLE_VALUE) { + /*configure port settings*/ + GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); + GetCommState(Win_Handle, &(Win_CommConfig.dcb)); + + /*set up parameters*/ + Win_CommConfig.dcb.fBinary=TRUE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + Win_CommConfig.dcb.fAbortOnError=FALSE; + Win_CommConfig.dcb.fNull=FALSE; + setBaudRate(Settings.BaudRate); + setDataBits(Settings.DataBits); + setStopBits(Settings.StopBits); + setParity(Settings.Parity); + setFlowControl(Settings.FlowControl); + setTimeout(Settings.Timeout_Millisec); + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + + //init event driven approach + if (queryMode() == QextSerialBase::EventDriven) { + Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + if (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { + qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); + UNLOCK_MUTEX(); + return false; + } + overlapThread->start(); + } + QIODevice::open(mode); + } + } else { + UNLOCK_MUTEX(); + return false; + } + UNLOCK_MUTEX(); + return isOpen(); +} + +/*! +\fn void Win_QextSerialPort::close() +Closes a serial port. This function has no effect if the serial port associated with the class +is not currently open. +*/ +void Win_QextSerialPort::close() +{ + LOCK_MUTEX(); + + if (isOpen()) { + flush(); + if (overlapThread->isRunning()) { + overlapThread->stop(); + if (QThread::currentThread() != overlapThread) + overlapThread->wait(); + } + if (CloseHandle(Win_Handle)) + Win_Handle = INVALID_HANDLE_VALUE; + _bytesToWrite = 0; + QIODevice::close(); + } + + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::flush() +Flushes all pending I/O to the serial port. This function has no effect if the serial port +associated with the class is not currently open. +*/ +void Win_QextSerialPort::flush() { + LOCK_MUTEX(); + if (isOpen()) { + FlushFileBuffers(Win_Handle); + } + UNLOCK_MUTEX(); +} + +/*! +\fn qint64 Win_QextSerialPort::size() const +This function will return the number of bytes waiting in the receive queue of the serial port. +It is included primarily to provide a complete QIODevice interface, and will not record errors +in the lastErr member (because it is const). This function is also not thread-safe - in +multithreading situations, use Win_QextSerialPort::bytesAvailable() instead. +*/ +qint64 Win_QextSerialPort::size() const { + int availBytes; + COMSTAT Win_ComStat; + DWORD Win_ErrorMask=0; + ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); + availBytes = Win_ComStat.cbInQue; + return (qint64)availBytes; +} + +/*! +\fn qint64 Win_QextSerialPort::bytesAvailable() +Returns the number of bytes waiting in the port's receive queue. This function will return 0 if +the port is not currently open, or -1 on error. +*/ +qint64 Win_QextSerialPort::bytesAvailable() const { + LOCK_MUTEX(); + if (isOpen()) { + DWORD Errors; + COMSTAT Status; + if (ClearCommError(Win_Handle, &Errors, &Status)) { + UNLOCK_MUTEX(); + return Status.cbInQue + QIODevice::bytesAvailable(); + } + UNLOCK_MUTEX(); + return (qint64)-1; + } + UNLOCK_MUTEX(); + return 0; +} + +/*! +\fn void Win_QextSerialPort::translateError(ulong error) +Translates a system-specific error code to a QextSerialPort error code. Used internally. +*/ +void Win_QextSerialPort::translateError(ulong error) { + if (error&CE_BREAK) { + lastErr=E_BREAK_CONDITION; + } + else if (error&CE_FRAME) { + lastErr=E_FRAMING_ERROR; + } + else if (error&CE_IOE) { + lastErr=E_IO_ERROR; + } + else if (error&CE_MODE) { + lastErr=E_INVALID_FD; + } + else if (error&CE_OVERRUN) { + lastErr=E_BUFFER_OVERRUN; + } + else if (error&CE_RXPARITY) { + lastErr=E_RECEIVE_PARITY_ERROR; + } + else if (error&CE_RXOVER) { + lastErr=E_RECEIVE_OVERFLOW; + } + else if (error&CE_TXFULL) { + lastErr=E_TRANSMIT_OVERFLOW; + } +} + +/*! +\fn qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) +Reads a block of data from the serial port. This function will read at most maxlen bytes from +the serial port and place them in the buffer pointed to by data. Return value is the number of +bytes actually read, or -1 on error. + +\warning before calling this function ensure that serial port associated with this class +is currently open (use isOpen() function to check if port is open). +*/ +qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) +{ + DWORD retVal; + + LOCK_MUTEX(); + + retVal = 0; + if (queryMode() == QextSerialBase::EventDriven) { + OVERLAPPED overlapRead; + overlapRead.Internal = 0; + overlapRead.InternalHigh = 0; + overlapRead.Offset = 0; + overlapRead.OffsetHigh = 0; + overlapRead.hEvent = CreateEvent(NULL, true, false, NULL); + if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { + if (GetLastError() == ERROR_IO_PENDING) + GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); + else { + lastErr = E_READ_FAILED; + retVal = (DWORD)-1; + } + } + CloseHandle(overlapRead.hEvent); + } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + lastErr = E_READ_FAILED; + retVal = (DWORD)-1; + } + + UNLOCK_MUTEX(); + + return (qint64)retVal; +} + +/*! +\fn qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) +Writes a block of data to the serial port. This function will write len bytes +from the buffer pointed to by data to the serial port. Return value is the number +of bytes actually written, or -1 on error. + +\warning before calling this function ensure that serial port associated with this class +is currently open (use isOpen() function to check if port is open). +*/ +qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) +{ + DWORD retVal; + + LOCK_MUTEX(); + + retVal = 0; + if (queryMode() == QextSerialBase::EventDriven) { + bytesToWriteLock->lockForWrite(); + _bytesToWrite += maxSize; + bytesToWriteLock->unlock(); + overlapWrite.Internal = 0; + overlapWrite.InternalHigh = 0; + overlapWrite.Offset = 0; + overlapWrite.OffsetHigh = 0; + overlapWrite.hEvent = CreateEvent(NULL, true, false, NULL); + if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapWrite)) { + lastErr = E_WRITE_FAILED; + retVal = (DWORD)-1; + } else + retVal = maxSize; + } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + lastErr = E_WRITE_FAILED; + retVal = (DWORD)-1; + } + + UNLOCK_MUTEX(); + + return (qint64)retVal; +} + +/*! +\fn void Win_QextSerialPort::ungetChar(char c) +This function is included to implement the full QIODevice interface, and currently has no +purpose within this class. This function is meaningless on an unbuffered device and currently +only prints a warning message to that effect. +*/ +void Win_QextSerialPort::ungetChar(char c) { + + /*meaningless on unbuffered sequential device - return error and print a warning*/ + TTY_WARNING("Win_QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); +} + +/*! +\fn void Win_QextSerialPort::setFlowControl(FlowType flow) +Sets the flow control used by the port. Possible values of flow are: +\verbatim + FLOW_OFF No flow control + FLOW_HARDWARE Hardware (RTS/CTS) flow control + FLOW_XONXOFF Software (XON/XOFF) flow control +\endverbatim +*/ +void Win_QextSerialPort::setFlowControl(FlowType flow) { + LOCK_MUTEX(); + if (Settings.FlowControl!=flow) { + Settings.FlowControl=flow; + } + if (isOpen()) { + switch(flow) { + + /*no flow control*/ + case FLOW_OFF: + Win_CommConfig.dcb.fOutxCtsFlow=FALSE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; + + /*software (XON/XOFF) flow control*/ + case FLOW_XONXOFF: + Win_CommConfig.dcb.fOutxCtsFlow=FALSE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX=TRUE; + Win_CommConfig.dcb.fOutX=TRUE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; + + case FLOW_HARDWARE: + Win_CommConfig.dcb.fOutxCtsFlow=TRUE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setParity(ParityType parity) +Sets the parity associated with the serial port. The possible values of parity are: +\verbatim + PAR_SPACE Space Parity + PAR_MARK Mark Parity + PAR_NONE No Parity + PAR_EVEN Even Parity + PAR_ODD Odd Parity +\endverbatim +*/ +void Win_QextSerialPort::setParity(ParityType parity) { + LOCK_MUTEX(); + if (Settings.Parity!=parity) { + Settings.Parity=parity; + } + if (isOpen()) { + Win_CommConfig.dcb.Parity=(unsigned char)parity; + switch (parity) { + + /*space parity*/ + case PAR_SPACE: + if (Settings.DataBits==DATA_8) { + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); + } + Win_CommConfig.dcb.fParity=TRUE; + break; + + /*mark parity - WINDOWS ONLY*/ + case PAR_MARK: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); + Win_CommConfig.dcb.fParity=TRUE; + break; + + /*no parity*/ + case PAR_NONE: + Win_CommConfig.dcb.fParity=FALSE; + break; + + /*even parity*/ + case PAR_EVEN: + Win_CommConfig.dcb.fParity=TRUE; + break; + + /*odd parity*/ + case PAR_ODD: + Win_CommConfig.dcb.fParity=TRUE; + break; + } + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setDataBits(DataBitsType dataBits) +Sets the number of data bits used by the serial port. Possible values of dataBits are: +\verbatim + DATA_5 5 data bits + DATA_6 6 data bits + DATA_7 7 data bits + DATA_8 8 data bits +\endverbatim + +\note +This function is subject to the following restrictions: +\par + 5 data bits cannot be used with 2 stop bits. +\par + 1.5 stop bits can only be used with 5 data bits. +\par + 8 data bits cannot be used with space parity on POSIX systems. + +*/ +void Win_QextSerialPort::setDataBits(DataBitsType dataBits) { + LOCK_MUTEX(); + if (Settings.DataBits!=dataBits) { + if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || + (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { + } + else { + Settings.DataBits=dataBits; + } + } + if (isOpen()) { + switch(dataBits) { + + /*5 data bits*/ + case DATA_5: + if (Settings.StopBits==STOP_2) { + TTY_WARNING("Win_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=5; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + + /*6 data bits*/ + case DATA_6: + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=6; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + + /*7 data bits*/ + case DATA_7: + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=7; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + + /*8 data bits*/ + case DATA_8: + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=8; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setStopBits(StopBitsType stopBits) +Sets the number of stop bits used by the serial port. Possible values of stopBits are: +\verbatim + STOP_1 1 stop bit + STOP_1_5 1.5 stop bits + STOP_2 2 stop bits +\endverbatim + +\note +This function is subject to the following restrictions: +\par + 2 stop bits cannot be used with 5 data bits. +\par + 1.5 stop bits cannot be used with 6 or more data bits. +\par + POSIX does not support 1.5 stop bits. +*/ +void Win_QextSerialPort::setStopBits(StopBitsType stopBits) { + LOCK_MUTEX(); + if (Settings.StopBits!=stopBits) { + if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || + (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { + } + else { + Settings.StopBits=stopBits; + } + } + if (isOpen()) { + switch (stopBits) { + + /*one stop bit*/ + case STOP_1: + Win_CommConfig.dcb.StopBits=ONESTOPBIT; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; + + /*1.5 stop bits*/ + case STOP_1_5: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); + if (Settings.DataBits!=DATA_5) { + TTY_WARNING("Win_QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); + } + else { + Win_CommConfig.dcb.StopBits=ONE5STOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + + /*two stop bits*/ + case STOP_2: + if (Settings.DataBits==DATA_5) { + TTY_WARNING("Win_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } + else { + Win_CommConfig.dcb.StopBits=TWOSTOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) +Sets the baud rate of the serial port. Note that not all rates are applicable on +all platforms. The following table shows translations of the various baud rate +constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * +are speeds that are usable on both Windows and POSIX. +\verbatim + + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- + BAUD50 110 50 + BAUD75 110 75 + *BAUD110 110 110 + BAUD134 110 134.5 + BAUD150 110 150 + BAUD200 110 200 + *BAUD300 300 300 + *BAUD600 600 600 + *BAUD1200 1200 1200 + BAUD1800 1200 1800 + *BAUD2400 2400 2400 + *BAUD4800 4800 4800 + *BAUD9600 9600 9600 + BAUD14400 14400 9600 + *BAUD19200 19200 19200 + *BAUD38400 38400 38400 + BAUD56000 56000 38400 + *BAUD57600 57600 57600 + BAUD76800 57600 76800 + *BAUD115200 115200 115200 + BAUD128000 128000 115200 + BAUD256000 256000 115200 + BAUD230400 230400 115200 + BAUD256000 256000 115200 + BAUD460800 460800 115200 + BAUD921600 921600 115200 +\endverbatim +*/ +void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) { + LOCK_MUTEX(); + if (Settings.BaudRate!=baudRate) { + switch (baudRate) { + case BAUD50: + case BAUD75: + case BAUD134: + case BAUD150: + case BAUD200: + Settings.BaudRate=BAUD110; + break; + + case BAUD1800: + Settings.BaudRate=BAUD1200; + break; + + case BAUD76800: + Settings.BaudRate=BAUD57600; + break; + + default: + Settings.BaudRate=baudRate; + break; + } + } + if (isOpen()) { + switch (baudRate) { + + /*50 baud*/ + case BAUD50: + TTY_WARNING("Win_QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*75 baud*/ + case BAUD75: + TTY_WARNING("Win_QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*110 baud*/ + case BAUD110: + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*134.5 baud*/ + case BAUD134: + TTY_WARNING("Win_QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*150 baud*/ + case BAUD150: + TTY_WARNING("Win_QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*200 baud*/ + case BAUD200: + TTY_WARNING("Win_QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; + + /*300 baud*/ + case BAUD300: + Win_CommConfig.dcb.BaudRate=CBR_300; + break; + + /*600 baud*/ + case BAUD600: + Win_CommConfig.dcb.BaudRate=CBR_600; + break; + + /*1200 baud*/ + case BAUD1200: + Win_CommConfig.dcb.BaudRate=CBR_1200; + break; + + /*1800 baud*/ + case BAUD1800: + TTY_WARNING("Win_QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); + Win_CommConfig.dcb.BaudRate=CBR_1200; + break; + + /*2400 baud*/ + case BAUD2400: + Win_CommConfig.dcb.BaudRate=CBR_2400; + break; + + /*4800 baud*/ + case BAUD4800: + Win_CommConfig.dcb.BaudRate=CBR_4800; + break; + + /*9600 baud*/ + case BAUD9600: + Win_CommConfig.dcb.BaudRate=CBR_9600; + break; + + /*14400 baud*/ + case BAUD14400: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_14400; + break; + + /*19200 baud*/ + case BAUD19200: + Win_CommConfig.dcb.BaudRate=CBR_19200; + break; + + /*38400 baud*/ + case BAUD38400: + Win_CommConfig.dcb.BaudRate=CBR_38400; + break; + + /*56000 baud*/ + case BAUD56000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_56000; + break; + + /*57600 baud*/ + case BAUD57600: + Win_CommConfig.dcb.BaudRate=CBR_57600; + break; + + /*76800 baud*/ + case BAUD76800: + TTY_WARNING("Win_QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); + Win_CommConfig.dcb.BaudRate=CBR_57600; + break; + + /*115200 baud*/ + case BAUD115200: + Win_CommConfig.dcb.BaudRate=CBR_115200; + break; + + /*128000 baud*/ + case BAUD128000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_128000; + break; + + /*256000 baud*/ + case BAUD256000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_256000; + break; + /*230400 baud*/ + case BAUD230400: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 230400 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_230400; + break; + /*460800 baud*/ + case BAUD460800: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 460800 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_460800; + break; + /*921600 baud*/ + case BAUD921600: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 921600 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_921600; + break; + } + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setDtr(bool set) +Sets DTR line to the requested state (high by default). This function will have no effect if +the port associated with the class is not currently open. +*/ +void Win_QextSerialPort::setDtr(bool set) { + LOCK_MUTEX(); + if (isOpen()) { + if (set) { + EscapeCommFunction(Win_Handle, SETDTR); + } + else { + EscapeCommFunction(Win_Handle, CLRDTR); + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setRts(bool set) +Sets RTS line to the requested state (high by default). This function will have no effect if +the port associated with the class is not currently open. +*/ +void Win_QextSerialPort::setRts(bool set) { + LOCK_MUTEX(); + if (isOpen()) { + if (set) { + EscapeCommFunction(Win_Handle, SETRTS); + } + else { + EscapeCommFunction(Win_Handle, CLRRTS); + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn ulong Win_QextSerialPort::lineStatus(void) +returns the line status as stored by the port function. This function will retrieve the states +of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines +can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned +long with specific bits indicating which lines are high. The following constants should be used +to examine the states of individual lines: + +\verbatim +Mask Line +------ ---- +LS_CTS CTS +LS_DSR DSR +LS_DCD DCD +LS_RI RI +\endverbatim + +This function will return 0 if the port associated with the class is not currently open. +*/ +ulong Win_QextSerialPort::lineStatus(void) { + unsigned long Status=0, Temp=0; + LOCK_MUTEX(); + if (isOpen()) { + GetCommModemStatus(Win_Handle, &Temp); + if (Temp&MS_CTS_ON) { + Status|=LS_CTS; + } + if (Temp&MS_DSR_ON) { + Status|=LS_DSR; + } + if (Temp&MS_RING_ON) { + Status|=LS_RI; + } + if (Temp&MS_RLSD_ON) { + Status|=LS_DCD; + } + } + UNLOCK_MUTEX(); + return Status; +} + +bool Win_QextSerialPort::waitForReadyRead(int msecs) +{ + //@todo implement + return false; +} + +qint64 Win_QextSerialPort::bytesToWrite() const +{ + return _bytesToWrite; +} + +void Win_QextSerialPort::monitorCommEvent() +{ + DWORD eventMask = 0; + + ResetEvent(overlap.hEvent); + if (!WaitCommEvent(Win_Handle, & eventMask, & overlap)) + if (GetLastError() != ERROR_IO_PENDING) + qCritical("WaitCommEvent error %ld\n", GetLastError()); + + if (WaitForSingleObject(overlap.hEvent, INFINITE) == WAIT_OBJECT_0) { + //overlap event occured + DWORD undefined; + if (!GetOverlappedResult(Win_Handle, & overlap, & undefined, false)) { + qWarning("CommEvent overlapped error %ld", GetLastError()); + return; + } + if (eventMask & EV_RXCHAR) { + if (sender() != this) + emit readyRead(); + } + if (eventMask & EV_TXEMPTY) { + DWORD numBytes; + GetOverlappedResult(Win_Handle, & overlapWrite, & numBytes, true); + bytesToWriteLock->lockForWrite(); + if (sender() != this) + emit bytesWritten(bytesToWrite()); + _bytesToWrite = 0; + bytesToWriteLock->unlock(); + } + if (eventMask & EV_DSR){ + if (lineStatus() & LS_DSR) + emit dsrChanged(true); + } + else{ + emit dsrChanged(false); + } + } +} + +void Win_QextSerialPort::terminateCommWait() +{ + SetCommMask(Win_Handle, 0); +} + + +/*! +\fn void Win_QextSerialPort::setTimeout(ulong millisec); +Sets the read and write timeouts for the port to millisec milliseconds. +Setting 0 indicates that timeouts are not used for read nor write operations; +however read() and write() functions will still block. Set -1 to provide +non-blocking behaviour (read() and write() will return immediately). + +\note this function does nothing in event driven mode. +*/ +void Win_QextSerialPort::setTimeout(long millisec) { + LOCK_MUTEX(); + Settings.Timeout_Millisec = millisec; + + if (millisec == -1) { + Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + } else { + Win_CommTimeouts.ReadIntervalTimeout = millisec; + Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; + } + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + if (queryMode() != QextSerialBase::EventDriven) + SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + + UNLOCK_MUTEX(); +} + + +Win_QextSerialThread::Win_QextSerialThread(Win_QextSerialPort * qesp): + QThread() +{ + this->qesp = qesp; + terminate = false; +} + +void Win_QextSerialThread::stop() +{ + terminate = true; + qesp->terminateCommWait(); +} + +void Win_QextSerialThread::run() +{ + while (!terminate) + qesp->monitorCommEvent(); + terminate = false; +} diff --git a/src/main.cc b/src/main.cc index a27a8d303027496e62beeebacdf9656dc420cce9..ba6d1af2df48400edd6b96cafd3f3ec17626ab62 100644 --- a/src/main.cc +++ b/src/main.cc @@ -39,6 +39,21 @@ This file is part of the QGROUNDCONTROL project #undef main #endif + +// Install a message handler so you do not need +// the MSFT debug tools installed to se +// qDebug(), qWarning(), qCritical and qAbort +#ifdef Q_OS_WIN +void msgHandler( QtMsgType type, const char* msg ) +{ + const char symbols[] = { 'I', 'E', '!', 'X' }; + QString output = QString("[%1] %2").arg( symbols[type] ).arg( msg ); + std::cerr << output.toStdString() << std::endl; + if( type == QtFatalMsg ) abort(); +} + +#endif + /** * @brief Starts the application * @@ -46,9 +61,15 @@ This file is part of the QGROUNDCONTROL project * @param argv Commandline arguments * @return exit code, 0 for normal exit and !=0 for error cases */ + int main(int argc, char *argv[]) { +// install the message handler +#ifdef Q_OS_WIN + qInstallMsgHandler( msgHandler ); +#endif + Core core(argc, argv); return core.exec(); } diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 775819d188721ba96ac6855a403692f139cf10c0..84fff57c0c7d5b3511251383ff7b6939a7b10b36 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -3,50 +3,39 @@ #include SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : - UAS(mavlink, id)//, - // Place other initializers here + UAS(mavlink, id) { - widgetTimer = new QTimer (this); - widgetTimer->setInterval(SLUGS_UPDATE_RATE); - - connect (widgetTimer, SIGNAL(timeout()), - this, SLOT(emitSignals())); - - widgetTimer->start(); - - // clear all the state structures - memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t)); - -#ifdef MAVLINK_ENABLED_SLUGS - - - 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)); - - - updateRoundRobin = 0; - - - - uasId = id; - - #endif + widgetTimer = new QTimer (this); + widgetTimer->setInterval(SLUGS_UPDATE_RATE); + + connect (widgetTimer, SIGNAL(timeout()), this, SLOT(emitSignals())); + widgetTimer->start(); + memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t));// clear all the state structures + + #ifdef MAVLINK_ENABLED_SLUGS + + + 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(&mlBoot ,0, sizeof(mavlink_boot_t)); + memset(&mlGpsDateTime ,0, sizeof(mavlink_gps_date_time_t)); + memset(&mlApMode ,0, sizeof(mavlink_set_mode_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)); + + memset(&mlScaled ,0, sizeof(mavlink_scaled_imu_t)); + memset(&mlServo ,0, sizeof(mavlink_servo_output_raw_t)); + memset(&mlChannels ,0, sizeof(mavlink_rc_channels_raw_t)); + + updateRoundRobin = 0; + uasId = id; + #endif } /** @@ -59,100 +48,110 @@ 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);// Let UAS handle the default message set if (message.sysid == uasId) { -#ifdef MAVLINK_ENABLED_SLUGS - // Handle your special messages mavlink_message_t* msg = &message; - switch (message.msgid) - { - - - - case MAVLINK_MSG_ID_RAW_IMU: - mavlink_msg_raw_imu_decode(&message, &mlRawImuData); - break; - - case MAVLINK_MSG_ID_BOOT: - mavlink_msg_boot_decode(&message,&mlBoot); - emit slugsBootMsg(uasId, mlBoot); - break; - - 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_ACTION_ACK: // 62 - mavlink_msg_action_ack_decode(&message,&mlActionAck); - break; - - case MAVLINK_MSG_ID_CPU_LOAD: //170 - mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData); - break; - - case MAVLINK_MSG_ID_AIR_DATA: //171 - mavlink_msg_air_data_decode(&message,&mlAirData); - break; - - 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: //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 = "< - -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 . - -======================================================================*/ - -#ifndef SLUGSMAV_H -#define SLUGSMAV_H - -#include "UAS.h" -#include "mavlink.h" -#include - -#define SLUGS_UPDATE_RATE 100 // in ms -class SlugsMAV : public UAS -{ - Q_OBJECT - Q_INTERFACES(UASInterface) -public: - SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); - -public slots: - /** @brief Receive a MAVLink message from this MAV */ - void receiveMessage(LinkInterface* link, mavlink_message_t message); - - void emitSignals (void); - -#ifdef MAVLINK_ENABLED_SLUGS - mavlink_pwm_commands_t* getPwmCommands(); -#endif - -signals: - - void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData); - void slugsGPSCogSog(int uasId, double cog, double sog); - -#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 +/*===================================================================== + +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 . + +======================================================================*/ + +#ifndef SLUGSMAV_H +#define SLUGSMAV_H + +#include "UAS.h" +#include "mavlink.h" +#include + +#define SLUGS_UPDATE_RATE 200 // in ms +class SlugsMAV : public UAS +{ + Q_OBJECT + Q_INTERFACES(UASInterface) + + enum SLUGS_ACTION { + SLUGS_ACTION_NONE, + SLUGS_ACTION_SUCCESS, + SLUGS_ACTION_FAIL, + SLUGS_ACTION_EEPROM, + SLUGS_ACTION_MODE_CHANGE, + SLUGS_ACTION_MODE_REPORT, + SLUGS_ACTION_PT_CHANGE, + SLUGS_ACTION_PT_REPORT, + SLUGS_ACTION_PID_CHANGE, + SLUGS_ACTION_PID_REPORT, + SLUGS_ACTION_WP_CHANGE, + SLUGS_ACTION_WP_REPORT, + SLUGS_ACTION_MLC_CHANGE, + SLUGS_ACTION_MLC_REPORT + }; + + +public: + SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); + +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); + void slugsGPSCogSog(int uasId, double cog, double sog); + +#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 slugsNavegation(int systemId, const mavlink_slugs_navigation_t& slugsNavigation); + void slugsDataLog(int systemId, const mavlink_data_log_t& dataLog); + void slugsGPSDateTime(int systemId, const mavlink_gps_date_time_t& gpsDateTime); + void slugsActionAck(int systemId, const mavlink_action_ack_t& actionAck); + + void slugsBootMsg(int uasId, mavlink_boot_t& boot); + void slugsAttitude(int uasId, mavlink_attitude_t& attitude); + + void slugsScaled(int uasId, const mavlink_scaled_imu_t& scaled); + void slugsServo(int uasId, const mavlink_servo_output_raw_t& servo); + void slugsChannels(int uasId, const mavlink_rc_channels_raw_t& channels); + +#endif + +protected: + 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_boot_t mlBoot; + mavlink_gps_date_time_t mlGpsDateTime; + mavlink_mid_lvl_cmds_t mlMidLevelCommands; + mavlink_set_mode_t mlApMode; + + 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; + + mavlink_scaled_imu_t mlScaled; + mavlink_servo_output_raw_t mlServo; + mavlink_rc_channels_raw_t mlChannels; + + + // 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 2c8030643c06f53315450ab0b1b30ee59806780e..6e3d4f2136e13ab1cf84a31ca46c91949899e02f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -244,13 +244,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_sys_status_decode(&message, &state); // FIXME - //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode; + //qDebug() << "1 SYSTEM STATUS:" << state.status; QString audiostring = "System " + getUASName(); QString stateAudio = ""; QString modeAudio = ""; bool statechanged = false; - bool modechanged = false; + bool modechanged = false; if (state.status != this->status) { @@ -259,6 +259,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) getStatusForCode((int)state.status, uasState, stateDescription); emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); + stateAudio = " changed status to " + uasState; } @@ -285,24 +286,44 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) case (uint8_t)MAV_MODE_MANUAL: mode = "MANUAL MODE"; break; + + #ifdef MAVLINK_ENABLED_SLUGS + case (uint8_t)MAV_MODE_AUTO: + mode = "WAYPOINT MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + mode = "MID-L CMDS MODE"; + break; + + case (uint8_t)MAV_MODE_TEST1: + mode = "PASST MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + mode = "SEL PT MODE"; + break; + #else case (uint8_t)MAV_MODE_AUTO: mode = "AUTO MODE"; break; case (uint8_t)MAV_MODE_GUIDED: mode = "GUIDED MODE"; break; - case (uint8_t)MAV_MODE_READY: - mode = "READY MODE"; - break; + case (uint8_t)MAV_MODE_TEST1: mode = "TEST1 MODE"; break; case (uint8_t)MAV_MODE_TEST2: mode = "TEST2 MODE"; break; + #endif + case (uint8_t)MAV_MODE_READY: + mode = "READY MODE"; + break; + case (uint8_t)MAV_MODE_TEST3: mode = "TEST3 MODE"; - break; + break; + case (uint8_t)MAV_MODE_RC_TRAINING: mode = "RC TRAINING MODE"; break; @@ -312,6 +333,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit modeChanged(this->getUASID(), mode, ""); + + //qDebug() << "2 SYSTEM MODE:" << mode; + modeAudio = " is now in " + mode; } currentVoltage = state.vbat/1000.0f; @@ -338,6 +362,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // COMMUNICATIONS DROP RATE emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f); + + + //add for development + //emit remoteControlRSSIChanged(state.packet_drop/1000.0f); + + //float en = state.packet_drop/1000.0f; + //emit remoteControlChannelRawChanged(0, en);//MAVLINK_MSG_ID_RC_CHANNELS_RAW + //emit remoteControlChannelScaledChanged(0, en/100.0f);//MAVLINK_MSG_ID_RC_CHANNELS_SCALED + + //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop; // AUDIO @@ -428,11 +462,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixTime(attitude.usec); - roll = QGC::limitAngleToPMPIf(attitude.roll); pitch = QGC::limitAngleToPMPIf(attitude.pitch); yaw = QGC::limitAngleToPMPIf(attitude.yaw); - emit valueChanged(uasId, "roll", "rad", roll, time); emit valueChanged(uasId, "pitch", "rad", pitch, time); emit valueChanged(uasId, "yaw", "rad", yaw, time); @@ -709,12 +741,24 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_raw_pressure_t pressure; mavlink_msg_raw_pressure_decode(&message, &pressure); quint64 time = this->getUnixTime(pressure.usec); + emit valueChanged(uasId, "abs pressure", "raw", pressure.press_abs, time); + emit valueChanged(uasId, "diff pressure 1", "raw", pressure.press_diff1, time); + emit valueChanged(uasId, "diff pressure 2", "raw", pressure.press_diff2, time); + emit valueChanged(uasId, "temperature", "raw", pressure.temperature, time); + } + break; + + case MAVLINK_MSG_ID_SCALED_PRESSURE: + { + mavlink_scaled_pressure_t pressure; + mavlink_msg_scaled_pressure_decode(&message, &pressure); + quint64 time = this->getUnixTime(pressure.usec); emit valueChanged(uasId, "abs pressure", "hPa", pressure.press_abs, time); - emit valueChanged(uasId, "diff pressure 1", "hPa", pressure.press_diff1, time); - emit valueChanged(uasId, "diff pressure 2", "hPa", pressure.press_diff2, time); - emit valueChanged(uasId, "temperature", "deg C", pressure.temperature/100.0f, time); + emit valueChanged(uasId, "diff pressure", "hPa", pressure.press_diff, time); + emit valueChanged(uasId, "temperature", "C", pressure.temperature/100.0, time); } break; + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { mavlink_rc_channels_raw_t channels; @@ -1052,6 +1096,7 @@ void UAS::setHomePosition(double lat, double lon, double alt) home.latitude = lat*1E7; home.longitude = lon*1E7; home.altitude = alt*1000; + qDebug() << "lat:" << home.latitude << " lon:" << home.longitude; mavlink_message_t msg; mavlink_msg_gps_set_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home); sendMessage(msg); @@ -1226,12 +1271,16 @@ void UAS::setMode(int mode) { if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) { - this->mode = mode; + //this->mode = mode; //no call assignament, update receive message from UAS mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); sendMessage(msg); qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; } + else + { + qDebug() << "uas Mode not assign: " << mode; + } } void UAS::sendMessage(mavlink_message_t message) @@ -1369,10 +1418,16 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc uasState = tr("EMERGENCY"); stateDescription = tr("EMERGENCY: Land Immediately!"); break; + case MAV_STATE_HILSIM: + uasState = tr("HIL SIM"); + stateDescription = tr("HIL Simulation, Sensors read from SIM"); + break; + case MAV_STATE_POWEROFF: uasState = tr("SHUTDOWN"); stateDescription = tr("Powering off system."); break; + default: uasState = tr("UNKNOWN"); stateDescription = tr("Unknown system state"); @@ -1965,12 +2020,14 @@ void UAS::clearWaypointList() void UAS::halt() { + mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); + } void UAS::go() @@ -2000,6 +2057,7 @@ void UAS::home() */ void UAS::emergencySTOP() { + mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); @@ -2042,6 +2100,29 @@ bool UAS::emergencyKILL() return result; } +void UAS::startHil(){ + + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_START_HILSIM); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + +} + +void UAS::stopHil(){ + + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_STOP_HILSIM); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + +} + + void UAS::shutdown() { bool result = false; @@ -2049,6 +2130,7 @@ void UAS::shutdown() msgBox.setIcon(QMessageBox::Critical); msgBox.setText("Shutting down the UAS"); msgBox.setInformativeText("Do you want to shut down the onboard computer?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); int ret = msgBox.exec(); @@ -2066,6 +2148,7 @@ void UAS::shutdown() // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); + result = true; } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 37cc722a89418cd0adc1752c8690fa2d8bde46f5..e39531619dd091968aecbe20058bd54f578c5802 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -233,6 +233,13 @@ public slots: void home(); void halt(); void go(); + /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ + void startHil(); + + /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/ + void stopHil(); + + /** @brief Stops the robot system. If it is an MAV, the robot starts the emergency landing procedure **/ void emergencySTOP(); @@ -365,6 +372,9 @@ protected slots: // MESSAGE RECEPTION /** @brief Receive a named value message */ void receiveMessageNamedValue(const mavlink_message_t& message); + +private: +// unsigned int mode; ///< The current mode of the MAV }; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index b245a1edf8b3866d924cff37db460ce0f9c77b09..4451170058623247bf18ae3b2ae558de68c6eefe 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -138,7 +138,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { - //qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_CMD) wp->command; + //qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command; Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); addWaypoint(lwp, false); @@ -351,6 +351,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq) { waypoints[i]->setId(i); } + emit waypointListChanged(); emit waypointListChanged(uas.getUASID()); return 0; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 89dc402e0e68247ba30d1f255dddfedcec83e360..3a8b9ed2afa9ebe98c22f3bbd3033fd6a7ee4620 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -393,29 +393,13 @@ void MainWindow::buildCommonWidgets() addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); } -#ifdef MAVLINK_ENABLED_SLUGS - //TODO temporaly debug - if (!slugsHilSimWidget) - { - slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); - slugsHilSimWidget->setWidget( new SlugsHilSim(this)); - addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); - } - - //TODO temporaly debug - if (!slugsCamControlWidget) - { - slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); - slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); - addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget(bool)), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); - } -#endif - if (!dataplotWidget) { dataplotWidget = new QGCDataPlot2D(this); addToCentralWidgetsMenu (dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); } + + } @@ -615,6 +599,15 @@ void MainWindow::buildSlugsWidgets() addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); } +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (!gEarthWidget) + { + gEarthWidget = new QGCGoogleEarthView(this); + addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + } + +#endif + if (!slugsDataWidget) { // Dialog widgets @@ -624,13 +617,6 @@ void MainWindow::buildSlugsWidgets() addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget(bool)), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); } - if (!slugsPIDControlWidget) - { - slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); - slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); - slugsPIDControlWidget->setObjectName("SLUGS_PID_CONTROL_DOCK_WIDGET"); - addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); - } if (!slugsHilSimWidget) { @@ -640,13 +626,29 @@ void MainWindow::buildSlugsWidgets() addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget(bool)), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); } + if (!controlParameterWidget){ + controlParameterWidget = new QDockWidget(tr("Control Parameters"), this); + controlParameterWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_PARAMETERWIDGET"); + controlParameterWidget->setWidget( new UASControlParameters(this) ); + addToToolsMenu (controlParameterWidget, tr("Control Parameters"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL_PARAM, Qt::LeftDockWidgetArea); + } + + if (!parametersDockWidget) + { + parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this); + parametersDockWidget->setWidget( new ParameterInterface(this) ); + parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); + addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea); + } + if (!slugsCamControlWidget) { - slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); - slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); + slugsCamControlWidget = new QDockWidget(tr("Camera Control"), this); + slugsCamControlWidget->setWidget(new SlugsPadCameraControl(this)); slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET"); addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget(bool)), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } + } @@ -1013,6 +1015,7 @@ void MainWindow::updateLocationSettings (Qt::DockWidgetArea location) } } + /** * Connect the signals and slots of the common window widgets */ @@ -1027,6 +1030,8 @@ void MainWindow::connectCommonWidgets() if (mapWidget && waypointsDockWidget->widget()) { + // + connect(waypointsDockWidget->widget(), SIGNAL(changePointList()), mapWidget, SLOT(clearWaypoints())); } //TODO temporaly debug @@ -1034,6 +1039,8 @@ void MainWindow::connectCommonWidgets() connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); } + + } void MainWindow::createCustomWidget() @@ -1049,6 +1056,7 @@ void MainWindow::createCustomWidget() QDockWidget* dock = new QDockWidget("Unnamed Tool", this); connect(tool, SIGNAL(destroyed()), dock, SLOT(deleteLater())); dock->setWidget(tool); + QAction* showAction = new QAction("Show Unnamed Tool", this); showAction->setCheckable(true); connect(dock, SIGNAL(visibilityChanged(bool)), showAction, SLOT(setChecked(bool))); @@ -1076,7 +1084,15 @@ void MainWindow::connectSlugsWidgets() slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); } + if (controlParameterWidget && controlParameterWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + controlParameterWidget->widget(), SLOT(activeUasSet(UASInterface*))); + } + if(controlDockWidget && controlParameterWidget) + { + connect(controlDockWidget->widget(), SIGNAL(changedMode(int)), controlParameterWidget->widget(), SLOT(changedMode(int))); + } } void MainWindow::arrangeCommonCenterStack() @@ -1128,6 +1144,10 @@ void MainWindow::arrangeSlugsCenterStack() if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget); +#endif + } void MainWindow::loadSettings() @@ -1706,18 +1726,27 @@ void MainWindow::UASCreated(UASInterface* uas) // Connect Slugs Actions connectSlugsActions(); +// if(slugsDataWidget) +// { +// SlugsDataSensorView *mm = dynamic_cast(slugsDataWidget->widget()); +// if(mm) +// { +// mm->addUAS(uas); +// } +// } + // FIXME: This type checking might be redundant - // if (slugsDataWidget) { - // SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); - // if (dataWidget) { - // SlugsMAV* mav2 = dynamic_cast(uas); - // if (mav2) { - (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); - // //loadSlugsView(); - // loadGlobalOperatorView(); - // } - // } - // } +// // if (slugsDataWidget) { +// // SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); +// // if (dataWidget) { +// // SlugsMAV* mav2 = dynamic_cast(uas); +// // if (mav2) { +// (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); +// // //loadSlugsView(); +// // loadGlobalOperatorView(); +// // } +// // } +// // } } break; @@ -1931,6 +1960,8 @@ void MainWindow::presentView() // UAS CONTROL showTheWidget(MENU_UAS_CONTROL, currentView); + showTheWidget(MENU_UAS_CONTROL_PARAM, currentView); + // UAS LIST showTheWidget(MENU_UAS_LIST, currentView); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index f242f331f41d2cced6db412702c17cf7f5d1c358..aaf6091f061e8864d16f06abfdfd6215c43feca9 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -70,12 +70,10 @@ This file is part of the QGROUNDCONTROL project #include "SlugsDataSensorView.h" #include "LogCompressor.h" -#include "SlugsPIDControl.h" - #include "SlugsHilSim.h" -#include "SlugsVideoCamControl.h" - +#include "SlugsPadCameraControl.h" +#include "UASControlParameters.h" /** * @brief Main Application Window @@ -121,6 +119,7 @@ public slots: void configure(); /** @brief Set the currently controlled UAS */ void setActiveUAS(UASInterface* uas); + /** @brief Add a new UAS */ void UASCreated(UASInterface* uas); /** @brief Update system specs of a UAS */ @@ -211,6 +210,7 @@ protected: // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over // this will be fixed in a future release. typedef enum _TOOLS_WIDGET_NAMES { + MENU_UAS_CONTROL_PARAM, MENU_UAS_CONTROL, MENU_UAS_INFO, MENU_CAMERA, @@ -380,6 +380,7 @@ protected: #endif // Dock widgets QPointer controlDockWidget; + QPointer controlParameterWidget; QPointer infoDockWidget; QPointer cameraDockWidget; QPointer listDockWidget; @@ -400,7 +401,6 @@ protected: QPointer rcViewDockWidget; QPointer hudDockWidget; QPointer slugsDataWidget; - QPointer slugsPIDControlWidget; QPointer slugsHilSimWidget; QPointer slugsCamControlWidget; diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index d1f2c18b3c43c040d0c64082656b61157124a961..3782e19b82a08e2468d582677d3c316468800cd9 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -93,6 +93,8 @@ void MapWidget::init() geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); mc->addLayer(geomLayer); + homePosition = new qmapcontrol::GeometryLayer("Station", mapadapter); + mc->addLayer(homePosition); // @@ -113,6 +115,16 @@ void MapWidget::init() lastZoom = settings.value("LAST_ZOOM", lastZoom).toInt(); settings.endGroup(); + index = 0; + settings.beginGroup("QGC_MAPINDEX"); + index = settings.value("MAP_INDEX", index).toInt(); + settings.endGroup(); + + settings.beginGroup("QGC_HOMEPOSITION"); + homeCoordinate.setY(settings.value("HOME_LATITUDE", homeCoordinate.y()).toDouble()); + homeCoordinate.setX(settings.value("HOME_LONGITUDE", homeCoordinate.x()).toDouble()); + settings.endGroup(); + // SET INITIAL POSITION AND ZOOM // Set default zoom level mc->setZoom(lastZoom); @@ -138,6 +150,14 @@ void MapWidget::init() connect(mapproviderGroup, SIGNAL(triggered(QAction*)), this, SLOT(mapproviderSelected(QAction*))); + + //mapSettings.beginGroup("Map_Widget"); + //QAction *act = new QAction(mapSettings.value("QAction").toString(), this); + //mapproviderSelected(act); + //mapSettings.endGroup(); + + + // Overlay seems currently broken // yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this); // yahooActionOverlay->setCheckable(true); @@ -183,11 +203,17 @@ void MapWidget::init() goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to")); goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to")); + setHome = new QPushButton(QIcon(":/images/actions/go-home.svg"), "", this); + setHome->setStyleSheet(buttonStyle); + setHome->setToolTip(tr("Set home")); + setHome->setStatusTip(tr("Set home")); + zoomin->setMaximumWidth(30); zoomout->setMaximumWidth(30); createPath->setMaximumWidth(30); // clearTracking->setMaximumWidth(30); followgps->setMaximumWidth(30); + setHome->setMaximumWidth(30); goToButton->setMaximumWidth(30); // Set checkable buttons @@ -195,6 +221,7 @@ void MapWidget::init() // create a style and the slots to change the background so it is easier to distinguish followgps->setCheckable(true); createPath->setCheckable(true); + setHome->setCheckable(true); // add buttons to control the map (zoom, GPS tracking and WP capture) QGridLayout* innerlayout = new QGridLayout(mc); @@ -204,6 +231,7 @@ void MapWidget::init() innerlayout->addWidget(zoomout, 1, 0); innerlayout->addWidget(followgps, 2, 0); innerlayout->addWidget(createPath, 3, 0); + innerlayout->addWidget(setHome, 4, 0); //innerlayout->addWidget(clearTracking, 4, 0); // Add spacers to compress buttons on the top left innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); @@ -220,17 +248,6 @@ void MapWidget::init() waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen); mc->layer("Waypoints")->addGeometry(waypointPath); - //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; - // Done set state initialized = true; @@ -262,6 +279,12 @@ void MapWidget::init() connect(createPath, SIGNAL(clicked(bool)), this, SLOT(createPathButtonClicked(bool))); + connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePositionClick(bool))); + + connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*,QPointF)), this, + SLOT(createHomePosition(const QMouseEvent*,QPointF))); + //connect(setHome, SIGNAL(clicked(bool)), this, SLOT(createHomePosition(bool))); + connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)), this, SLOT(captureGeometryClick(Geometry*, QPoint))); @@ -272,10 +295,44 @@ void MapWidget::init() connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)), this, SLOT(captureGeometryEndDrag(Geometry*, QPointF))); + connect(homePosition, SIGNAL(geometryClicked(Geometry*,QPoint)), + this, SLOT(captureGeometryClick(Geometry*, QPoint))); + + connect(homePosition, SIGNAL(geometryDragged(Geometry*, QPointF)), + this, SLOT(captureGeometryDragHome(Geometry*, QPointF))); + + connect(homePosition, SIGNAL(geometryEndDrag(Geometry*, QPointF)), + this, SLOT(captureGeometryEndDrag(Geometry*, QPointF))); + + this->loadSettingsMap(index); + this->createHomePosition(homeCoordinate); + qDebug() << "CHECK END"; } } +void MapWidget::loadSettingsMap(int8_t index) +{ + switch(index) + { + case 0: + mapproviderSelected(osmAction); + break; + case 1: + mapproviderSelected(yahooActionMap); + break; + case 2: + mapproviderSelected(yahooActionSatellite); + break; + case 3: + mapproviderSelected(googleActionMap); + break; + case 4: + mapproviderSelected(googleSatAction); + break; + } +} + void MapWidget::goTo() { bool ok; @@ -316,6 +373,8 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::OSMMapAdapter(); l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); + homePosition->setMapAdapter(mapadapter); + index = 0; if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); @@ -332,6 +391,8 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::YahooMapAdapter(); l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); + homePosition->setMapAdapter(mapadapter); + index = 1; if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); @@ -348,6 +409,8 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1"); l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); + homePosition->setMapAdapter(mapadapter); + index = 2; if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); @@ -361,6 +424,8 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::GoogleMapAdapter(); l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); + homePosition->setMapAdapter(mapadapter); + index = 3; if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); @@ -375,6 +440,8 @@ void MapWidget::mapproviderSelected(QAction* action) mapadapter = new qmapcontrol::GoogleSatMapAdapter(); l->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter); + homePosition->setMapAdapter(mapadapter); + index = 4; if (isVisible()) mc->updateRequestNew(); mc->setZoom(zoom); @@ -430,7 +497,6 @@ void MapWidget::createPathButtonClicked(bool checked) * @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) { if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()) @@ -468,7 +534,6 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina // Refresh the screen if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); } - // emit signal mouse was clicked //emit captureMapCoordinateClick(coordinate); } @@ -692,6 +757,38 @@ void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) mc->setMouseMode(qmapcontrol::MapControl::Panning); } + if (!setHome->isChecked()) + { + mc->setMouseMode(qmapcontrol::MapControl::Panning); + + if(mav) + { + // Update homePosition + UASManager::instance()->setHomePosition( + static_cast(homeCoordinate.y()), + static_cast(homeCoordinate.x()), 0); + } + } + +} + +void MapWidget::captureGeometryDragHome(Geometry *geom, QPointF coordinate) +{ + if (isVisible()) mc->updateRequest(geom->boundingBox().toRect()); + + Waypoint2DIcon* point2Find = dynamic_cast (geom); + + if (point2Find)// && wps.count() > index) + { + // Update visual + point2Find->setCoordinate(coordinate); + homeCoordinate.setX(coordinate.x()); + homeCoordinate.setY(coordinate.y()); + + qmapcontrol::Point* tempPoint = new qmapcontrol::Point(homeCoordinate.x(), homeCoordinate.y(),"g"); + + if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect()); + } } MapWidget::~MapWidget() @@ -699,6 +796,7 @@ MapWidget::~MapWidget() delete mc; delete m_ui; } + /** * * @param uas the UAS/MAV to monitor/display with the HUD @@ -995,7 +1093,7 @@ void MapWidget::wheelEvent(QWheelEvent *event) detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom)); // visual field of camera - updateCameraPosition(20*newZoom,0,"no"); + //updateCameraPosition(20*newZoom,0,"no"); } } @@ -1062,7 +1160,19 @@ void MapWidget::hideEvent(QHideEvent* event) settings.setValue("LAST_LONGITUDE", currentPos.x()); settings.setValue("LAST_ZOOM", mc->currentZoom()); settings.endGroup(); + + settings.beginGroup("QGC_MAPINDEX"); + settings.setValue("MAP_INDEX", index); + settings.endGroup(); + + settings.beginGroup("QGC_HOMEPOSITION"); + settings.setValue("HOME_LATITUDE", homeCoordinate.y()); + settings.setValue("HOME_LONGITUDE", homeCoordinate.x()); + settings.endGroup(); + settings.sync(); + + } } @@ -1131,72 +1241,55 @@ void MapWidget::clearPath(int uas) } } -void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) +void MapWidget::createHomePosition(const QMouseEvent *event, const QPointF coordinate) { - Q_UNUSED(dir); - Q_UNUSED(bearing); - if (mc) + if (QEvent::MouseButtonRelease == event->type() && setHome->isChecked()) { - // FIXME Mariano - //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); + this->createHomePosition(coordinate); + } +} - QPen* camBorderPen = new QPen(QColor(255,0,0)); - camBorderPen->setWidth(2); +void MapWidget::createHomePosition(const QPointF coordinate) +{ + homeCoordinate= coordinate; + Waypoint2DIcon* tempCirclePoint; - //radio = mc->currentZoom() + double latitude = homeCoordinate.y(); + double longitude = homeCoordinate.x(); - if(drawCamBorder) - { - //clear camera borders - mc->layer("Camera")->clearGeometries(); + tempCirclePoint = new Waypoint2DIcon( + longitude, + latitude, + 20, "g", qmapcontrol::Point::Middle); - //create a camera borders - qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen); + QPen* pencil = new QPen(Qt::blue); + tempCirclePoint->setPen(pencil); - //camBorder->setCoordinate(currentPos); + mc->layer("Station")->clearGeometries(); + mc->layer("Station")->addGeometry(tempCirclePoint); - mc->layer("Camera")->addGeometry(camBorder); - // mc->layer("Camera")->addGeometry(camLine); - if (isVisible()) mc->updateRequestNew(); + qmapcontrol::Point* tempPoint = new qmapcontrol::Point(latitude, longitude,"g"); - } - else - { - //clear camera borders - mc->layer("Camera")->clearGeometries(); - if (isVisible()) mc->updateRequestNew(); - - } + if (isVisible()) + { + mc->updateRequest(tempPoint->boundingBox().toRect()); } } -void MapWidget::drawBorderCamAtMap(bool status) -{ - drawCamBorder = status; - updateCameraPosition(20,0,"no"); - -} - -QPointF MapWidget::getPointxBearing_Range(double lat1, double lon1, double bearing, double distance) +void MapWidget::createHomePositionClick(bool click) { - QPointF temp; + Q_UNUSED(click); - double rad = M_PI/180; + if (!setHome->isChecked()) + { + if(mav) + { + UASManager::instance()->setHomePosition( + static_cast(homeCoordinate.y()), + static_cast(homeCoordinate.x()), 0); - bearing = bearing*rad; - temp.setX((lon1 + ((distance/60) * (sin(bearing))))); - temp.setY((lat1 + ((distance/60) * (cos(bearing))))); - return temp; + //qDebug()<<"Set home position "< - - +#include class QMenu; class Waypoint; @@ -76,8 +75,8 @@ public slots: void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec); 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); + //void updateCameraPosition(double distance, double bearing, QString dir); + //QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance); /** @brief Clear the waypoints overlay layer */ void clearWaypoints(int uas=0); @@ -93,7 +92,7 @@ public slots: void updateWaypoint(int uas, Waypoint* wp); void updateWaypoint(int uas, Waypoint* wp, bool updateView); - void drawBorderCamAtMap(bool status); + //void drawBorderCamAtMap(bool status); /** @brief Bring up dialog to go to a specific location */ void goTo(); @@ -118,6 +117,7 @@ protected: QPushButton* followgps; QPushButton* createPath; QPushButton* clearTracking; + QPushButton* setHome; QLabel* gpsposition; QMenu* mapMenu; QPushButton* mapButton; @@ -128,9 +128,7 @@ protected: qmapcontrol::Layer* overlay; ///< Street overlay (foreground) qmapcontrol::Layer* tracks; ///< Layer for UAV tracks qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints - - //only for experiment - qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator + qmapcontrol::GeometryLayer* homePosition; ///< Layer for station control int zoomLevel; int detailZoom; ///< Steps zoomed in further than qMapControl allows @@ -142,8 +140,6 @@ protected: QMap uasIcons; QMap uasTrails; QMap mavPens; - //QMap > mavWps; - //QMap waypointPaths; UASInterface* mav; quint64 lastUpdate; bool initialized; @@ -157,20 +153,23 @@ protected: void captureGeometryDrag(Geometry* geom, QPointF coordinate); void captureGeometryEndDrag(Geometry* geom, QPointF coordinate); + void captureGeometryDragHome(Geometry* geom, QPointF coordinate); + void createPathButtonClicked(bool checked); /** @brief Create the graphic representation of the waypoint */ void createWaypointGraphAtMap(int id, const QPointF coordinate); void mapproviderSelected(QAction* action); + void createHomePosition(const QMouseEvent* event, const QPointF coordinate); + void createHomePosition(const QPointF coordinate); + void createHomePositionClick(bool click); + void loadSettingsMap(int8_t index); + signals: - //void movePoint(QPointF newCoord); - //void captureMapCoordinateClick(const QPointF coordinate); //ROCA - //void createGlobalWP(bool value, QPointF centerCoordinate); void waypointCreated(Waypoint* wp); void sendGeometryEndDrag(const QPointF coordinate, const int index); - private: Ui::MapWidget *m_ui; QList wps; @@ -180,14 +179,8 @@ private: QPen* pointPen; int wpExists(const QPointF coordinate); bool waypointIsDrag; - - - qmapcontrol::LineString* camLine; - QList camPoints; - QPointF lastCamBorderPos; - bool drawCamBorder; - int radioCamera; - + QPointF homeCoordinate; + int8_t index; }; #endif // MAPWIDGET_H diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index f7a693bd3e03a8733734723a9219d3b2736c4c5b..86d63736c4f65e2a2f4c0b7817fbd34ba270477f 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -43,33 +43,17 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) : uasId(-1), rssi(0.0f), updated(false), - channelLayout(new QVBoxLayout()), - ui(new Ui::QGCRemoteControlView) + channelLayout(new QVBoxLayout())//, + //ui(new Ui::QGCRemoteControlView) { - - //ui->setupUi(this); + ui->setupUi(this); QGridLayout* layout = new QGridLayout(this); layout->addLayout(channelLayout, 1, 0, 1, 2); - // Name label nameLabel = new QLabel(this); - nameLabel->setText("No MAV selected yet.."); layout->addWidget(nameLabel, 0, 0, 1, 2); - // RSSI bar - // Create new layout - QHBoxLayout* rssiLayout = new QHBoxLayout(); - rssiLayout->setSpacing(5); - // Add content - rssiLayout->addWidget(new QLabel(tr("Signal"), this)); - // Append raw label - // Append progress bar - rssiBar = new QProgressBar(this); - rssiBar->setMinimum(0); - rssiBar->setMaximum(100); - rssiBar->setValue(0); - rssiLayout->addWidget(rssiBar); - layout->addItem(rssiLayout, 2, 0, 1, 2); - setVisible(false); + this->setVisible(false); + //setVisible(false); calibrate = new QPushButton(tr("Calibrate"), this); QHBoxLayout *calibrateButtonLayout = new QHBoxLayout(); @@ -112,10 +96,9 @@ void QGCRemoteControlView::setUASId(int id) nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName())); calibrationWindow->setUASId(id); connect(newUAS, SIGNAL(radioCalibrationReceived(const QPointer&)), calibrationWindow, SLOT(receive(const QPointer&))); - connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); + connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(setChannelRaw(int,float))); - connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannel(int,float))); connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float))); } } @@ -127,7 +110,7 @@ void QGCRemoteControlView::setChannelRaw(int channelId, float raw) { // This is a new channel, append it this->raw.append(raw); - this->normalized.append(0); + //this->normalized.append(0); appendChannelWidget(channelId); } else @@ -141,25 +124,25 @@ void QGCRemoteControlView::setChannelRaw(int channelId, float raw) redraw(); } -void QGCRemoteControlView::setChannelScaled(int channelId, float normalized) -{ - if (this->raw.size() <= channelId) // using raw vector as size indicator - { - // This is a new channel, append it - this->normalized.append(normalized); - this->raw.append(0); - appendChannelWidget(channelId); - } - else - { - // This is an existing channel, update it - this->normalized[channelId] = normalized; - } - updated = true; +//void QGCRemoteControlView::setChannelScaled(int channelId, float normalized) +//{ +// if (this->raw.size() <= channelId) // using raw vector as size indicator +// { +// // This is a new channel, append it +// this->normalized.append(normalized); +// this->raw.append(0); +// appendChannelWidget(channelId); +// } +// else +// { +// // This is an existing channel, update it +// this->normalized[channelId] = normalized; +// } +// updated = true; - // FIXME Will be timer based in the future - redraw(); -} +// // FIXME Will be timer based in the future +// redraw(); +//} void QGCRemoteControlView::setRemoteRSSI(float rssiNormalized) { @@ -193,18 +176,26 @@ void QGCRemoteControlView::redraw() if(isVisible() && updated) { // Update raw values - for(int i = 0; i < rawLabels.count(); i++) - { - rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4, 10, QChar('0'))); - } + //for(int i = 0; i < rawLabels.count(); i++) + //{ + //rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4, 10, QChar('0'))); + //} // Update percent bars for(int i = 0; i < progressBars.count(); i++) { - progressBars.at(i)->setValue(normalized.at(i)*100.0f); + rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4, 10, QChar('0'))); + //int vv = normalized.at(i)*100.0f; + //progressBars.at(i)->setValue(vv); + int vv = raw.at(i)*1.0f; + progressBars.at(i)->setValue(vv); } // Update RSSI - rssiBar->setValue(rssi*100); + if(rssi>0) + { + //rssiBar->setValue(rssi);//*100); + } + updated = false; } } diff --git a/src/ui/QGCRemoteControlView.h b/src/ui/QGCRemoteControlView.h index 0027748385a54fc91729f9e5db7cf0ddc0b57df9..8a4e7ecf8f408bd5e34227c2a1df0c8c891d5090 100644 --- a/src/ui/QGCRemoteControlView.h +++ b/src/ui/QGCRemoteControlView.h @@ -1,82 +1,82 @@ -/*===================================================================== - -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 Declaration of QGCRemoteControlView - * @author Lorenz Meier - */ - -#ifndef QGCREMOTECONTROLVIEW_H -#define QGCREMOTECONTROLVIEW_H - -#include -#include -#include - -#include "RadioCalibration/RadioCalibrationWindow.h" - -namespace Ui { - class QGCRemoteControlView; -} - -class QVBoxLayout; -class QLabel; -class QProgressBar; - -class QGCRemoteControlView : public QWidget { - Q_OBJECT -public: - QGCRemoteControlView(QWidget *parent = 0); - ~QGCRemoteControlView(); - -public slots: - void setUASId(int id); - void setChannelRaw(int channelId, float raw); - void setChannelScaled(int channelId, float normalized); - void setRemoteRSSI(float rssiNormalized); - void redraw(); - -protected slots: - void appendChannelWidget(int channelId); - -protected: - void changeEvent(QEvent *e); - int uasId; - float rssi; - bool updated; - QVBoxLayout* channelLayout; - QVector raw; - QVector normalized; - QVector rawLabels; - QVector progressBars; - QProgressBar* rssiBar; - QLabel* nameLabel; - QPushButton *calibrate; - RadioCalibrationWindow *calibrationWindow; - -private: - Ui::QGCRemoteControlView *ui; -}; - -#endif // QGCREMOTECONTROLVIEW_H +/*===================================================================== + +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 Declaration of QGCRemoteControlView + * @author Lorenz Meier + */ + +#ifndef QGCREMOTECONTROLVIEW_H +#define QGCREMOTECONTROLVIEW_H + +#include +#include +#include + +#include "RadioCalibration/RadioCalibrationWindow.h" + +namespace Ui { + class QGCRemoteControlView; +} + +class QVBoxLayout; +class QLabel; +class QProgressBar; + +class QGCRemoteControlView : public QWidget { + Q_OBJECT +public: + QGCRemoteControlView(QWidget *parent = 0); + ~QGCRemoteControlView(); + +public slots: + void setUASId(int id); + void setChannelRaw(int channelId, float raw); + //void setChannelScaled(int channelId, float normalized); + void setRemoteRSSI(float rssiNormalized); + void redraw(); + +protected slots: + void appendChannelWidget(int channelId); + +protected: + void changeEvent(QEvent *e); + int uasId; + float rssi; + bool updated; + QVBoxLayout* channelLayout; + QVector raw; + QVector normalized; + QVector rawLabels; + QVector progressBars; + QProgressBar* rssiBar; + QLabel* nameLabel; + QPushButton *calibrate; + RadioCalibrationWindow *calibrationWindow; + +private: + Ui::QGCRemoteControlView *ui; +}; + +#endif // QGCREMOTECONTROLVIEW_H diff --git a/src/ui/QGCRemoteControlView.ui b/src/ui/QGCRemoteControlView.ui index fc7177c64bcde199580a657f7cfaaf00553bf356..9d863cfa57d71e5230c8cde7b56e71003d9d2046 100644 --- a/src/ui/QGCRemoteControlView.ui +++ b/src/ui/QGCRemoteControlView.ui @@ -1,333 +1,19 @@ - - - QGCRemoteControlView - - - - 0 - 0 - 400 - 300 - - - - Form - - - - - 300 - 260 - 93 - 27 - - - - Calibrate - - - - - - 10 - 270 - 171 - 17 - - - - Remote Control detected - - - - - - 10 - 10 - 71 - 17 - - - - Channel 1 - - - - - - 10 - 30 - 71 - 17 - - - - Channel 2 - - - - - - 10 - 50 - 71 - 17 - - - - Channel 3 - - - - - - 10 - 70 - 71 - 17 - - - - Channel 4 - - - - - - 10 - 90 - 71 - 17 - - - - Channel 5 - - - - - - 10 - 110 - 71 - 17 - - - - Channel 6 - - - - - - 10 - 130 - 71 - 17 - - - - Channel 7 - - - - - - 10 - 150 - 71 - 17 - - - - Channel 8 - - - - - - 10 - 170 - 62 - 17 - - - - RSSI - - - - - - 200 - 10 - 118 - 16 - - - - -100 - - - 0 - - - true - - - false - - - %v% - - - - - - 200 - 30 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 50 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 70 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 90 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 110 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 130 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 200 - 150 - 118 - 16 - - - - -100 - - - 0 - - - %v% - - - - - - 90 - 10 - 41 - 17 - - - - 1120 - - - - - - + + + QGCRemoteControlView + + + + 0 + 0 + 155 + 106 + + + + Form + + + + + diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index f975cc0b142fadc74cc394058b0a4885eb34633f..2acbb9cc44860d5c7c5e999bad82135a768c0acf 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -1,328 +1,337 @@ -#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 = qobject_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(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double))); - - - - - //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&))); - connect(slugsMav,SIGNAL(slugsAirData(int, const mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int, const mavlink_air_data_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)); - - ui->m_Mxr->setText(QString::number(rawData.xmag)); - ui->m_Myr->setText(QString::number(rawData.ymag)); - ui->m_Mzr->setText(QString::number(rawData.zmag)); - - ui->m_Gxr->setText(QString::number(rawData.xgyro)); - ui->m_Gyr->setText(QString::number(rawData.ygyro)); - ui->m_Gzr->setText(QString::number(rawData.zgyro)); - -} - -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)); - - //qDebug()<<"GPS Position = "<ed_x->setPlainText(QString::number(x)); - ui->ed_y->setPlainText(QString::number(y)); - ui->ed_z->setPlainText(QString::number(z)); - - //qDebug()<<"Local Position = "<ed_vx->setPlainText(QString::number(vx)); - ui->ed_vy->setPlainText(QString::number(vy)); - ui->ed_vz->setPlainText(QString::number(vz)); - - //qDebug()<<"Speed Local Position = "<m_Roll->setPlainText(QString::number(slugroll)); - ui->m_Pitch->setPlainText(QString::number(slugpitch)); - ui->m_Yaw->setPlainText(QString::number(slugyaw)); - - // qDebug()<<"Attitude change = "<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); - - QString month, day; - - month = QString::number(gpsDateTime.month); - day = QString::number(gpsDateTime.day); - - if(gpsDateTime.month < 10) month = "0" + QString::number(gpsDateTime.month); - if(gpsDateTime.day < 10) day = "0" + QString::number(gpsDateTime.day); - - - ui->m_GpsDate->setText(day + "/" + - month + "/" + - QString::number(gpsDateTime.year)); - - QString hour, min, sec; - - hour = QString::number(gpsDateTime.hour); - min = QString::number(gpsDateTime.min); - sec = QString::number(gpsDateTime.sec); - - if(gpsDateTime.hour < 10) hour = "0" + QString::number(gpsDateTime.hour); - if(gpsDateTime.min < 10) min = "0" + QString::number(gpsDateTime.min); - if(gpsDateTime.sec < 10) sec = "0" + QString::number(gpsDateTime.sec); - - ui->m_GpsTime->setText(hour + ":" + - min + ":" + - sec); - - ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); - - -} - -/** - * @brief Updates the air data widget - 171 -*/ -void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData) -{ - Q_UNUSED(systemId); - ui->ed_dynamic->setText(QString::number(airData.dynamicPressure)); - ui->ed_static->setText(QString::number(airData.staticPressure)); - ui->ed_temp->setText(QString::number(airData.temperature)); - -// qDebug()<<"Air Data = "<m_GpsCog->setText(QString::number(cog)); - ui->m_GpsSog->setText(QString::number(sog)); - -} - - - - - - -#endif // MAVLINK_ENABLED_SLUGS +#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 = qobject_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(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double))); + + //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&))); + connect(slugsMav,SIGNAL(slugsAirData(int, const mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int, const mavlink_air_data_t&))); + + + connect(slugsMav, SIGNAL(slugsChannels(int, const mavlink_rc_channels_raw_t&)), this, SLOT(slugsRCRawChannels(int, const mavlink_rc_channels_raw_t&))); + connect(slugsMav, SIGNAL(slugsServo(int, const mavlink_servo_output_raw_t&)), this, SLOT(slugsRCServo(int,const mavlink_servo_output_raw_t&))); + connect(slugsMav, SIGNAL(slugsScaled(int, const mavlink_scaled_imu_t&)), this, SLOT(slugsFilteredDataChanged(int, const mavlink_scaled_imu_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)); + + ui->m_Mxr->setText(QString::number(rawData.xmag)); + ui->m_Myr->setText(QString::number(rawData.ymag)); + ui->m_Mzr->setText(QString::number(rawData.zmag)); + + ui->m_Gxr->setText(QString::number(rawData.xgyro)); + ui->m_Gyr->setText(QString::number(rawData.ygyro)); + ui->m_Gzr->setText(QString::number(rawData.zgyro)); + +} + + +void SlugsDataSensorView::slugsRCRawChannels(int systemId, const mavlink_rc_channels_raw_t &gpsDateTime) +{Q_UNUSED(systemId); + + ui->tbRCThrottle->setText(QString::number(gpsDateTime.chan1_raw)); + ui->tbRCAileron->setText(QString::number(gpsDateTime.chan2_raw)); + ui->tbRCRudder->setText(QString::number(gpsDateTime.chan3_raw)); + ui->tbRCElevator->setText(QString::number(gpsDateTime.chan4_raw)); +} + +void SlugsDataSensorView::slugsRCServo(int systemId, const mavlink_servo_output_raw_t &gpsDateTime) +{Q_UNUSED(systemId); + + ui->m_pwmThro->setText(QString::number(gpsDateTime.servo1_raw)); + ui->m_pwmAile->setText(QString::number(gpsDateTime.servo2_raw)); + ui->m_pwmRudd->setText(QString::number(gpsDateTime.servo3_raw)); + ui->m_pwmElev->setText(QString::number(gpsDateTime.servo4_raw)); + + ui->m_pwmThroTrim->setText(QString::number(gpsDateTime.servo5_raw)); + ui->m_pwmAileTrim->setText(QString::number(gpsDateTime.servo6_raw)); + ui->m_pwmRuddTrim->setText(QString::number(gpsDateTime.servo7_raw)); + ui->m_pwmElevTrim->setText(QString::number(gpsDateTime.servo8_raw)); +} + +void SlugsDataSensorView::setActiveUAS(UASInterface* uas){ + activeUAS = uas; + addUAS(activeUAS); +} + +#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)); + + //qDebug()<<"GPS Position = "<ed_x->setText(QString::number(x)); + ui->ed_y->setText(QString::number(y)); + ui->ed_z->setText(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->setText(QString::number(vx)); + ui->ed_vy->setText(QString::number(vy)); + ui->ed_vz->setText(QString::number(vz)); + + //qDebug()<<"Speed Local Position = "<m_Roll->setText(QString::number(slugroll)); + ui->m_Pitch->setText(QString::number(slugpitch)); + ui->m_Yaw->setText(QString::number(slugyaw)); + + // qDebug()<<"Attitude change = "<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_servo_output_raw_t& pwmCommands){ +// Q_UNUSED(systemId); +// ui->m_pwmThro->setText(QString::number(pwmCommands.servo1_raw));//.dt_c)); +// ui->m_pwmAile->setText(QString::number(pwmCommands.servo2_raw));//dla_c)); +// ui->m_pwmElev->setText(QString::number(pwmCommands.servo4_raw));//dle_c)); +// ui->m_pwmRudd->setText(QString::number(pwmCommands.servo3_raw));//dr_c)); + +// ui->m_pwmThroTrim->setText(QString::number(pwmCommands.servo5_raw));//dre_c)); +// ui->m_pwmAileTrim->setText(QString::number(pwmCommands.servo6_raw));//dlf_c)); +// ui->m_pwmElevTrim->setText(QString::number(pwmCommands.servo8_raw));//drf_c)); +// ui->m_pwmRuddTrim->setText(QString::number(pwmCommands.servo7_raw));//aux1)); + +//} + +void SlugsDataSensorView::slugsFilteredDataChanged(int systemId, + const mavlink_scaled_imu_t& filteredData){ + Q_UNUSED(systemId); + ui->m_Axf->setText(QString::number(filteredData.xacc/1000.0f)); + ui->m_Ayf->setText(QString::number(filteredData.yacc/1000.0f)); + ui->m_Azf->setText(QString::number(filteredData.zacc/1000.0f)); + ui->m_Gxf->setText(QString::number(filteredData.xgyro/1000.0f)); + ui->m_Gyf->setText(QString::number(filteredData.ygyro/1000.0f)); + ui->m_Gzf->setText(QString::number(filteredData.zgyro/1000.0f)); + ui->m_Mxf->setText(QString::number(filteredData.xmag/1000.0f)); + ui->m_Myf->setText(QString::number(filteredData.ymag/1000.0f)); + ui->m_Mzf->setText(QString::number(filteredData.zmag/1000.0f)); +} + +void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, + const mavlink_gps_date_time_t& gpsDateTime){ + Q_UNUSED(systemId); + + QString month, day; + + month = QString::number(gpsDateTime.month); + day = QString::number(gpsDateTime.day); + + if(gpsDateTime.month < 10) month = "0" + QString::number(gpsDateTime.month); + if(gpsDateTime.day < 10) day = "0" + QString::number(gpsDateTime.day); + + + ui->m_GpsDate->setText(day + "/" + + month + "/" + + QString::number(gpsDateTime.year)); + + QString hour, min, sec; + + hour = QString::number(gpsDateTime.hour); + min = QString::number(gpsDateTime.min); + sec = QString::number(gpsDateTime.sec); + + if(gpsDateTime.hour < 10) hour = "0" + QString::number(gpsDateTime.hour); + if(gpsDateTime.min < 10) min = "0" + QString::number(gpsDateTime.min); + if(gpsDateTime.sec < 10) sec = "0" + QString::number(gpsDateTime.sec); + + ui->m_GpsTime->setText(hour + ":" + + min + ":" + + sec); + + ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); + + +} + +/** + * @brief Updates the air data widget - 171 +*/ +void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData) +{ + Q_UNUSED(systemId); + ui->ed_dynamic->setText(QString::number(airData.dynamicPressure)); + ui->ed_static->setText(QString::number(airData.staticPressure)); + ui->ed_temp->setText(QString::number(airData.temperature)); +} + +/** + * @brief set COG and SOG values + * + * COG and SOG GPS display on the Widgets +*/ +void SlugsDataSensorView::slugsGPSCogSog(int systemId, double cog, double sog) +{ + Q_UNUSED(systemId); + + ui->m_GpsCog->setText(QString::number(cog)); + ui->m_GpsSog->setText(QString::number(sog)); +} + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h index b1adcca6a18b4dd6519858f9e82d0343185946a5..16e78ae419ed1a6bed92d3d9f2658640bbb76353 100644 --- a/src/ui/SlugsDataSensorView.h +++ b/src/ui/SlugsDataSensorView.h @@ -1,209 +1,212 @@ -/*===================================================================== - -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); - - - /** - * @brief Updates the Raw Data widget - */ - 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 set COG and SOG values - * - * COG and SOG GPS display on the Widgets - */ - void slugsGPSCogSog(int systemId, - double cog, - double sog); - - - - /** - * @brief Updates the CPU load widget - 170 - */ - void slugsCpuLoadChanged(int systemId, - const mavlink_cpu_load_t& cpuLoad); - - /** - * @brief Updates the air data widget - 171 - */ - void slugsAirDataChanged(int systemId, - const mavlink_air_data_t& airData); - - /** - * @brief Updates the sensor bias widget - 172 - */ - void slugsSensorBiasChanged(int systemId, - const mavlink_sensor_bias_t& sensorBias); - - /** - * @brief Updates the diagnostic widget - 173 - */ - void slugsDiagnosticMessageChanged(int systemId, - const mavlink_diagnostic_t& diagnostic); - - - /** - * @brief Updates the Navigation widget - 176 - */ - void slugsNavegationChanged(int systemId, - const mavlink_slugs_navigation_t& slugsNavigation); - - /** - * @brief Updates the Data Log widget - 177 - */ - void slugsDataLogChanged(int systemId, - const mavlink_data_log_t& dataLog); - - /** - * @brief Updates the PWM Commands widget - 175 - */ - void slugsPWMChanged(int systemId, - const mavlink_pwm_commands_t& pwmCommands); - - /** - * @brief Updates the filtered sensor measurements widget - 178 - */ - void slugsFilteredDataChanged(int systemId, - const mavlink_filtered_data_t& filteredData); - - - /** - * @brief Updates the gps Date Time widget - 179 - */ - 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 +/*===================================================================== + +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); + + + /** + * @brief Updates the Raw Data widget + */ + 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 set COG and SOG values + * + * COG and SOG GPS display on the Widgets + */ + void slugsGPSCogSog(int systemId, + double cog, + double sog); + + + + /** + * @brief Updates the CPU load widget - 170 + */ + void slugsCpuLoadChanged(int systemId, + const mavlink_cpu_load_t& cpuLoad); + + /** + * @brief Updates the air data widget - 171 + */ + void slugsAirDataChanged(int systemId, + const mavlink_air_data_t& airData); + + /** + * @brief Updates the sensor bias widget - 172 + */ + void slugsSensorBiasChanged(int systemId, + const mavlink_sensor_bias_t& sensorBias); + + /** + * @brief Updates the diagnostic widget - 173 + */ + void slugsDiagnosticMessageChanged(int systemId, + const mavlink_diagnostic_t& diagnostic); + + + /** + * @brief Updates the Navigation widget - 176 + */ + void slugsNavegationChanged(int systemId, + const mavlink_slugs_navigation_t& slugsNavigation); + + /** + * @brief Updates the Data Log widget - 177 + */ + void slugsDataLogChanged(int systemId, + const mavlink_data_log_t& dataLog); + +// /** +// * @brief Updates the PWM Commands widget - 175 +// */ +// void slugsPWMChanged(int systemId, +// const mavlink_servo_output_raw_t& pwmCommands); + + /** + * @brief Updates the filtered sensor measurements widget - 178 + */ + void slugsFilteredDataChanged(int systemId, + const mavlink_scaled_imu_t& filteredData); + + + /** + * @brief Updates the gps Date Time widget - 179 + */ + void slugsGPSDateTimeChanged(int systemId, + const mavlink_gps_date_time_t& gpsDateTime); + + + void slugsRCRawChannels(int systemId, + const mavlink_rc_channels_raw_t& gpsDateTime); + + void slugsRCServo(int systemId, + const mavlink_servo_output_raw_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 index 7f10004047cb1232bf7b8dbac3b17e55ae6bc5dc..fbe5309c4e5c5a640f0d22eab363b07d164f3b5b 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -1,4480 +1,3140 @@ - - - SlugsDataSensorView - - - - 0 - 0 - 399 - 598 - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 399 - 604 - - - - 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 - 5 - - - - - - - - - 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 - 84 - - - - - - - - - - - - 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 - 84 - - - - - - - - - - - - - - - - + + + SlugsDataSensorView + + + + 0 + 0 + 495 + 671 + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Form + + + + + + 1 + + + + Navigation + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 50 + false + + + + Position + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + QFrame::NoFrame + + + X + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Y + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + QFrame::NoFrame + + + Z + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vx + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vy + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Vz + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 50 + false + + + + Attitude + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Roll + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Pitch + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Yaw + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 50 + false + + + + Navigation + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + U_m + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Pitch C + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Psi_d C + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Phi C + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay body + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + From WP + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + To WP + + + + + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 17 + 13 + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Tot Run + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Dist to G + + + + + + + + + + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + Attitude + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + Sensor Biases + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Axb + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ayb + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Azb + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gxb + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gyb + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gzb + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 311 + 171 + + + + + 10 + + + + PWM Commands + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Thro + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Aile + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Elev + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Rudd + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Thro Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Aile Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Elev Trim + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Rudd Trim + + + + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 60 + 18 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + + + + + + + + + GPS Data + + + + + + + + + + + 10 + 75 + true + + + + Throttle + + + + + + + + 10 + 75 + true + + + + Aileron + + + + + + + + 10 + 75 + true + + + + Rudder + + + + + + + + 10 + 75 + true + + + + Elevator + + + + + + + + 10 + 75 + true + + + + SOG + + + + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + Sensor + + + + + + + + GPS Data + + + + + + + + + 10 + 75 + true + + + + Date + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Time + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + # Sats + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + COG + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + SOG + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + + + + + 10 + 75 + true + + + + Latitude + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Longitude + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + + 10 + 75 + true + + + + Height + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Raw Data + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Filtered Data + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + CPU Load + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Sensor + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Control + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Batt Volt + + + + + + + + + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + Air Data + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Dynamic + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Static + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Temperature + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + Messages + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 50 + false + + + + Diagnostic Messages + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Fl1 + + + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Sh1 + + + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Fl2 + + + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Sh2 + + + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Fl3 + + + + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + Sh3 + + + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 50 + false + + + + Log Messages + + + + + + + + + 10 + 75 + true + + + + Fl1 + + + + + + + + + + + 10 + 75 + true + + + + Fl4 + + + + + + + + + + + 10 + 75 + true + + + + Fl2 + + + + + + + + + + + 10 + 75 + true + + + + Fl5 + + + + + + + + + + + 10 + 75 + true + + + + Fl3 + + + + + + + + + + + 10 + 75 + true + + + + Fl6 + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + + diff --git a/src/ui/SlugsHilSim.cc b/src/ui/SlugsHilSim.cc index 3530c8154fa346ff9d291b9d2abc97374c27632f..31b3cdc936b150d9d97f5c4fa2b1a65e096cc240 100644 --- a/src/ui/SlugsHilSim.cc +++ b/src/ui/SlugsHilSim.cc @@ -1,356 +1,359 @@ -/*===================================================================== - -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 - * @author Alejandro Molina - */ - - -#include "SlugsHilSim.h" -#include "ui_SlugsHilSim.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(); - - #ifdef MAVLINK_ENABLED_SLUGS - memset(&tmpAirData, 0, sizeof(mavlink_air_data_t)); - memset(&tmpAttitudeData, 0, sizeof(mavlink_attitude_t)); - memset(&tmpGpsData, 0, sizeof(mavlink_gps_raw_t)); - memset(&tmpGpsTime, 0, sizeof(mavlink_gps_date_time_t)); - memset(&tmpLocalPositionData, 0, sizeof(mavlink_sensor_bias_t)); - memset(&tmpRawImuData, 0, sizeof(mavlink_raw_imu_t)); - #endif - - foreach (LinkInterface* link, LinkManager::instance()->getLinks()) - { - addToCombo(link); - } -} - -SlugsHilSim::~SlugsHilSim() -{ - rxSocket->disconnectFromHost(); - delete ui; -} - -void SlugsHilSim::addToCombo(LinkInterface* theLink){ - - linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink); - ui->cb_mavlinkLinks->addItem(theLink->getName()); - - 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); - - sendMessageToSlugs(); - - commandDatagramToSimulink(); - } - - 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) -{ - #ifdef MAVLINK_ENABLED_SLUGS - unsigned char i = 0; - - - 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++); - - tmpGpsData.lat = getFloatFromDatagram(datagram, &i); - tmpGpsData.lon = getFloatFromDatagram(datagram, &i); - tmpGpsData.alt = getFloatFromDatagram(datagram, &i); - - tmpGpsData.hdg = getUint16FromDatagram(datagram, &i); - tmpGpsData.v = getUint16FromDatagram(datagram, &i); - - tmpGpsData.eph = getUint16FromDatagram(datagram, &i); - tmpGpsData.fix_type = datagram->at(i++); - tmpGpsTime.visSat = datagram->at(i++); - i++; - - tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i); - tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i); - tmpAirData.temperature= getUint16FromDatagram(datagram, &i); - - // TODO Salto en el Datagrama - i=i+8; - - tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i); - tmpRawImuData.ygyro = getUint16FromDatagram(datagram, &i); - tmpRawImuData.zgyro = getUint16FromDatagram(datagram, &i); - tmpRawImuData.xacc = getUint16FromDatagram(datagram, &i); - tmpRawImuData.yacc = getUint16FromDatagram(datagram, &i); - tmpRawImuData.zacc = getUint16FromDatagram(datagram, &i); - tmpRawImuData.xmag = getUint16FromDatagram(datagram, &i); - tmpRawImuData.ymag = getUint16FromDatagram(datagram, &i); - tmpRawImuData.zmag = getUint16FromDatagram(datagram, &i); - - tmpAttitudeData.roll = getFloatFromDatagram(datagram, &i); - tmpAttitudeData.pitch = getFloatFromDatagram(datagram, &i); - tmpAttitudeData.yaw = getFloatFromDatagram(datagram, &i); - - tmpAttitudeData.rollspeed = getFloatFromDatagram(datagram, &i); - tmpAttitudeData.pitchspeed = getFloatFromDatagram(datagram, &i); - tmpAttitudeData.yawspeed = getFloatFromDatagram(datagram, &i); - - // TODO Crear Paquete SYNC TIME - i=i+2; - - tmpLocalPositionData.x = getFloatFromDatagram(datagram, &i); - tmpLocalPositionData.y = getFloatFromDatagram(datagram, &i); - tmpLocalPositionData.z = getFloatFromDatagram(datagram, &i); - tmpLocalPositionData.vx = getFloatFromDatagram(datagram, &i); - tmpLocalPositionData.vy = getFloatFromDatagram(datagram, &i); - tmpLocalPositionData.vz = getFloatFromDatagram(datagram, &i); - - // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model - i++; - - ui->ed_1->setText(QString::number(tmpRawImuData.xacc)); - ui->ed_2->setText(QString::number(tmpRawImuData.yacc)); - ui->ed_3->setText(QString::number(tmpRawImuData.zacc)); - - ui->tbA->setText(QString::number(tmpRawImuData.xgyro)); - ui->tbB->setText(QString::number(tmpRawImuData.ygyro)); - ui->tbC->setText(QString::number(tmpRawImuData.zgyro)); - -#else - Q_UNUSED(datagram); -#endif -} - -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) -{ - #ifdef MAVLINK_ENABLED_SLUGS - // HIL code to go here... - //hilLink = linksAvailable - // FIXME Mariano - - hilLink =linksAvailable.value(cbIndex); - -#else - Q_UNUSED(cbIndex) - #endif -} - -void SlugsHilSim::sendMessageToSlugs() -{ - #ifdef MAVLINK_ENABLED_SLUGS - mavlink_message_t msg; - - mavlink_msg_local_position_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpLocalPositionData); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); - - mavlink_msg_attitude_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpAttitudeData); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); - - mavlink_msg_raw_imu_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpRawImuData); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); - - mavlink_msg_air_data_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpAirData); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); - - mavlink_msg_gps_raw_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpGpsData); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); - - mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID, - MG::SYSTEM::COMPID, - &msg, - &tmpGpsTime); - activeUas->sendMessage(hilLink, msg); - memset(&msg, 0, sizeof(mavlink_message_t)); -#endif -} - - -void SlugsHilSim::commandDatagramToSimulink() -{ - #ifdef MAVLINK_ENABLED_SLUGS - //mavlink_pwm_commands_t* pwdC = (static_cast(activeUas))->getPwmCommands(); - - mavlink_pwm_commands_t* pwdC; - - if(pwdC != NULL){ - } - - QByteArray data; - data.resize(22); - - unsigned char i=0; - setUInt16ToDatagram(data, &i, 1);//pwdC->dt_c); - setUInt16ToDatagram(data, &i, 2);//pwdC->dla_c); - setUInt16ToDatagram(data, &i, 3);//pwdC->dra_c); - setUInt16ToDatagram(data, &i, 4);//pwdC->dr_c); - setUInt16ToDatagram(data, &i, 5);//pwdC->dle_c); - setUInt16ToDatagram(data, &i, 6);//pwdC->dre_c); - setUInt16ToDatagram(data, &i, 7);//pwdC->dlf_c); - setUInt16ToDatagram(data, &i, 8);//pwdC->drf_c); - setUInt16ToDatagram(data, &i, 9);//pwdC->aux1); - setUInt16ToDatagram(data, &i, 10);//pwdC->aux2); - setUInt16ToDatagram(data, &i, 11);//value default - - txSocket->writeDatagram(data, QHostAddress::Broadcast, ui->ed_txPort->text().toInt()); -#endif -} - -void SlugsHilSim::setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value) -{ - tUint16ToChar tmpUnion; - tmpUnion.uiData= value; - - datagram[(*pos)++]= tmpUnion.chData[0]; - datagram[(*pos)++]= tmpUnion.chData[1]; -} +/*===================================================================== + +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 + * @author Alejandro Molina + */ + + +#include "SlugsHilSim.h" +#include "ui_SlugsHilSim.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(); + + #ifdef MAVLINK_ENABLED_SLUGS + memset(&tmpAirData, 0, sizeof(mavlink_air_data_t)); + memset(&tmpAttitudeData, 0, sizeof(mavlink_attitude_t)); + memset(&tmpGpsData, 0, sizeof(mavlink_gps_raw_t)); + memset(&tmpGpsTime, 0, sizeof(mavlink_gps_date_time_t)); + memset(&tmpLocalPositionData, 0, sizeof(mavlink_sensor_bias_t)); + memset(&tmpRawImuData, 0, sizeof(mavlink_raw_imu_t)); + #endif + + foreach (LinkInterface* link, LinkManager::instance()->getLinks()) + { + addToCombo(link); + } +} + +SlugsHilSim::~SlugsHilSim() +{ + rxSocket->disconnectFromHost(); + delete ui; +} + +void SlugsHilSim::addToCombo(LinkInterface* theLink){ + + linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink); + ui->cb_mavlinkLinks->addItem(theLink->getName()); + + 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); + + activeUas->startHil(); + + } 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(); + activeUas->stopHil(); + } +} + +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); + + sendMessageToSlugs(); + + commandDatagramToSimulink(); + } + + 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) +{ + #ifdef MAVLINK_ENABLED_SLUGS + unsigned char i = 0; + + + 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++); + + tmpGpsData.lat = getFloatFromDatagram(datagram, &i); + tmpGpsData.lon = getFloatFromDatagram(datagram, &i); + tmpGpsData.alt = getFloatFromDatagram(datagram, &i); + + tmpGpsData.hdg = getUint16FromDatagram(datagram, &i); + tmpGpsData.v = getUint16FromDatagram(datagram, &i); + + tmpGpsData.eph = getUint16FromDatagram(datagram, &i); + tmpGpsData.fix_type = datagram->at(i++); + tmpGpsTime.visSat = datagram->at(i++); + i++; + + tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i); + tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i); + tmpAirData.temperature= getUint16FromDatagram(datagram, &i); + + // TODO Salto en el Datagrama + i=i+8; + + tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i); + tmpRawImuData.ygyro = getUint16FromDatagram(datagram, &i); + tmpRawImuData.zgyro = getUint16FromDatagram(datagram, &i); + tmpRawImuData.xacc = getUint16FromDatagram(datagram, &i); + tmpRawImuData.yacc = getUint16FromDatagram(datagram, &i); + tmpRawImuData.zacc = getUint16FromDatagram(datagram, &i); + tmpRawImuData.xmag = getUint16FromDatagram(datagram, &i); + tmpRawImuData.ymag = getUint16FromDatagram(datagram, &i); + tmpRawImuData.zmag = getUint16FromDatagram(datagram, &i); + + tmpAttitudeData.roll = getFloatFromDatagram(datagram, &i); + tmpAttitudeData.pitch = getFloatFromDatagram(datagram, &i); + tmpAttitudeData.yaw = getFloatFromDatagram(datagram, &i); + + tmpAttitudeData.rollspeed = getFloatFromDatagram(datagram, &i); + tmpAttitudeData.pitchspeed = getFloatFromDatagram(datagram, &i); + tmpAttitudeData.yawspeed = getFloatFromDatagram(datagram, &i); + + // TODO Crear Paquete SYNC TIME + i=i+2; + + tmpLocalPositionData.x = getFloatFromDatagram(datagram, &i); + tmpLocalPositionData.y = getFloatFromDatagram(datagram, &i); + tmpLocalPositionData.z = getFloatFromDatagram(datagram, &i); + tmpLocalPositionData.vx = getFloatFromDatagram(datagram, &i); + tmpLocalPositionData.vy = getFloatFromDatagram(datagram, &i); + tmpLocalPositionData.vz = getFloatFromDatagram(datagram, &i); + + // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model + i++; + + ui->ed_1->setText(QString::number(tmpRawImuData.xacc)); + ui->ed_2->setText(QString::number(tmpRawImuData.yacc)); + ui->ed_3->setText(QString::number(tmpRawImuData.zacc)); + + ui->tbA->setText(QString::number(tmpRawImuData.xgyro)); + ui->tbB->setText(QString::number(tmpRawImuData.ygyro)); + ui->tbC->setText(QString::number(tmpRawImuData.zgyro)); + +#else + Q_UNUSED(datagram); +#endif +} + +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) +{ + #ifdef MAVLINK_ENABLED_SLUGS + // HIL code to go here... + //hilLink = linksAvailable + // FIXME Mariano + + hilLink =linksAvailable.value(cbIndex); + +#else + Q_UNUSED(cbIndex) + #endif +} + +void SlugsHilSim::sendMessageToSlugs() +{ + #ifdef MAVLINK_ENABLED_SLUGS + mavlink_message_t msg; + + mavlink_msg_local_position_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpLocalPositionData); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_attitude_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpAttitudeData); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_raw_imu_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpRawImuData); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_air_data_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpAirData); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_gps_raw_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpGpsData); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); + + mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID, + MG::SYSTEM::COMPID, + &msg, + &tmpGpsTime); + activeUas->sendMessage(hilLink, msg); + memset(&msg, 0, sizeof(mavlink_message_t)); +#endif +} + + +void SlugsHilSim::commandDatagramToSimulink() +{ + #ifdef MAVLINK_ENABLED_SLUGS + //mavlink_pwm_commands_t* pwdC = (static_cast(activeUas))->getPwmCommands(); + + //mavlink_pwm_commands_t* pwdC; + +// if(pwdC != NULL){ +// } + + QByteArray data; + data.resize(22); + + unsigned char i=0; + setUInt16ToDatagram(data, &i, 1);//pwdC->dt_c); + setUInt16ToDatagram(data, &i, 2);//pwdC->dla_c); + setUInt16ToDatagram(data, &i, 3);//pwdC->dra_c); + setUInt16ToDatagram(data, &i, 4);//pwdC->dr_c); + setUInt16ToDatagram(data, &i, 5);//pwdC->dle_c); + setUInt16ToDatagram(data, &i, 6);//pwdC->dre_c); + setUInt16ToDatagram(data, &i, 7);//pwdC->dlf_c); + setUInt16ToDatagram(data, &i, 8);//pwdC->drf_c); + setUInt16ToDatagram(data, &i, 9);//pwdC->aux1); + setUInt16ToDatagram(data, &i, 10);//pwdC->aux2); + setUInt16ToDatagram(data, &i, 11);//value default + + txSocket->writeDatagram(data, QHostAddress::Broadcast, ui->ed_txPort->text().toInt()); +#endif +} + +void SlugsHilSim::setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value) +{ + tUint16ToChar tmpUnion; + tmpUnion.uiData= value; + + datagram[(*pos)++]= tmpUnion.chData[0]; + datagram[(*pos)++]= tmpUnion.chData[1]; +} diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp deleted file mode 100644 index dd6b2fccc2c83b707669c21b12884ea125e7eac6..0000000000000000000000000000000000000000 --- a/src/ui/SlugsPIDControl.cpp +++ /dev/null @@ -1,735 +0,0 @@ -#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); // 10 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 = qobject_cast(uas); - - if (slugsMav) - { - 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())); - connect(ui->getGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartGet())); - } - - #endif // MAVLINK_ENABLED_SLUG - - // Set this UAS as active if it is the first one - if(!activeUAS) - { - activeUAS = uas; - systemId = activeUAS->getUASID(); - connect_editLinesPDIvalues(); - } - -} - -/** - * @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->P2dT_FF_set->text().toFloat(); - pidMessage.iVal = 0;//ui->dR_I_set->text().toFloat(); - pidMessage.dVal = 0;//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) -{ - Q_UNUSED(systemId); - 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; - - - } - } -#else - Q_UNUSED(PIDtype); - #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 deleted file mode 100644 index 3ced5eab8bf61088c67850341eefc7ce402081c0..0000000000000000000000000000000000000000 --- a/src/ui/SlugsPIDControl.h +++ /dev/null @@ -1,292 +0,0 @@ -#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 deleted file mode 100644 index 6de57a0178d35a4d94f14b72a70e166ae82714cf..0000000000000000000000000000000000000000 --- a/src/ui/SlugsPIDControl.ui +++ /dev/null @@ -1,1061 +0,0 @@ - - - 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 - - - - - - - - - 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 - - - - - - - - - - Recepcion - - - - - - - - - - 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 index bee159a52c6e0231623309595d609ff5ae3eeac7..e064f22c27b606f585dea37f0d8a21915c4b3eda 100644 --- a/src/ui/SlugsPadCameraControl.cpp +++ b/src/ui/SlugsPadCameraControl.cpp @@ -1,275 +1,201 @@ -#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; - bearingPad = 0; - distancePad = 0; - directionPad = "no"; - -} - -SlugsPadCameraControl::~SlugsPadCameraControl() -{ - delete ui; -} - -void SlugsPadCameraControl::mouseMoveEvent(QMouseEvent *event) -{ - Q_UNUSED(event); - //emit mouseMoveCoord(event->x(),event->y()); - - if(dragging) - { - - // getDeltaPositionPad(event->x(), event->y()); - - } - - -} - -void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) -{ - //emit mousePressCoord(event->x(),event->y()); - dragging = true; - x1 = event->x(); - y1 = event->y(); - -} - -void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) -{ - dragging = false; - //emit mouseReleaseCoord(event->x(),event->y()); - getDeltaPositionPad(event->x(), event->y()); - - xFin = event->x(); - yFin = 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.setPen(Qt::white); - - //QPointF coordTemp = getPointBy_BearingDistance(ui->frame->width()/2,ui->frame->height()/2,bearingPad,distancePad); - - painter.drawLine(QPoint(ui->frame->width()/2,ui->frame->height()/2), - QPoint(xFin,yFin)); - - - // 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); - - // this only convert real bearing to frame widget bearing - bearing = bearing +90; - if(bearing>= 360) bearing = bearing - 360; - - - - 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 = "right up"; - } - else - { - if((bearing > 60)&&(bearing <= 105) ) - { - emit dirCursorText("right"); - //bearing = 315; - dir = "right"; - } - else - { - if((bearing > 105)&&(bearing <= 150) ) - { - emit dirCursorText("right down"); - //bearing = 315; - dir = "right 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"; - } - - } - - } - - } - - } - - } - - } - - } - - - bearingPad = bearing; - distancePad = dist; - directionPad = dir; - emit changeCursorPosition(bearing, dist, dir); - - update(); - - - -} - -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; -} - - -QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, double lat1, - double lon2, double lat2) -{ - double cateto_opuesto,cateto_adyacente, hipotenusa, distancia; - double marcacion = 0.0; - - //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.0; - - - 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; - - - - return QPointF(marcacion,distancia); - -} - - - -QPointF SlugsPadCameraControl::getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia) -{ - double lon2 = 0; - double lat2 = 0; - double rad= M_PI/180; - - rumbo = rumbo*rad; - lon2=(lon1 + ((distancia/60) * (sin(rumbo)))); - lat2=(lat1 + ((distancia/60) * (cos(rumbo)))); - - return QPointF(lon2,lat2); -} - +#include "SlugsPadCameraControl.h" +#include "ui_SlugsPadCameraControl.h" + +SlugsPadCameraControl::SlugsPadCameraControl(QWidget *parent) : + QWidget(parent), + ui(new Ui::SlugsPadCameraControl), + dragging(0) +{ + ui->setupUi(this); + x1= 0; + y1 = 0; + motion = NONE; +} + +SlugsPadCameraControl::~SlugsPadCameraControl() +{ + delete ui; +} + +void SlugsPadCameraControl::activeUasSet(UASInterface *uas) +{ + if(uas) + { + this->activeUAS= uas; + } +} + +void SlugsPadCameraControl::mouseMoveEvent(QMouseEvent *event) +{ + Q_UNUSED(event); + + if(dragging) + { + getDeltaPositionPad(event->x(), event->y()); + } +} + +void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) +{ + if(!dragging) + { + dragging = true; + x1 = event->x(); + y1 = event->y(); + } +} + +void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) +{ + if(dragging) + { + dragging = false; + getDeltaPositionPad(event->x(), event->y()); + + xFin = event->x(); + yFin = event->y(); + } +} + +void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) +{ + QPointF localMeasures = ObtenerMarcacionDistanciaPixel(y1,x1,y2,x2); + + if(localMeasures.y()>10) + { + QString dir = "nd"; + + double bearing = localMeasures.x(); + + bearing = bearing +90; + + if(bearing>= 360) + { + bearing = bearing - 360; + } + + if(bearing >337.5 || bearing <=22.5) + { + motion= UP; + movePad = QPoint(0, 1); + dir = "UP"; + } + else if(bearing >22.5 && bearing <=67.5) + { + motion= RIGHT_UP; + movePad = QPoint(1, 1); + dir = "RIGHT UP"; + } + else if(bearing >67.5 && bearing <=112.5) + { + motion= RIGHT; + movePad = QPoint(1, 0); + dir = "RIGHT"; + } + else if(bearing >112.5 && bearing <= 157.5) + { + motion= RIGHT_DOWN; + movePad = QPoint(1, -1); + dir = "RIGHT DOWN"; + } + else if(bearing >157.5 && bearing <=202.5) + { + motion= DOWN; + movePad = QPoint(0, -1); + dir = "DOWN"; + } + else if(bearing >202.5 && bearing <=247.5) + { + motion= LEFT_DOWN; + movePad = QPoint(-1, -1); + dir = "LEFT DOWN"; + } + else if(bearing >247.5 && bearing <=292.5) + { + motion= LEFT; + movePad = QPoint(-1, 0); + dir = "LEFT"; + } + else if(bearing >292.5 && bearing <=337.5) + { + motion= LEFT_UP; + movePad = QPoint(-1, 1); + dir = "LEFT UP"; + } + + emit changeMotionCamera(motion); + + ui->lbPixel->setText(QString::number(localMeasures.y())); + ui->lbDirection->setText(dir); + + //qDebug()< 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; + + return QPointF(marcacion,hipotenusa);// distancia); +} + +void SlugsPadCameraControl::keyPressEvent(QKeyEvent *event) +{ + switch (event->key()) + { + case Qt::Key_Left: + emit changeMotionCamera(LEFT); + break; + + case Qt::Key_Right: + emit changeMotionCamera(RIGHT); + break; + + case Qt::Key_Down: + emit changeMotionCamera(DOWN); + break; + + case Qt::Key_Up: + emit changeMotionCamera(UP); + break; + + default: + QWidget::keyPressEvent(event); + } +} diff --git a/src/ui/SlugsPadCameraControl.h b/src/ui/SlugsPadCameraControl.h index dbf555088868714785ea3b04530003f55d1658a4..32e8e2e6b5cbbaa702970fd489d7a267efc49ed1 100644 --- a/src/ui/SlugsPadCameraControl.h +++ b/src/ui/SlugsPadCameraControl.h @@ -1,60 +1,68 @@ -#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); - QPointF getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia); - - - - -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; - int xFin; - int yFin; - double bearingPad; - double distancePad; - QString directionPad; - -}; - -#endif // SLUGSPADCAMERACONTROL_H +#ifndef SLUGSPADCAMERACONTROL_H +#define SLUGSPADCAMERACONTROL_H + +#include +#include +#include +#include +#include +#include +#include +#include "UASManager.h" + +namespace Ui { + class SlugsPadCameraControl; +} + +class SlugsPadCameraControl : public QWidget //QGraphicsView// +{ + Q_OBJECT + +public: + explicit SlugsPadCameraControl(QWidget *parent = 0); + + ~SlugsPadCameraControl(); + + enum MotionCamera { + UP, + DOWN, + LEFT, + RIGHT, + RIGHT_UP, + RIGHT_DOWN, + LEFT_UP, + LEFT_DOWN, + NONE + }; + +public slots: + void getDeltaPositionPad(int x, int y); + QPointF ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2); + void activeUasSet(UASInterface *uas); + +signals: + void changeMotionCamera(MotionCamera); + +protected: + void mousePressEvent(QMouseEvent* event); + void mouseReleaseEvent(QMouseEvent* event); + void mouseMoveEvent(QMouseEvent* event); + void keyPressEvent(QKeyEvent *event); + //void paintEvent(QPaintEvent *pe); + + +private: + Ui::SlugsPadCameraControl *ui; + bool dragging; + int x1; + int y1; + int xFin; + int yFin; + QString directionPad; + MotionCamera motion; + UASInterface* activeUAS; + QPoint movePad; + +}; + +#endif // SLUGSPADCAMERACONTROL_H diff --git a/src/ui/SlugsPadCameraControl.ui b/src/ui/SlugsPadCameraControl.ui index f96e02a006d094f0e8a0fcab16ebfe8506c07bb8..5b7fa9310714be5128dc4e6dce23d08674373f2c 100644 --- a/src/ui/SlugsPadCameraControl.ui +++ b/src/ui/SlugsPadCameraControl.ui @@ -1,52 +1,94 @@ - - - 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 - - - - - - - - + + + SlugsPadCameraControl + + + + 0 + 0 + 200 + 200 + + + + + 200 + 200 + + + + Form + + + background-color: rgb(255, 170, 0); + + + + 1 + + + 1 + + + + + + 200 + 200 + + + + true + + + background-color: rgb(135, 206, 235); + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + Qt::Vertical + + + + 20 + 156 + + + + + + + + + + + + + ---- + + + + + + + ---- + + + + + + + + + + + + + diff --git a/src/ui/SlugsVideoCamControl.cpp b/src/ui/SlugsVideoCamControl.cpp index a7c9c67624e450342302e0b047f1a69aa5ebb210..5d3645517b8199cff1f2967bb9a7084490222239 100644 --- a/src/ui/SlugsVideoCamControl.cpp +++ b/src/ui/SlugsVideoCamControl.cpp @@ -1,95 +1,95 @@ -#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) -{ - ui->setupUi(this); -// x1= 0; -// y1 = 0; - - connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); - 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))); - - -} - -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); -} - +#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) +{ + ui->setupUi(this); +// x1= 0; +// y1 = 0; + + connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); + 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))); + + +} + +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 deleted file mode 100644 index 0180fd52a5c8737d73a1104113548c62715e30ea..0000000000000000000000000000000000000000 --- a/src/ui/SlugsVideoCamControl.h +++ /dev/null @@ -1,78 +0,0 @@ -#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: - /** - * @brief status = true: emit signal to draw a border cam over the map - */ - void changeViewCamBorderAtMapStatus(bool status); - /** - * @brief show the values of mousepad on ui (labels) and emit a changeCamPosition(signal) - * with values: - * bearing and distance from mouse over the pad - * dirText: direction of mouse movement in text format (up, right,right up,right down, - * left, left up, left down, down) - */ - void getDeltaPositionPad(double bearing, double distance, QString dirText); - -// /** -// * @brief -// */ -// void mousePadPressEvent(int x, int y); -// void mousePadReleaseEvent(int x, int y); -// void mousePadMoveEvent(int x, int y); - -signals: - /** - * @brief emit values from mousepad: - * bearing and distance from mouse over the pad - * dirText: direction of mouse movement in text format (up, right,right up,right down, - * left, left up, left down, down) - */ - void changeCamPosition(double distance, double bearing, QString textDir); - /** - * @brief emit signal to draw a border cam over the map if status is true - */ - 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 deleted file mode 100644 index 7bbe214933ae49d2fc3bc199e2161806de3d4178..0000000000000000000000000000000000000000 --- a/src/ui/SlugsVideoCamControl.ui +++ /dev/null @@ -1,68 +0,0 @@ - - - SlugsVideoCamControl - - - - 0 - 0 - 165 - 191 - - - - - 0 - 0 - - - - true - - - Form - - - - - - - - true - - - Coord_X - - - - - - - true - - - Coord_Y - - - - - - - Camera at Map - - - - - - - Camera Pos - - - - - - - - - - diff --git a/src/ui/UASControlParameters.ui b/src/ui/UASControlParameters.ui new file mode 100644 index 0000000000000000000000000000000000000000..48bd71db22adda4f6b1572a04a291a5c690671a3 --- /dev/null +++ b/src/ui/UASControlParameters.ui @@ -0,0 +1,246 @@ + + + UASControlParameters + + + + 0 + 0 + 204 + 246 + + + + + 200 + 150 + + + + + 267 + 16777215 + + + + Form + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 100 + 0 + + + + + 16777215 + 16777215 + + + + ---- + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 0 + + + + Commands + + + + + + + + Height (m) + + + + + + + 1500.000000000000000 + + + + + + + + + + + Airspeed (m/s) + + + + + + + 500.000000000000000 + + + + + + + + + + + Turn Rate (rad/s) + + + + + + + 180.000000000000000 + + + + + + + + + + + Set + + + + + + + Get + + + + + + + + + Qt::Vertical + + + + 20 + 27 + + + + + + + + + Passthrough + + + + + + + + Elevator + + + + + + + Rudder + + + + + + + Throttle + + + + + + + Ailerons + + + + + + + + + Set + + + + + + + Qt::Vertical + + + + 20 + 26 + + + + + + + + + + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index e34d351dce7b213d067b8283ad412c3b33988639..e04b51c8f5bbe038b7a3d4bd210b947a94d4fa11 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -190,12 +190,13 @@ void WaypointList::add() // Create global frame waypoint per default wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, 0.0, 0.0, 0.0, true, true, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); uas->getWaypointManager()->addWaypoint(wp); + } } } -void WaypointList::addCurrentPositonWaypoint() -{ +void WaypointList::addCurrentPositonWaypoint(){ + /* TODO: implement with new waypoint structure if (uas) { @@ -462,6 +463,9 @@ void WaypointList::moveUp(Waypoint* wp) uas->getWaypointManager()->moveWaypoint(i, i-1); } } + + //emitir seal de cambio orden en la lista, + //la debe capturar el mapwidget para volver a dibujar la ruta } void WaypointList::moveDown(Waypoint* wp) @@ -521,13 +525,6 @@ void WaypointList::on_clearWPListButton_clicked() widget->remove(); } } - else - { -// if(isGlobalWP) -// { -// emit clearPathclicked(); -// } - } } ///** @brief The MapWidget informs that a waypoint global was changed on the map */ diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index e07c701c2e7ebc5085a8a8e2036cd5bddebda6bf..5d5023a84a21890dad075462349b2b052268c037 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -108,7 +108,7 @@ public slots: signals: void clearPathclicked(); void createWaypointAtMap(const QPointF coordinate); - + void changePointList(); protected: virtual void changeEvent(QEvent *e); diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index c4218bd1e7d3205958a44cd4aea067396fd40e72..00ca2a11f7227de12f8cb8e571064f72551b11b6 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -113,6 +113,7 @@ void WaypointView::moveDown() emit moveDownWaypoint(wp); } + void WaypointView::remove() { emit removeWaypoint(wp); diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc index 9ff0639f8ec345a76d202a295f378f6914d099dd..b32b4f06f117f6e8edc2ad6fe209cb60ddb214f9 100644 --- a/src/ui/designer/QGCActionButton.cc +++ b/src/ui/designer/QGCActionButton.cc @@ -1,139 +1,139 @@ -#include "QGCActionButton.h" -#include "ui_QGCActionButton.h" - -#include "MAVLinkProtocol.h" -#include "UASManager.h" - -const char* kActionLabels[MAV_ACTION_NB] = -{"HOLD", - "START MOTORS", - "LAUNCH", - "RETURN", - "EMERGENCY LAND", - "EMERGENCY KILL", - "CONFIRM KILL", - "CONTINUE", - "STOP MOTORS", - "HALT", - "SHUTDOWN", - "REBOOT", - "SET MANUAL", - "SET AUTO", - "READ STORAGE", - "WRITE STORAGE", - "CALIBRATE RC", - "CALIBRATE GYRO", - "CALIBRATE MAG", - "CALIBRATE ACC", - "CALIBRATE PRESSURE", - "START REC", - "PAUSE REC", - "STOP REC", - "TAKEOFF", - "NAVIGATE", - "LAND", - "LOITER", - "SET ORIGIN", - "RELAY ON", - "RELAY OFF", - "GET IMAGE", - "START VIDEO", - "STOP VIDEO", - "RESET MAP", - "RESET PLAN"}; - -QGCActionButton::QGCActionButton(QWidget *parent) : - QGCToolWidgetItem("Button", parent), - ui(new Ui::QGCActionButton), - uas(NULL) -{ - ui->setupUi(this); - - connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); - connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); - connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); - connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); - - // Hide all edit items - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // add action labels to combobox - for (int i = 0; i < MAV_ACTION_NB; i++) - { - ui->editActionComboBox->addItem(kActionLabels[i]); - } -} - -QGCActionButton::~QGCActionButton() -{ - delete ui; -} - -void QGCActionButton::sendAction() -{ - if (QGCToolWidgetItem::uas) - { - MAV_ACTION action = static_cast( - ui->editActionComboBox->currentIndex()); - - QGCToolWidgetItem::uas->setAction(action); - } - else - { - qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; - } -} - -void QGCActionButton::setActionButtonName(QString text) -{ - ui->actionButton->setText(text); -} - -void QGCActionButton::startEditMode() -{ - ui->editActionComboBox->show(); - ui->editActionsRefreshButton->show(); - ui->editFinishButton->show(); - ui->editNameLabel->show(); - ui->editButtonName->show(); - isInEditMode = true; -} - -void QGCActionButton::endEditMode() -{ - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // Write to settings - emit editingFinished(); - - isInEditMode = false; -} - -void QGCActionButton::writeSettings(QSettings& settings) -{ - settings.setValue("TYPE", "BUTTON"); - settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); - settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); - settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); - settings.sync(); -} - -void QGCActionButton::readSettings(const QSettings& settings) -{ - ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - - ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - qDebug() << "DONE READING SETTINGS"; -} +#include "QGCActionButton.h" +#include "ui_QGCActionButton.h" + +#include "MAVLinkProtocol.h" +#include "UASManager.h" + +const char* kActionLabels[MAV_ACTION_NB] = +{"HOLD", + "START MOTORS", + "LAUNCH", + "RETURN", + "EMERGENCY LAND", + "EMERGENCY KILL", + "CONFIRM KILL", + "CONTINUE", + "STOP MOTORS", + "HALT", + "SHUTDOWN", + "REBOOT", + "SET MANUAL", + "SET AUTO", + "READ STORAGE", + "WRITE STORAGE", + "CALIBRATE RC", + "CALIBRATE GYRO", + "CALIBRATE MAG", + "CALIBRATE ACC", + "CALIBRATE PRESSURE", + "START REC", + "PAUSE REC", + "STOP REC", + "TAKEOFF", + "NAVIGATE", + "LAND", + "LOITER", + "SET ORIGIN", + "RELAY ON", + //"RELAY OFF", + //"GET IMAGE", + //"START VIDEO", + //"STOP VIDEO", + "RESET MAP", + "RESET PLAN"}; + +QGCActionButton::QGCActionButton(QWidget *parent) : + QGCToolWidgetItem("Button", parent), + ui(new Ui::QGCActionButton), + uas(NULL) +{ + ui->setupUi(this); + + connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); + connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); + connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); + connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); + + // Hide all edit items + ui->editActionComboBox->hide(); + ui->editActionsRefreshButton->hide(); + ui->editFinishButton->hide(); + ui->editNameLabel->hide(); + ui->editButtonName->hide(); + + // add action labels to combobox + for (int i = 0; i < MAV_ACTION_NB; i++) + { + ui->editActionComboBox->addItem(kActionLabels[i]); + } +} + +QGCActionButton::~QGCActionButton() +{ + delete ui; +} + +void QGCActionButton::sendAction() +{ + if (QGCToolWidgetItem::uas) + { + MAV_ACTION action = static_cast( + ui->editActionComboBox->currentIndex()); + + QGCToolWidgetItem::uas->setAction(action); + } + else + { + qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; + } +} + +void QGCActionButton::setActionButtonName(QString text) +{ + ui->actionButton->setText(text); +} + +void QGCActionButton::startEditMode() +{ + ui->editActionComboBox->show(); + ui->editActionsRefreshButton->show(); + ui->editFinishButton->show(); + ui->editNameLabel->show(); + ui->editButtonName->show(); + isInEditMode = true; +} + +void QGCActionButton::endEditMode() +{ + ui->editActionComboBox->hide(); + ui->editActionsRefreshButton->hide(); + ui->editFinishButton->hide(); + ui->editNameLabel->hide(); + ui->editButtonName->hide(); + + // Write to settings + emit editingFinished(); + + isInEditMode = false; +} + +void QGCActionButton::writeSettings(QSettings& settings) +{ + settings.setValue("TYPE", "BUTTON"); + settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); + settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); + settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); + settings.sync(); +} + +void QGCActionButton::readSettings(const QSettings& settings) +{ + ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); + ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); + ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); + + ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); + ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); + ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); + qDebug() << "DONE READING SETTINGS"; +} diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 0ffea43d91e68041b33af138b92ca8fa4895932e..4f08956decc0c266e0b41200c79f0c330dca5d48 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -95,14 +95,13 @@ void Linecharts::addSystem(UASInterface* uas) LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this); addWidget(widget); plots.insert(uas->getUASID(), widget); -#ifndef MAVLINK_ENABLED_SLUGS // Values without unit //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); // Values with unit as double connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); // Values with unit as integer connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), widget, SLOT(appendData(int,QString,QString,int,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/uas/UASControlParameters.cpp b/src/ui/uas/UASControlParameters.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f04d77f29b45b0a68cc46945dda0c239ce4610b3 --- /dev/null +++ b/src/ui/uas/UASControlParameters.cpp @@ -0,0 +1,237 @@ +#include "UASControlParameters.h" +#include "ui_UASControlParameters.h" + +#define CONTROL_MODE_LOCKED "MODE LOCKED" +#define CONTROL_MODE_MANUAL "MODE MANUAL" + +#ifdef MAVLINK_ENABLED_SLUGS + #define CONTROL_MODE_GUIDED "MODE MID-L CMDS" + #define CONTROL_MODE_AUTO "MODE WAYPOINT" + #define CONTROL_MODE_TEST1 "MODE PASST" + #define CONTROL_MODE_TEST2 "MODE SEL PT" +#else + #define CONTROL_MODE_GUIDED "MODE GUIDED" + #define CONTROL_MODE_AUTO "MODE AUTO" + #define CONTROL_MODE_TEST1 "MODE TEST1" + #define CONTROL_MODE_TEST2 "MODE TEST2" +#endif + +#define CONTROL_MODE_READY "MODE TEST3" +#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" + +#define CONTROL_MODE_LOCKED_INDEX 1 +#define CONTROL_MODE_MANUAL_INDEX 2 +#define CONTROL_MODE_GUIDED_INDEX 3 +#define CONTROL_MODE_AUTO_INDEX 4 +#define CONTROL_MODE_TEST1_INDEX 5 +#define CONTROL_MODE_TEST2_INDEX 6 +#define CONTROL_MODE_TEST3_INDEX 7 +#define CONTROL_MODE_READY_INDEX 8 +#define CONTROL_MODE_RC_TRAINING_INDEX 9 + +UASControlParameters::UASControlParameters(QWidget *parent) : + QWidget(parent), + ui(new Ui::UASControlParameters) +{ + ui->setupUi(this); + + ui->btSetCtrl->setStatusTip(tr("Set Passthrough")); + + connect(ui->btGetCommands, SIGNAL(clicked()), this, SLOT(getCommands())); + + connect(ui->btSetCtrl, SIGNAL(clicked()), this, SLOT(setPassthrough())); +} + +UASControlParameters::~UASControlParameters() +{ + delete ui; +} + +void UASControlParameters::changedMode(int mode) +{ + QString modeTemp; + + switch (mode) + { + case (uint8_t)MAV_MODE_LOCKED: + modeTemp = "LOCKED MODE"; + break; + case (uint8_t)MAV_MODE_MANUAL: + modeTemp = "MANUAL MODE"; + break; +#ifdef MAVLINK_ENABLED_SLUGS + case (uint8_t)MAV_MODE_AUTO: + modeTemp = "WAYPOINT MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + modeTemp = "MID-L CMDS MODE"; + break; + + case (uint8_t)MAV_MODE_TEST1: + modeTemp = "PASST MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + modeTemp = "SEL PT MODE"; + break; +#else + case (uint8_t)MAV_MODE_AUTO: + mode = "AUTO MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + mode = "GUIDED MODE"; + break; + + case (uint8_t)MAV_MODE_TEST1: + mode = "TEST1 MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + mode = "TEST2 MODE"; + break; +#endif + case (uint8_t)MAV_MODE_READY: + modeTemp = "READY MODE"; + break; + break; + case (uint8_t)MAV_MODE_TEST3: + modeTemp = "TEST3 MODE"; + break; + case (uint8_t)MAV_MODE_RC_TRAINING: + modeTemp = "RC TRAINING MODE"; + break; + default: + modeTemp = "UNINIT MODE"; + break; + } + + + if(modeTemp != this->mode) + { + ui->lbMode->setStyleSheet("background-color: rgb(165, 42, 42)"); + } + else + { + ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)"); + } +} + +void UASControlParameters::activeUasSet(UASInterface *uas) +{ + if(uas) + { + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) ); + + activeUAS= uas; + } +} + +void UASControlParameters::updateGlobalPosition(UASInterface * a, double b, double c, double aa, quint64 ab) +{ + Q_UNUSED(a); + Q_UNUSED(b); + Q_UNUSED(c); + Q_UNUSED(ab); + this->altitude=aa; +} + +void UASControlParameters::speedChanged(UASInterface* uas, double vx, double vy, double vz, quint64 time) +{ + Q_UNUSED(time); + Q_UNUSED(uas); + this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0)); + //ui->sbAirSpeed->setValue(speed); +} + +void UASControlParameters::updateAttitude(UASInterface *uas, double roll, double pitch, double yaw, quint64 time) +{ + Q_UNUSED(uas); + Q_UNUSED(pitch); + Q_UNUSED(yaw); + Q_UNUSED(time); + //ui->sbTurnRate->setValue(roll); + this->roll = roll; +} + +void UASControlParameters::setCommands() +{ + if(this->activeUAS) + { + UAS* myUas= static_cast(this->activeUAS); + + mavlink_message_t msg; + + tempCmds.uCommand = ui->sbAirSpeed->value(); + tempCmds.hCommand = ui->sbHeight->value(); + tempCmds.rCommand = ui->sbTurnRate->value(); + + mavlink_msg_mid_lvl_cmds_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCmds); + myUas->sendMessage(msg); + } +} + +void UASControlParameters::getCommands() +{ + ui->sbAirSpeed->setValue(this->speed); + ui->sbHeight->setValue(this->altitude); + ui->sbTurnRate->setValue(this->roll); +} + +void UASControlParameters::setPassthrough() +{ + if(this->activeUAS) + { + UAS* myUas= static_cast(this->activeUAS); + + mavlink_message_t msg; + + int8_t tmpBit=0; + + if(ui->cxdle_c->isChecked())//left elevator command + { + tmpBit+=8; + } + if(ui->cxdr_c->isChecked())//rudder command + { + tmpBit+=16; + } + + if(ui->cxdla_c->isChecked())//left aileron command + { + tmpBit+=64; + } + if(ui->cxdt_c->isChecked())//throttle command + { + tmpBit+=128; + } + + generic_16bit r; + r.b[1] = 0; + r.b[0] = tmpBit;//255; + + tempCtrl.target= this->activeUAS->getUASID(); + tempCtrl.bitfieldPt= (uint16_t)r.s; + + mavlink_msg_ctrl_srfc_pt_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &this->tempCtrl); + myUas->sendMessage(msg); + //qDebug()<mode = mode; + ui->lbMode->setText(this->mode); + + ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)"); +} + +void UASControlParameters::thrustChanged(UASInterface *uas, double throttle) +{ + Q_UNUSED(uas); + this->throttle= throttle; +} diff --git a/src/ui/uas/UASControlParameters.h b/src/ui/uas/UASControlParameters.h new file mode 100644 index 0000000000000000000000000000000000000000..c21f5e53dbc2e3b8bae88da086f97d673b88f1a5 --- /dev/null +++ b/src/ui/uas/UASControlParameters.h @@ -0,0 +1,51 @@ +#ifndef UASCONTROLPARAMETERS_H +#define UASCONTROLPARAMETERS_H + +#include +#include "UASManager.h" +#include "SlugsMAV.h" +#include +#include + +namespace Ui { + class UASControlParameters; +} + +class UASControlParameters : public QWidget +{ + Q_OBJECT + +public: + explicit UASControlParameters(QWidget *parent = 0); + ~UASControlParameters(); + +public slots: + void changedMode(int mode); + void activeUasSet(UASInterface* uas); + void updateGlobalPosition(UASInterface*,double,double,double,quint64); + void speedChanged(UASInterface*,double,double,double,quint64); + void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time); + void setCommands(); + void getCommands(); + + void setPassthrough(); + void updateMode(int uas,QString mode,QString description); + void thrustChanged(UASInterface* uas,double throttle); + +private: + Ui::UASControlParameters *ui; + QTimer* refreshTimerGet; + UASInterface* activeUAS; + double speed; + double roll; + double altitude; + double throttle; + QString mode; + QString REDcolorStyle; + QPointer radio; + LinkInterface* hilLink; + mavlink_mid_lvl_cmds_t tempCmds; + mavlink_ctrl_srfc_pt_t tempCtrl; +}; + +#endif // UASCONTROLPARAMETERS_H diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 359c7a96f77aec305f0e8cb14269e501547af597..278a5328bb2eb48f5a785bcaff1fc2935971ccfe 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -1,293 +1,306 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009, 2010 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK 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. - - PIXHAWK 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 PIXHAWK. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Definition of widget controlling one MAV - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include "UASControlWidget.h" -#include -#include - -#define CONTROL_MODE_LOCKED "MODE LOCKED" -#define CONTROL_MODE_MANUAL "MODE MANUAL" -#define CONTROL_MODE_GUIDED "MODE GUIDED" -#define CONTROL_MODE_AUTO "MODE AUTO" -#define CONTROL_MODE_TEST1 "MODE TEST1" -#define CONTROL_MODE_TEST2 "MODE TEST2" -#define CONTROL_MODE_TEST3 "MODE TEST3" -#define CONTROL_MODE_READY "MODE TEST3" -#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" - -#define CONTROL_MODE_LOCKED_INDEX 1 -#define CONTROL_MODE_MANUAL_INDEX 2 -#define CONTROL_MODE_GUIDED_INDEX 3 -#define CONTROL_MODE_AUTO_INDEX 4 -#define CONTROL_MODE_TEST1_INDEX 5 -#define CONTROL_MODE_TEST2_INDEX 6 -#define CONTROL_MODE_TEST3_INDEX 7 -#define CONTROL_MODE_READY_INDEX 8 -#define CONTROL_MODE_RC_TRAINING_INDEX 9 - -UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), -uas(0), -engineOn(false) -{ - ui.setupUi(this); - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - ui.modeComboBox->clear(); - ui.modeComboBox->insertItem(0, "Select.."); - ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED); - ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL); - ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED); - ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2); - ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3); - ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY); - ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING); - connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); - connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); - - ui.modeComboBox->setCurrentIndex(0); - - ui.gridLayout->setAlignment(Qt::AlignTop); -} - -void UASControlWidget::setUAS(UASInterface* uas) -{ - if (this->uas != 0) - { - UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas); - disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(enable_motors())); - disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); - disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); - disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); - //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); - disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - } - - // Connect user interface controls - connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); - connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); - connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); - connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); - //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); - connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); - - ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); - -// // Check if additional controls should be loaded -// UAS* mav = dynamic_cast(uas); -// if (mav) -// { -// QPushButton* startRecButton = new QPushButton(tr("Record")); -// connect(startRecButton, SIGNAL(clicked()), mav, SLOT(startDataRecording())); -// ui.gridLayout->addWidget(startRecButton, 7, 1); - -// QPushButton* pauseRecButton = new QPushButton(tr("Pause")); -// connect(pauseRecButton, SIGNAL(clicked()), mav, SLOT(pauseDataRecording())); -// ui.gridLayout->addWidget(pauseRecButton, 7, 3); - -// QPushButton* stopRecButton = new QPushButton(tr("Stop")); -// connect(stopRecButton, SIGNAL(clicked()), mav, SLOT(stopDataRecording())); -// ui.gridLayout->addWidget(stopRecButton, 7, 4); -// } - - - this->uas = uas->getUASID(); - setBackgroundColor(uas->getColor()); -} - -UASControlWidget::~UASControlWidget() -{ - -} - -void UASControlWidget::updateStatemachine() -{ - - if (engineOn) - { - ui.controlButton->setText(tr("Stop Engine")); - } - else - { - ui.controlButton->setText(tr("Activate Engine")); - } -} - -/** - * Set the background color based on the MAV color. If the MAV is selected as the - * currently actively controlled system, the frame color is highlighted - */ -void UASControlWidget::setBackgroundColor(QColor color) -{ - // UAS color - QColor uasColor = color; - QString colorstyle; - QString borderColor = "#4A4A4F"; - borderColor = "#FA4A4F"; - uasColor = uasColor.darker(900); - colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }", - uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); - setStyleSheet(colorstyle); - QPalette palette = this->palette(); - palette.setBrush(QPalette::Window, QBrush(uasColor)); - setPalette(palette); - setAutoFillBackground(true); -} - - -void UASControlWidget::updateMode(int uas,QString mode,QString description) -{ - Q_UNUSED(uas); - Q_UNUSED(mode); - Q_UNUSED(description); -} - -void UASControlWidget::updateState(int state) -{ - switch (state) - { - case (int)MAV_STATE_ACTIVE: - engineOn = true; - ui.controlButton->setText(tr("Stop Engine")); - break; - case (int)MAV_STATE_STANDBY: - engineOn = false; - ui.controlButton->setText(tr("Activate Engine")); - break; - } -} - -void UASControlWidget::setMode(int mode) -{ - // Adapt context button mode - if (mode == CONTROL_MODE_LOCKED_INDEX) - { - uasMode = (unsigned int)MAV_MODE_LOCKED; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_MANUAL_INDEX) - { - uasMode = (unsigned int)MAV_MODE_MANUAL; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_GUIDED_INDEX) - { - uasMode = (unsigned int)MAV_MODE_GUIDED; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_AUTO_INDEX) - { - uasMode = (unsigned int)MAV_MODE_AUTO; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST1_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST1; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST2_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST2; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_TEST3_INDEX) - { - uasMode = (unsigned int)MAV_MODE_TEST3; - ui.modeComboBox->setCurrentIndex(mode); - } - else if (mode == CONTROL_MODE_RC_TRAINING_INDEX) - { - uasMode = (unsigned int)MAV_MODE_RC_TRAINING; - ui.modeComboBox->setCurrentIndex(mode); - } - else - { - qDebug() << "ERROR! MODE NOT FOUND"; - uasMode = 0; - } - - - qDebug() << "SET MODE REQUESTED" << uasMode; -} - -void UASControlWidget::transmitMode() -{ - if (uasMode != 0) - { - UASInterface* mav = UASManager::instance()->getUASForId(this->uas); - if (mav) - { - mav->setMode(uasMode); - ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(mav->getUASName())); - } - } -} - -void UASControlWidget::cycleContextButton() -{ - UAS* mav = dynamic_cast(UASManager::instance()->getUASForId(this->uas)); - if (mav) - { - - if (!engineOn) - { - mav->enable_motors(); - ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName())); - } - else - { - mav->disable_motors(); - ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName())); - } - // Update state now and in several intervals when MAV might have changed state - updateStatemachine(); - - QTimer::singleShot(50, this, SLOT(updateStatemachine())); - QTimer::singleShot(200, this, SLOT(updateStatemachine())); - - //ui.controlButton->setText(tr("Force Landing")); - //ui.controlButton->setText(tr("KILL VEHICLE")); - } - -} - +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK 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. + + PIXHAWK 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 PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of widget controlling one MAV + * + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "UASControlWidget.h" +#include +#include +#include "QGC.h" + +#define CONTROL_MODE_LOCKED "MODE LOCKED" +#define CONTROL_MODE_MANUAL "MODE MANUAL" + +#ifdef MAVLINK_ENABLED_SLUGS + #define CONTROL_MODE_GUIDED "MODE MID-L CMDS" + #define CONTROL_MODE_AUTO "MODE WAYPOINT" + #define CONTROL_MODE_TEST1 "MODE PASST" + #define CONTROL_MODE_TEST2 "MODE SEL PT" +#else + #define CONTROL_MODE_GUIDED "MODE GUIDED" + #define CONTROL_MODE_AUTO "MODE AUTO" + #define CONTROL_MODE_TEST1 "MODE TEST1" + #define CONTROL_MODE_TEST2 "MODE TEST2" +#endif + +#define CONTROL_MODE_TEST3 "MODE TEST3" +#define CONTROL_MODE_READY "MODE READY" +#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" + +#define CONTROL_MODE_LOCKED_INDEX 1 +#define CONTROL_MODE_MANUAL_INDEX 2 +#define CONTROL_MODE_GUIDED_INDEX 3 +#define CONTROL_MODE_AUTO_INDEX 4 +#define CONTROL_MODE_TEST1_INDEX 5 +#define CONTROL_MODE_TEST2_INDEX 6 +#define CONTROL_MODE_TEST3_INDEX 7 +#define CONTROL_MODE_READY_INDEX 8 +#define CONTROL_MODE_RC_TRAINING_INDEX 9 + +UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), +uas(0), +engineOn(false) +{ + ui.setupUi(this); + + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + ui.modeComboBox->clear(); + ui.modeComboBox->insertItem(0, "Select.."); + ui.modeComboBox->insertItem(CONTROL_MODE_LOCKED_INDEX, CONTROL_MODE_LOCKED); + ui.modeComboBox->insertItem(CONTROL_MODE_MANUAL_INDEX, CONTROL_MODE_MANUAL); + ui.modeComboBox->insertItem(CONTROL_MODE_GUIDED_INDEX, CONTROL_MODE_GUIDED); + ui.modeComboBox->insertItem(CONTROL_MODE_AUTO_INDEX, CONTROL_MODE_AUTO); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST1_INDEX, CONTROL_MODE_TEST1); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST2_INDEX, CONTROL_MODE_TEST2); + ui.modeComboBox->insertItem(CONTROL_MODE_TEST3_INDEX, CONTROL_MODE_TEST3); + ui.modeComboBox->insertItem(CONTROL_MODE_READY_INDEX, CONTROL_MODE_READY); + ui.modeComboBox->insertItem(CONTROL_MODE_RC_TRAINING_INDEX, CONTROL_MODE_RC_TRAINING); + connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); + connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); + + ui.modeComboBox->setCurrentIndex(0); + + ui.gridLayout->setAlignment(Qt::AlignTop); + +} + +void UASControlWidget::setUAS(UASInterface* uas) +{ + if (this->uas != 0) + { + UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas); + disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(enable_motors())); + disconnect(ui.liftoffButton, SIGNAL(clicked()), oldUAS, SLOT(launch())); + disconnect(ui.landButton, SIGNAL(clicked()), oldUAS, SLOT(home())); + disconnect(ui.shutdownButton, SIGNAL(clicked()), oldUAS, SLOT(shutdown())); + //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); + disconnect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + disconnect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + } + + // Connect user interface controls + connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton())); + connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); + connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home())); + connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); + //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition())); + connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int))); + + ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName()); + +// // Check if additional controls should be loaded +// UAS* mav = dynamic_cast(uas); +// if (mav) +// { +// QPushButton* startRecButton = new QPushButton(tr("Record")); +// connect(startRecButton, SIGNAL(clicked()), mav, SLOT(startDataRecording())); +// ui.gridLayout->addWidget(startRecButton, 7, 1); + +// QPushButton* pauseRecButton = new QPushButton(tr("Pause")); +// connect(pauseRecButton, SIGNAL(clicked()), mav, SLOT(pauseDataRecording())); +// ui.gridLayout->addWidget(pauseRecButton, 7, 3); + +// QPushButton* stopRecButton = new QPushButton(tr("Stop")); +// connect(stopRecButton, SIGNAL(clicked()), mav, SLOT(stopDataRecording())); +// ui.gridLayout->addWidget(stopRecButton, 7, 4); +// } + + + this->uas = uas->getUASID(); + setBackgroundColor(uas->getColor()); +} + +UASControlWidget::~UASControlWidget() +{ + +} + +void UASControlWidget::updateStatemachine() +{ + + if (engineOn) + { + ui.controlButton->setText(tr("Stop Engine")); + } + else + { + ui.controlButton->setText(tr("Activate Engine")); + } +} + +/** + * Set the background color based on the MAV color. If the MAV is selected as the + * currently actively controlled system, the frame color is highlighted + */ +void UASControlWidget::setBackgroundColor(QColor color) +{ + // UAS color + QColor uasColor = color; + QString colorstyle; + QString borderColor = "#4A4A4F"; + borderColor = "#FA4A4F"; + uasColor = uasColor.darker(900); + colorstyle = colorstyle.sprintf("QLabel { border-radius: 3px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 0px solid %s; }", + uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); + setStyleSheet(colorstyle); + QPalette palette = this->palette(); + palette.setBrush(QPalette::Window, QBrush(uasColor)); + setPalette(palette); + setAutoFillBackground(true); +} + + +void UASControlWidget::updateMode(int uas,QString mode,QString description) +{ + Q_UNUSED(uas); + Q_UNUSED(mode); + Q_UNUSED(description); +} + +void UASControlWidget::updateState(int state) +{ + switch (state) + { + case (int)MAV_STATE_ACTIVE: + engineOn = true; + ui.controlButton->setText(tr("Stop Engine")); + break; + case (int)MAV_STATE_STANDBY: + engineOn = false; + ui.controlButton->setText(tr("Activate Engine")); + break; + } +} + +void UASControlWidget::setMode(int mode) +{ + // Adapt context button mode + if (mode == CONTROL_MODE_LOCKED_INDEX) + { + uasMode = (unsigned int)MAV_MODE_LOCKED; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_MANUAL_INDEX) + { + uasMode = (unsigned int)MAV_MODE_MANUAL; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_GUIDED_INDEX) + { + uasMode = (unsigned int)MAV_MODE_GUIDED; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_AUTO_INDEX) + { + uasMode = (unsigned int)MAV_MODE_AUTO; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST1_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST1; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST2_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST2; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_TEST3_INDEX) + { + uasMode = (unsigned int)MAV_MODE_TEST3; + ui.modeComboBox->setCurrentIndex(mode); + } + else if (mode == CONTROL_MODE_RC_TRAINING_INDEX) + { + uasMode = (unsigned int)MAV_MODE_RC_TRAINING; + ui.modeComboBox->setCurrentIndex(mode); + } + else + { + qDebug() << "ERROR! MODE NOT FOUND"; + uasMode = 0; + } + + + qDebug() << "SET MODE REQUESTED" << uasMode; + + emit changedMode(mode); +} + +void UASControlWidget::transmitMode() +{ + if (uasMode != 0) + { + UASInterface* mav = UASManager::instance()->getUASForId(this->uas); + if (mav) + { + mav->setMode(uasMode); + ui.lastActionLabel->setText(QString("Set new mode for system %1").arg(mav->getUASName())); + } + } +} + +void UASControlWidget::cycleContextButton() +{ + UAS* mav = dynamic_cast(UASManager::instance()->getUASForId(this->uas)); + if (mav) + { + + if (!engineOn) + { + mav->enable_motors(); + ui.lastActionLabel->setText(QString("Enabled motors on %1").arg(mav->getUASName())); + } + else + { + mav->disable_motors(); + ui.lastActionLabel->setText(QString("Disabled motors on %1").arg(mav->getUASName())); + } + // Update state now and in several intervals when MAV might have changed state + updateStatemachine(); + + QTimer::singleShot(50, this, SLOT(updateStatemachine())); + QTimer::singleShot(200, this, SLOT(updateStatemachine())); + + //ui.controlButton->setText(tr("Force Landing")); + //ui.controlButton->setText(tr("KILL VEHICLE")); + } + +} + diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h index 003e25e7b30a26b68e8dca7d50a40bd9ec9198e5..4ec60f6930fefe0bbfc90369bdb708e92acec918 100644 --- a/src/ui/uas/UASControlWidget.h +++ b/src/ui/uas/UASControlWidget.h @@ -1,83 +1,87 @@ -/*===================================================================== - -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 class UASControlWidget - * - * @author Lorenz Meier - * - */ - -#ifndef _UASCONTROLWIDGET_H_ -#define _UASCONTROLWIDGET_H_ - -#include -#include -#include -#include -#include -#include - -/** - * @brief Widget controlling one MAV - */ -class UASControlWidget : public QWidget -{ - Q_OBJECT - -public: - UASControlWidget(QWidget *parent = 0); - ~UASControlWidget(); - -public slots: - /** @brief Set the system this widget controls */ - void setUAS(UASInterface* uas); - /** @brief Trigger next context action */ - void cycleContextButton(); - /** @brief Set the operation mode of the MAV */ - void setMode(int mode); - /** @brief Transmit the operation mode */ - void transmitMode(); - /** @brief Update the mode */ - void updateMode(int uas,QString mode,QString description); - /** @brief Update state */ - void updateState(int state); - /** @brief Update internal state machine */ - void updateStatemachine(); - -protected slots: - /** @brief Set the background color for the widget */ - void setBackgroundColor(QColor color); - -protected: - int uas; ///< Reference to the current uas - unsigned int uasMode; ///< Current uas mode - bool engineOn; ///< Engine state - -private: - Ui::uasControl ui; - -}; - -#endif // _UASCONTROLWIDGET_H_ +/*===================================================================== + +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 class UASControlWidget + * + * @author Lorenz Meier + * + */ + +#ifndef _UASCONTROLWIDGET_H_ +#define _UASCONTROLWIDGET_H_ + +#include +#include +#include +#include +#include +#include + +/** + * @brief Widget controlling one MAV + */ +class UASControlWidget : public QWidget +{ + Q_OBJECT + +public: + UASControlWidget(QWidget *parent = 0); + ~UASControlWidget(); + +public slots: + /** @brief Set the system this widget controls */ + void setUAS(UASInterface* uas); + /** @brief Trigger next context action */ + void cycleContextButton(); + /** @brief Set the operation mode of the MAV */ + void setMode(int mode); + /** @brief Transmit the operation mode */ + void transmitMode(); + /** @brief Update the mode */ + void updateMode(int uas,QString mode,QString description); + /** @brief Update state */ + void updateState(int state); + /** @brief Update internal state machine */ + void updateStatemachine(); + +signals: + void changedMode(int); + + +protected slots: + /** @brief Set the background color for the widget */ + void setBackgroundColor(QColor color); + +protected: + int uas; ///< Reference to the current uas + unsigned int uasMode; ///< Current uas mode + bool engineOn; ///< Engine state + +private: + Ui::uasControl ui; + +}; + +#endif // _UASCONTROLWIDGET_H_ diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index fa9df58f3eb0d75a547dfa11761fc0f1c8933353..a057740875af7f7adf1fbcba0b8c017f15ed8b73 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -1,217 +1,220 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK 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. - - PIXHAWK 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 PIXHAWK. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Implementation of class UASInfoWidget - * - * @author Lorenz Meier - * - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) -{ - ui.setupUi(this); - this->name = name; - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); - - activeUAS = NULL; - - //instruments = new QMap(); - - // Set default battery type - // setBattery(0, LIPOLY, 3); - startTime = MG::TIME::getGroundTimeNow(); - // startVoltage = 0.0f; - - // lastChargeLevel = 0.5f; - // lastRemainingTime = 1; - - // Set default values - /** Set two voltage decimals and zero charge level decimals **/ - this->voltageDecimals = 2; - this->loadDecimals = 2; - - this->voltage = 0; - this->chargeLevel = 0; - this->load = 0; - receiveLoss = 0; - sendLoss = 0; - changed = true; - errors = QMap(); - - updateTimer = new QTimer(this); - connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); - updateTimer->start(updateInterval); - - this->setVisible(false); -} - -UASInfoWidget::~UASInfoWidget() -{ - -} - -void UASInfoWidget::showEvent(QShowEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - updateTimer->start(updateInterval); -} - -void UASInfoWidget::hideEvent(QHideEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - updateTimer->stop(); -} - -void UASInfoWidget::addUAS(UASInterface* uas) -{ - if (uas != NULL) - { - connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int))); - connect(uas, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); - connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); - connect(uas, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); - - // Set this UAS as active if it is the first one - if (activeUAS == 0) activeUAS = uas; - } -} - -void UASInfoWidget::setActiveUAS(UASInterface* uas) -{ - activeUAS = uas; -} - -void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) -{ - setVoltage(uas, voltage); - setChargeLevel(uas, percent); - setTimeRemaining(uas, seconds); -} - -void UASInfoWidget::updateErrorCount(int uasid, QString component, QString device, int count) -{ - //qDebug() << __FILE__ << __LINE__ << activeUAS->getUASID() << "=" << uasid; - if (activeUAS->getUASID() == uasid) - { - errors.remove(component + ":" + device); - errors.insert(component + ":" + device, count); - } -} - -/** - * - */ -void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) -{ - if (activeUAS == uas) - { - this->load = load; - } -} - -void UASInfoWidget::updateReceiveLoss(int uasId, float receiveLoss) -{ - Q_UNUSED(uasId); - this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f; -} - -/** - The send loss is typically calculated on the GCS based on packets - that were received scrambled from the MAV - */ -void UASInfoWidget::updateSendLoss(int uasId, float sendLoss) -{ - Q_UNUSED(uasId); - this->sendLoss = this->sendLoss * 0.8f + sendLoss * 0.2f; -} - -void UASInfoWidget::setVoltage(UASInterface* uas, double voltage) -{ - Q_UNUSED(uas); - this->voltage = voltage; -} - -void UASInfoWidget::setChargeLevel(UASInterface* uas, double chargeLevel) -{ - if (activeUAS == uas) - { - this->chargeLevel = chargeLevel; - } -} - -void UASInfoWidget::setTimeRemaining(UASInterface* uas, double seconds) -{ - if (activeUAS == uas) - { - this->timeRemaining = seconds; - } -} - -void UASInfoWidget::refresh() -{ - ui.voltageLabel->setText(QString::number(this->voltage, 'f', voltageDecimals)); - ui.batteryBar->setValue(qMax(0,qMin(static_cast(this->chargeLevel), 100))); - - ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals)); - ui.loadBar->setValue(qMax(0, qMin(static_cast(this->load), 100))); - - ui.receiveLossBar->setValue(qMax(0, qMin(static_cast(receiveLoss), 100))); - ui.receiveLossLabel->setText(QString::number(receiveLoss, 'f', 2)); - - ui.sendLossBar->setValue(sendLoss); - ui.sendLossLabel->setText(QString::number(sendLoss, 'f', 2)); - - QString errorString; - QMapIterator i(errors); - while (i.hasNext()) - { - i.next(); - errorString += QString(i.key() + ": %1 ").arg(i.value()); - - // FIXME - errorString.replace("IMU:", ""); - - - } - ui.errorLabel->setText(errorString); -} +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK 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. + + PIXHAWK 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 PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class UASInfoWidget + * + * @author Lorenz Meier + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) +{ + ui.setupUi(this); + this->name = name; + + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + + activeUAS = NULL; + + //instruments = new QMap(); + + // Set default battery type + // setBattery(0, LIPOLY, 3); + startTime = MG::TIME::getGroundTimeNow(); + // startVoltage = 0.0f; + + // lastChargeLevel = 0.5f; + // lastRemainingTime = 1; + + // Set default values + /** Set two voltage decimals and zero charge level decimals **/ + this->voltageDecimals = 2; + this->loadDecimals = 2; + + this->voltage = 0; + this->chargeLevel = 0; + this->load = 0; + receiveLoss = 0; + sendLoss = 0; + changed = true; + errors = QMap(); + + updateTimer = new QTimer(this); + connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); + updateTimer->start(updateInterval); + + this->setVisible(false); +} + +UASInfoWidget::~UASInfoWidget() +{ + +} + +void UASInfoWidget::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + updateTimer->start(updateInterval); +} + +void UASInfoWidget::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + updateTimer->stop(); +} + +void UASInfoWidget::addUAS(UASInterface* uas) +{ + if (uas != NULL) + { + connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int))); + connect(uas, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); + connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); + connect(uas, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); + + // Set this UAS as active if it is the first one + if (activeUAS == 0) activeUAS = uas; + } +} + +void UASInfoWidget::setActiveUAS(UASInterface* uas) +{ + activeUAS = uas; +} + +void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) +{ + setVoltage(uas, voltage); + setChargeLevel(uas, percent); + setTimeRemaining(uas, seconds); +} + +void UASInfoWidget::updateErrorCount(int uasid, QString component, QString device, int count) +{ + //qDebug() << __FILE__ << __LINE__ << activeUAS->getUASID() << "=" << uasid; + if (activeUAS->getUASID() == uasid) + { + errors.remove(component + ":" + device); + errors.insert(component + ":" + device, count); + } +} + +/** + * + */ +void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) +{ + if (activeUAS == uas) + { + this->load = load; + } +} + +void UASInfoWidget::updateReceiveLoss(int uasId, float receiveLoss) +{ + Q_UNUSED(uasId); + this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f; +} + +/** + The send loss is typically calculated on the GCS based on packets + that were received scrambled from the MAV + */ +void UASInfoWidget::updateSendLoss(int uasId, float sendLoss) +{ + Q_UNUSED(uasId); + this->sendLoss = this->sendLoss * 0.8f + sendLoss * 0.2f; +} + +void UASInfoWidget::setVoltage(UASInterface* uas, double voltage) +{ + Q_UNUSED(uas); + this->voltage = voltage; +} + +void UASInfoWidget::setChargeLevel(UASInterface* uas, double chargeLevel) +{ + if (activeUAS == uas) + { + this->chargeLevel = chargeLevel; + } +} + +void UASInfoWidget::setTimeRemaining(UASInterface* uas, double seconds) +{ + if (activeUAS == uas) + { + this->timeRemaining = seconds; + } +} + +void UASInfoWidget::refresh() +{ + ui.voltageLabel->setText(QString::number(this->voltage, 'f', voltageDecimals)); + ui.batteryBar->setValue(qMax(0,qMin(static_cast(this->chargeLevel), 100))); + + ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals)); + ui.loadBar->setValue(qMax(0, qMin(static_cast(this->load), 100))); + + ui.receiveLossBar->setValue(qMax(0, qMin(static_cast(receiveLoss), 100))); + ui.receiveLossLabel->setText(QString::number(receiveLoss, 'f', 2)); + + ui.sendLossBar->setValue(sendLoss); + ui.sendLossLabel->setText(QString::number(sendLoss, 'f', 2)); + + ui.label_5->setText(QString::number(this->load, 'f', loadDecimals)); + ui.progressBar->setValue(qMax(0, qMin(static_cast(this->load), 100))); + + QString errorString; + QMapIterator i(errors); + while (i.hasNext()) + { + i.next(); + errorString += QString(i.key() + ": %1 ").arg(i.value()); + + // FIXME + errorString.replace("IMU:", ""); + + + } + ui.errorLabel->setText(errorString); +} diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index bb30a918d0143fd0f07979f4ffd22e5b76a807e0..cdb14a136a1b2e622e02b89dd34aafa9f9c762d7 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -214,7 +214,11 @@ void UASView::updateActiveUAS(UASInterface* uas, bool active) void UASView::updateMode(int sysId, QString status, QString description) { Q_UNUSED(description); + + //int aa=this->uas->getUASID(); if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status); + + m_ui->modeLabel->setText(status); } void UASView::mouseDoubleClickEvent (QMouseEvent * event)