From e4e8e9a5c86a1bca8f0f8901366a68565a27b4d7 Mon Sep 17 00:00:00 2001 From: Bryan Godbolt Date: Tue, 24 Aug 2010 07:26:43 -0600 Subject: [PATCH] working for test --- qgroundcontrol.pro | 8 ++- src/Core.cc | 1 - src/comm/OpalLink.cc | 135 +++++++++++++++++++++++-------------------- src/comm/OpalLink.h | 15 +++-- 4 files changed, 83 insertions(+), 76 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index aeb4612cc..b4d8480f1 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -153,7 +153,8 @@ HEADERS += src/MG.h \ src/ui/QGCPxImuFirmwareUpdate.h \ src/comm/MAVLinkLightProtocol.h \ src/ui/QGCDataPlot2D.h \ - src/ui/linechart/IncrementalPlot.h + src/ui/linechart/IncrementalPlot.h \ + src/comm/OpalRT.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -218,8 +219,9 @@ SOURCES += src/main.cc \ RESOURCES = mavground.qrc # Include RT-LAB Library -win32 { - LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin -lOpalApi +win32 { + LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \ + -lOpalApi INCLUDEPATH += src/lib/opalrt SOURCES += src/comm/OpalLink.cc HEADERS += src/comm/OpalLink.h diff --git a/src/Core.cc b/src/Core.cc index bdd612dcb..e1be758f7 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -138,7 +138,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Add OpalRT Link, but do not connect OpalLink* opalLink = new OpalLink(); mainWindow->addLink(opalLink); -#warning OPAL LINK NOW AUTO CONNECTING IN CORE.CC #endif // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index 90ff9805e..eb88a87a1 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -35,7 +35,7 @@ OpalLink::OpalLink() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(true), getSignalsTimer(new QTimer(this)), - getSignalsPeriod(1000), + getSignalsPeriod(10), receiveBuffer(new QQueue()), systemID(1), componentID(1) @@ -74,22 +74,11 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) void OpalLink::readBytes() { receiveDataMutex.lock(); - const qint64 maxLength = 2048; - char bytes[maxLength]; - qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); - QByteArray message = receiveBuffer->dequeue(); - if (maxLength < message.size()) - { - qDebug() << "OpalLink::readBytes:: Buffer Overflow"; +// qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); +// QByteArray message = receiveBuffer->dequeue(); - memcpy(bytes, message.data(), maxLength); - } - else - { - memcpy(bytes, message.data(), message.size()); - } - emit bytesReceived(this, message); + emit bytesReceived(this, receiveBuffer->dequeue()); receiveDataMutex.unlock(); } @@ -104,7 +93,9 @@ void OpalLink::receiveMessage(mavlink_message_t message) // If link is connected if (isConnected()) { + receiveDataMutex.lock(); receiveBuffer->enqueue(QByteArray(buffer, len)); + receiveDataMutex.unlock(); readBytes(); } @@ -115,7 +106,7 @@ void OpalLink::heartbeat() if (m_heartbeatsEnabled) { - qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; +// qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; mavlink_message_t beat; mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); receiveMessage(beat); @@ -125,41 +116,57 @@ void OpalLink::heartbeat() void OpalLink::getSignals() { +// getSignalsMutex.lock(); // qDebug() << "OpalLink::getSignals(): Attempting to acquire signals"; -// -// -// unsigned long timeout = 0; -// unsigned short acqGroup = 0; //this is actually group 1 in the model -// unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS; -// unsigned short *numSignals = new unsigned short(0); -// double *timestep = new double(0); -// double values[NUM_OUTPUT_SIGNALS] = {}; -// unsigned short *lastValues = new unsigned short(false); -// unsigned short *decimation = new unsigned short(0); -// -// int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep, -// values, lastValues, decimation); -// -// if (returnVal == EOK ) -// { -// qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues); -// } -// else if (returnVal == EAGAIN) -// { -// qDebug() << "OpalLink::getSignals: Data was not ready"; -// } -// // if returnVal == EAGAIN => data just wasn't ready -// else if (returnVal != EAGAIN) -// { -// getSignalsTimer->stop(); -// displayErrorMsg(); -// } -// /* deallocate used memory */ -// -// delete timestep; -// delete lastValues; -// delete lastValues; -// delete decimation; + + + unsigned long timeout = 0; + unsigned short acqGroup = 0; //this is actually group 1 in the model + unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS; + unsigned short *numSignals = new unsigned short(0); + double *timestep = new double(0); + double values[NUM_OUTPUT_SIGNALS] = {}; + unsigned short *lastValues = new unsigned short(false); + unsigned short *decimation = new unsigned short(0); + + while (!(*lastValues)) + { + int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep, + values, lastValues, decimation); + + if (returnVal == EOK ) + { + // qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues); + mavlink_message_t local_position; + mavlink_msg_local_position_pack(systemID, componentID, &local_position, + (*timestep)*1000000, + values[OpalRT::X_POS], + values[OpalRT::Y_POS], + values[OpalRT::Z_POS], + values[OpalRT::X_VEL], + values[OpalRT::Y_VEL], + values[OpalRT::Z_VEL]); + receiveMessage(local_position); + } + else if (returnVal == EAGAIN) + { +// qDebug() << "OpalLink::getSignals: Data was not ready"; + } + // if returnVal == EAGAIN => data just wasn't ready + else if (returnVal != EAGAIN) + { + getSignalsTimer->stop(); + displayErrorMsg(); + } + } + + /* deallocate used memory */ + + delete numSignals; + delete timestep; + delete lastValues; + delete decimation; +// getSignalsMutex.unlock(); } @@ -170,7 +177,7 @@ void OpalLink::getSignals() */ void OpalLink::run() { - qDebug() << "OpalLink::run():: Starting the thread"; +// qDebug() << "OpalLink::run():: Starting the thread"; } int OpalLink::getId() @@ -202,18 +209,18 @@ bool OpalLink::connect() short modelState; /// \todo allow configuration of instid in window -// if (OpalConnect(101, false, &modelState) == EOK) -// { + if (OpalConnect(101, false, &modelState) == EOK) + { connectState = true; emit connected(); heartbeatTimer->start(1000/heartbeatRate); getSignalsTimer->start(getSignalsPeriod); -// } -// else -// { -// connectState = false; -// displayErrorMsg(); -// } + } + else + { + connectState = false; + displayErrorMsg(); + } emit connected(connectState); return connectState; @@ -235,11 +242,11 @@ void OpalLink::displayErrorMsg() void OpalLink::setLastErrorMsg() { -// char buf[512]; -// unsigned short len; -// OpalGetLastErrMsg(buf, sizeof(buf), &len); -// lastErrorMsg.clear(); -// lastErrorMsg.append(buf); + char buf[512]; + unsigned short len; + OpalGetLastErrMsg(buf, sizeof(buf), &len); + lastErrorMsg.clear(); + lastErrorMsg.append(buf); } diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h index ad0cbe65b..217b95148 100644 --- a/src/comm/OpalLink.h +++ b/src/comm/OpalLink.h @@ -44,15 +44,13 @@ This file is part of the QGROUNDCONTROL project #include "mavlink.h" #include "mavlink_types.h" #include "configuration.h" +#include "OpalRT.h" #include "errno.h" - - - -// FIXME -//#include "OpalApi.h" - +#ifdef OPAL_RT +#include "OpalApi.h" +#endif @@ -66,9 +64,9 @@ This file is part of the QGROUNDCONTROL project #define NUM_OUTPUT_SIGNALS 6 /** - * @brief Interface to OPAL-RT targets + * @brief Interface to OpalRT targets * - * This is an interface to the Opal-RT hardware-in-the-loop (HIL) simulator. + * This is an interface to the OpalRT hardware-in-the-loop (HIL) simulator. * This class receives MAVLink packets as if it is a true link, but it * interprets the packets internally, and calls the appropriate api functions. * @@ -144,6 +142,7 @@ protected: QMutex statisticsMutex; QMutex receiveDataMutex; +// QMutex getSignalsMutex; QString lastErrorMsg; void setLastErrorMsg(); void displayErrorMsg(); -- 2.22.0