diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index ad2ef1c..f9f3b87 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -5,6 +5,8 @@ OpalLink::OpalLink() : heartbeatTimer(new QTimer(this)), heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(true), + getSignalsTimer(new QTimer(this)), + getSignalsPeriod(1000), receiveBuffer(new QQueue()), systemID(1), componentID(1) @@ -17,85 +19,153 @@ OpalLink::OpalLink() : LinkManager::instance()->add(this); // Start heartbeat timer, emitting a heartbeat at the configured rate - qDebug() << "OpalLink::OpalLink:: Connect the timer"; QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat())); - heartbeatTimer->start(1000/heartbeatRate); + + QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals())); } -void OpalLink::run() + +/* + * + Communication + * + */ + +qint64 OpalLink::bytesAvailable() { - qDebug() << "OpalLink::run():: Starting the thread"; + return 0; } -int OpalLink::getId() +void OpalLink::writeBytes(const char *bytes, qint64 length) { - return id; + } -QString OpalLink::getName() + +void OpalLink::readBytes(char *bytes, qint64 maxLength) { - return name; + + receiveDataMutex.lock(); + qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); + QByteArray message = receiveBuffer->dequeue(); + if (maxLength < message.size()) + { + qDebug() << "OpalLink::readBytes:: Buffer Overflow"; + + memcpy(bytes, message.data(), maxLength); + } + else + { + memcpy(bytes, message.data(), message.size()); + } + + emit bytesReceived(this, message); + receiveDataMutex.unlock(); + } -void OpalLink::setName(QString name) +void OpalLink::receiveMessage(mavlink_message_t message) { - this->name = name; - emit nameChanged(this->name); -} -bool OpalLink::isConnected() { - //qDebug() << "OpalLink::isConnected:: connectState: " << connectState; - return connectState; + // Create buffer + char buffer[MAVLINK_MAX_PACKET_LEN]; + // Write message into buffer, prepending start sign + int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message); + // If link is connected + if (isConnected()) + { + receiveBuffer->enqueue(QByteArray(buffer, len)); + emit bytesReady(this); + } + } -qint64 OpalLink::getNominalDataRate() +void OpalLink::heartbeat() { - return 0; //unknown + + if (m_heartbeatsEnabled) + { + qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; + mavlink_message_t beat; + mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); + receiveMessage(beat); + } + } -int OpalLink::getLinkQuality() +void OpalLink::getSignals() { - return -1; //not supported + 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; + } -qint64 OpalLink::getTotalUpstream() +/* + * + Administrative + * + */ +void OpalLink::run() { - statisticsMutex.lock(); - qint64 totalUpstream = bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); - statisticsMutex.unlock(); - return totalUpstream; + qDebug() << "OpalLink::run():: Starting the thread"; } -qint64 OpalLink::getTotalDownstream() { - statisticsMutex.lock(); - qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); - statisticsMutex.unlock(); - return totalDownstream; +int OpalLink::getId() +{ + return id; } -qint64 OpalLink::getCurrentUpstream() +QString OpalLink::getName() { - return 0; //unknown + return name; } -qint64 OpalLink::getMaxUpstream() +void OpalLink::setName(QString name) { - return 0; //unknown + this->name = name; + emit nameChanged(this->name); } -qint64 OpalLink::getBitsSent() { - return bitsSentTotal; +bool OpalLink::isConnected() { + //qDebug() << "OpalLink::isConnected:: connectState: " << connectState; + return connectState; } -qint64 OpalLink::getBitsReceived() { - return bitsReceivedTotal; -} -bool OpalLink::isFullDuplex() -{ - return false; -} bool OpalLink::connect() { @@ -105,22 +175,17 @@ bool OpalLink::connect() if (OpalConnect(101, false, &modelState) == EOK) { connectState = true; + emit connected(); + heartbeatTimer->start(1000/heartbeatRate); + getSignalsTimer->start(getSignalsPeriod); } else { connectState = false; - setLastErrorMsg(); - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(lastErrorMsg); - msgBox.exec(); + displayErrorMsg(); } emit connected(connectState); - if (connectState) - { - emit connected(); - } return connectState; } @@ -129,6 +194,15 @@ bool OpalLink::disconnect() return false; } +void OpalLink::displayErrorMsg() +{ + setLastErrorMsg(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(lastErrorMsg); + msgBox.exec(); +} + void OpalLink::setLastErrorMsg() { char buf[512]; @@ -138,64 +212,58 @@ void OpalLink::setLastErrorMsg() lastErrorMsg.append(buf); } -qint64 OpalLink::bytesAvailable() + +/* + * + Statisctics + * + */ + +qint64 OpalLink::getNominalDataRate() { - return 0; + return 0; //unknown } -void OpalLink::writeBytes(const char *bytes, qint64 length) +int OpalLink::getLinkQuality() { - + return -1; //not supported } - -void OpalLink::readBytes(char *bytes, qint64 maxLength) +qint64 OpalLink::getTotalUpstream() { + statisticsMutex.lock(); + qint64 totalUpstream = bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalUpstream; +} - receiveDataMutex.lock(); - qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); - QByteArray message = receiveBuffer->dequeue(); - if (maxLength < message.size()) - { - qDebug() << "OpalLink::readBytes:: Buffer Overflow"; - - memcpy(bytes, message.data(), maxLength); - } - else - { - memcpy(bytes, message.data(), message.size()); - } - - emit bytesReceived(this, message); - receiveDataMutex.unlock(); - +qint64 OpalLink::getTotalDownstream() { + statisticsMutex.lock(); + qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000); + statisticsMutex.unlock(); + return totalDownstream; } -void OpalLink::heartbeat() +qint64 OpalLink::getCurrentUpstream() { + return 0; //unknown +} - if (m_heartbeatsEnabled) - { - qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; - mavlink_message_t beat; - mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); - receiveMessage(beat); - } +qint64 OpalLink::getMaxUpstream() +{ + return 0; //unknown +} +qint64 OpalLink::getBitsSent() { + return bitsSentTotal; } -void OpalLink::receiveMessage(mavlink_message_t message) -{ +qint64 OpalLink::getBitsReceived() { + return bitsReceivedTotal; +} - // Create buffer - char buffer[MAVLINK_MAX_PACKET_LEN]; - // Write message into buffer, prepending start sign - int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message); - // If link is connected - if (isConnected()) - { - receiveBuffer->enqueue(QByteArray(buffer, len)); - emit bytesReady(this); - } +bool OpalLink::isFullDuplex() +{ + return false; } diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h index da2ed0a..1c9a408 100644 --- a/src/comm/OpalLink.h +++ b/src/comm/OpalLink.h @@ -26,6 +26,12 @@ #include "OpalApi.h" #include "string.h" +/* + Configuration info for the model + */ + +#define NUM_OUTPUT_SIGNALS 6 + class OpalLink : public LinkInterface { Q_OBJECT @@ -72,6 +78,8 @@ public slots: void heartbeat(); + void getSignals(); + protected slots: void receiveMessage(mavlink_message_t message); @@ -95,12 +103,17 @@ protected: QMutex receiveDataMutex; QString lastErrorMsg; void setLastErrorMsg(); + void displayErrorMsg(); void setName(QString name); QTimer* heartbeatTimer; ///< Timer to emit heartbeats int heartbeatRate; ///< Heartbeat rate, controls the timer interval bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission + + QTimer* getSignalsTimer; + int getSignalsPeriod; + QQueue* receiveBuffer; QByteArray* sendBuffer; diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 61c1ed5..ea0b82b 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -244,7 +244,6 @@ void UDPLink::setName(QString name) emit nameChanged(this->name); } - qint64 UDPLink::getNominalDataRate() { return 54000000; // 54 Mbit }