OpalLink.cc 5.64 KB
Newer Older
1 2
#include "OpalLink.h"

3 4 5 6 7
OpalLink::OpalLink() :
        connectState(false),
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
        m_heartbeatsEnabled(true),
8 9
        getSignalsTimer(new QTimer(this)),
        getSignalsPeriod(1000),
10 11 12
        receiveBuffer(new QQueue<QByteArray>()),
        systemID(1),
        componentID(1)
13
{
14
    start(QThread::LowPriority);
Bryan Godbolt's avatar
Bryan Godbolt committed
15 16 17 18 19

    // Set unique ID and add link to the list of links
    this->id = getNextLinkId();
    this->name = tr("OpalRT link ") + QString::number(getId());
    LinkManager::instance()->add(this);
20 21 22

    // Start heartbeat timer, emitting a heartbeat at the configured rate
    QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat()));
23 24

    QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals()));
25 26
}

27 28 29 30 31 32 33 34

/*
 *
  Communication
 *
 */

qint64 OpalLink::bytesAvailable()
35
{
36
    return 0;
Bryan Godbolt's avatar
Bryan Godbolt committed
37 38
}

39
void OpalLink::writeBytes(const char *bytes, qint64 length)
Bryan Godbolt's avatar
Bryan Godbolt committed
40
{
41

Bryan Godbolt's avatar
Bryan Godbolt committed
42 43
}

44 45

void OpalLink::readBytes(char *bytes, qint64 maxLength)
Bryan Godbolt's avatar
Bryan Godbolt committed
46
{
47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64

    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();

Bryan Godbolt's avatar
Bryan Godbolt committed
65 66
}

67
void OpalLink::receiveMessage(mavlink_message_t message)
Bryan Godbolt's avatar
Bryan Godbolt committed
68 69
{

70 71 72 73 74 75 76 77 78 79 80
    // 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);
    }

Bryan Godbolt's avatar
Bryan Godbolt committed
81 82
}

83
void OpalLink::heartbeat()
Bryan Godbolt's avatar
Bryan Godbolt committed
84
{
85 86 87 88 89 90 91 92 93

    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);
    }

Bryan Godbolt's avatar
Bryan Godbolt committed
94 95
}

96
void OpalLink::getSignals()
Bryan Godbolt's avatar
Bryan Godbolt committed
97
{
98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133
    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;

Bryan Godbolt's avatar
Bryan Godbolt committed
134 135
}

136 137 138 139 140 141
/*
 *
  Administrative
 *
 */
void OpalLink::run()
Bryan Godbolt's avatar
Bryan Godbolt committed
142
{
143
    qDebug() << "OpalLink::run():: Starting the thread";
144 145
}

146 147 148
int OpalLink::getId()
{
    return id;
Bryan Godbolt's avatar
Bryan Godbolt committed
149 150
}

151
QString OpalLink::getName()
Bryan Godbolt's avatar
Bryan Godbolt committed
152
{
153
    return name;
Bryan Godbolt's avatar
Bryan Godbolt committed
154 155
}

156
void OpalLink::setName(QString name)
Bryan Godbolt's avatar
Bryan Godbolt committed
157
{
158 159
    this->name = name;
    emit nameChanged(this->name);
Bryan Godbolt's avatar
Bryan Godbolt committed
160 161
}

162 163 164
bool OpalLink::isConnected() {
    //qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
    return connectState;
Bryan Godbolt's avatar
Bryan Godbolt committed
165 166 167 168
}



169 170 171

bool OpalLink::connect()
{
Bryan Godbolt's avatar
Bryan Godbolt committed
172
    short modelState;
173 174 175 176 177

    /// \todo allow configuration of instid in window
    if (OpalConnect(101, false, &modelState) == EOK)
    {
        connectState = true;
178 179 180
        emit connected();
        heartbeatTimer->start(1000/heartbeatRate);
        getSignalsTimer->start(getSignalsPeriod);
181 182 183 184
    }
    else
    {
        connectState = false;
185
        displayErrorMsg();
186 187 188 189
    }

    emit connected(connectState);
    return connectState;
190 191 192 193 194 195 196
}

bool OpalLink::disconnect()
{
    return false;
}

197 198 199 200 201 202 203 204 205
void OpalLink::displayErrorMsg()
{
    setLastErrorMsg();
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Critical);
    msgBox.setText(lastErrorMsg);
    msgBox.exec();
}

206 207 208 209 210 211 212 213 214
void OpalLink::setLastErrorMsg()
{
    char buf[512];
    unsigned short len;
    OpalGetLastErrMsg(buf, sizeof(buf), &len);
    lastErrorMsg.clear();
    lastErrorMsg.append(buf);
}

215 216 217 218 219 220 221 222

/*
 *
  Statisctics
 *
 */

qint64 OpalLink::getNominalDataRate()
223
{
224
    return 0; //unknown
225
}
Bryan Godbolt's avatar
Bryan Godbolt committed
226

227
int OpalLink::getLinkQuality()
Bryan Godbolt's avatar
Bryan Godbolt committed
228
{
229
    return -1; //not supported
Bryan Godbolt's avatar
Bryan Godbolt committed
230 231
}

232
qint64 OpalLink::getTotalUpstream()
Bryan Godbolt's avatar
Bryan Godbolt committed
233
{
234 235 236 237 238
    statisticsMutex.lock();
    qint64 totalUpstream =  bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
    statisticsMutex.unlock();
    return totalUpstream;
}
Bryan Godbolt's avatar
Bryan Godbolt committed
239

240 241 242 243 244
qint64 OpalLink::getTotalDownstream() {
    statisticsMutex.lock();
    qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
    statisticsMutex.unlock();
    return totalDownstream;
245 246
}

247
qint64 OpalLink::getCurrentUpstream()
248
{
249 250
    return 0; //unknown
}
251

252 253 254 255
qint64 OpalLink::getMaxUpstream()
{
    return 0; //unknown
}
256

257 258
qint64 OpalLink::getBitsSent() {
    return bitsSentTotal;
259 260
}

261 262 263
qint64 OpalLink::getBitsReceived() {
    return bitsReceivedTotal;
}
264 265


266 267 268
bool OpalLink::isFullDuplex()
{
    return false;
Bryan Godbolt's avatar
Bryan Godbolt committed
269
}