OpalLink.cc 22.4 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

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 <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of class OpalLink
 *   @author Bryan Godbolt <godbolt@ualberta.ca>
 */

30
31
#include "OpalLink.h"

32
OpalLink::OpalLink() :
33
34
35
36
37
38
39
40
41
42
43
44
45
46
    connectState(false),
    heartbeatTimer(new QTimer(this)),
    heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
    m_heartbeatsEnabled(true),
    getSignalsTimer(new QTimer(this)),
    getSignalsPeriod(10),
    receiveBuffer(new QQueue<QByteArray>()),
    systemID(1),
    componentID(1),
    params(NULL),
    opalInstID(101),
    sendRCValues(false),
    sendRawController(false),
    sendPosition(false)
47
{
48
    start(QThread::LowPriority);
Bryan Godbolt's avatar
Bryan Godbolt committed
49
50
51
52
53

    // 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);
54
55
56

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

    QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals()));
59
60
}

61
62
63
64
65
66
67
68

/*
 *
  Communication
 *
 */

qint64 OpalLink::bytesAvailable()
69
{
70
    return 0;
Bryan Godbolt's avatar
Bryan Godbolt committed
71
72
}

73
void OpalLink::writeBytes(const char *bytes, qint64 length)
Bryan Godbolt's avatar
Bryan Godbolt committed
74
{
75
76
    /* decode the message */
    mavlink_message_t msg;
77
    mavlink_status_t status;
78
    int decodeSuccess = 0;
79
    for (int i=0; (!(decodeSuccess=mavlink_parse_char(this->getId(), bytes[i], &msg, &status))&& i<length); ++i);
80
81

    /* perform the appropriate action */
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
    if (decodeSuccess) {
        switch(msg.msgid) {
        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
            qDebug() << "OpalLink::writeBytes(): request params";

            mavlink_message_t param;


            OpalRT::ParameterList::const_iterator paramIter;
            for (paramIter = params->begin(); paramIter != params->end(); ++paramIter) {
                mavlink_msg_param_value_pack(systemID,
                                             (*paramIter).getComponentID(),
                                             &param,
                                             (*paramIter).getParamID().toInt8_t(),
                                             (static_cast<OpalRT::Parameter>(*paramIter)).getValue(),
                                             params->count(),
                                             params->indexOf(*paramIter));
                receiveMessage(param);
            }
101
102


103
104
        }
        case MAVLINK_MSG_ID_PARAM_SET: {
105
106
107

//                qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";

108
109
110
            mavlink_param_set_t param;
            mavlink_msg_param_set_decode(&msg, &param);
            OpalRT::QGCParamID paramName((char*)param.param_id);
111
112
113

//                qDebug() << "OpalLink::writeBytes():paramName: " << paramName;

114
115
            if ((*params).contains(param.target_component, paramName)) {
                OpalRT::Parameter p = (*params)(param.target_component, paramName);
116
//                    qDebug() << __FILE__ << ":" << __LINE__ << ": "  << p;
117
118
119
120
121
122
123
124
125
126
127
128
129
                // Set the param value in Opal-RT
                p.setValue(param.param_value);

                // Get the param value from Opal-RT to make sure it was set properly
                mavlink_message_t paramMsg;
                mavlink_msg_param_value_pack(systemID,
                                             p.getComponentID(),
                                             &paramMsg,
                                             p.getParamID().toInt8_t(),
                                             p.getValue(),
                                             params->count(),
                                             params->indexOf(p));
                receiveMessage(paramMsg);
130
            }
131
132
        }
        break;
133
134
135
136
137
138
139
//        case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS:
//        {
//        	mavlink_request_rc_channels_t rc;
//        	mavlink_msg_request_rc_channels_decode(&msg, &rc);
//        	this->sendRCValues = static_cast<bool>(rc.enabled);
//        }
//        break;
140
#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
141
        case MAVLINK_MSG_ID_RADIO_CALIBRATION: {
142
143
            mavlink_radio_calibration_t radio;
            mavlink_msg_radio_calibration_decode(&msg, &radio);
144
145
146
147
148
149
150
151
152
//            qDebug() << "RADIO CALIBRATION RECEIVED";
//            qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
//            qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
//            qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
//            qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1];
//            qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4];
//            qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4];

            /* AILERON SERVO */
Bryan Godbolt's avatar
Bryan Godbolt committed
153
            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
154
                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
155
156
            if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
Bryan Godbolt's avatar
Bryan Godbolt committed
157
            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
158
                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
159
160
            if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
Bryan Godbolt's avatar
Bryan Godbolt committed
161
            if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
162
                params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
163
164
            if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
165
166
167
            /* ELEVATOR SERVO */
            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
168
169
            if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
170
171
            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
172
173
            if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
174
175
            if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
176
177
            if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT"))
                params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
            /* THROTTLE SERVO */
            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET1_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET1_IN").setValue(((radio.throttle[1]>900 /*in us?*/)?radio.throttle[1]/1000:radio.throttle[1]));
            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET2_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET2_IN").setValue(((radio.throttle[2]>900 /*in us?*/)?radio.throttle[2]/1000:radio.throttle[2]));
            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET3_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET3_IN").setValue(((radio.throttle[3]>900 /*in us?*/)?radio.throttle[3]/1000:radio.throttle[3]));
            if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET4_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET4_IN").setValue(((radio.throttle[4]>900 /*in us?*/)?radio.throttle[4]/1000:radio.throttle[4]));
            /* RUDDER SERVO */
            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN").setValue(((radio.rudder[0]>900 /*in us?*/)?radio.rudder[0]/1000:radio.rudder[0]));
            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN").setValue(((radio.rudder[1]>900 /*in us?*/)?radio.rudder[1]/1000:radio.rudder[1]));
            if (params->contains(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN").setValue(((radio.rudder[2]>900 /*in us?*/)?radio.rudder[2]/1000:radio.rudder[2]));
            /* GYRO MODE/GAIN SWITCH */
            if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN").setValue(((radio.gyro[0]>900 /*in us?*/)?radio.gyro[0]/1000:radio.gyro[0]));
            if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN").setValue(((radio.gyro[1]>900 /*in us?*/)?radio.gyro[1]/1000:radio.gyro[1]));
            /* PITCH SERVO */
            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET0_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET0_IN").setValue(((radio.pitch[0]>900 /*in us?*/)?radio.pitch[0]/1000:radio.pitch[0]));
            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET1_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET1_IN").setValue(((radio.pitch[1]>900 /*in us?*/)?radio.pitch[1]/1000:radio.pitch[1]));
            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET2_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET2_IN").setValue(((radio.pitch[2]>900 /*in us?*/)?radio.pitch[2]/1000:radio.pitch[2]));
            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET3_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET3_IN").setValue(((radio.pitch[3]>900 /*in us?*/)?radio.pitch[3]/1000:radio.pitch[3]));
            if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET4_IN"))
                params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET4_IN").setValue(((radio.pitch[4]>900 /*in us?*/)?radio.pitch[4]/1000:radio.pitch[4]));
212
213
214
        }
        break;
#endif
215
#ifdef MAVLINK_ENABLED_PIXHAWK
216
        case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
217
218
            mavlink_request_data_stream_t stream;
            mavlink_msg_request_data_stream_decode(&msg, &stream);
219
            switch (stream.req_stream_id) {
220
221
222
223
224
225
226
227
228
229
            case 0: // All data types
                break;
            case 1: // Raw Sensor Data
                break;
            case 2: // extended system status
                break;
            case 3: // rc channel data
                sendRCValues = (stream.start_stop == 1?true:false);
                break;
            case 4: // raw controller
230
231
232
233
234
                if (stream.start_stop == 1)
                    sendRawController = true;
                else
                    sendRawController = false;
                break;
235
236
237
238
239
240
241
242
243
244
245
            case 5: // raw sensor fusion
                break;
            case 6: // position
                sendPosition = (stream.start_stop == 1?true:false);
                break;
            case 7: // extra 1
                break;
            case 8: // extra 2
                break;
            case 9: // extra 3
                break;
246
247
248
249
250
            default:
                qDebug() << __FILE__ << __LINE__ << "Received Unknown Data Strem Request with ID" << stream.req_stream_id;
            }
        }
        break;
251
252
253
        default: {
            qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
        }
254
        }
255
#endif
256
    }
Bryan Godbolt's avatar
Bryan Godbolt committed
257
258
}

259

260
void OpalLink::readBytes()
Bryan Godbolt's avatar
Bryan Godbolt committed
261
{
262
    receiveDataMutex.lock();
Bryan Godbolt's avatar
Bryan Godbolt committed
263
    emit bytesReceived(this, receiveBuffer->dequeue());
264
265
    receiveDataMutex.unlock();

Bryan Godbolt's avatar
Bryan Godbolt committed
266
267
}

268
void OpalLink::receiveMessage(mavlink_message_t message)
Bryan Godbolt's avatar
Bryan Godbolt committed
269
270
{

271
272
273
274
275
    // 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
276
    if (isConnected()) {
Bryan Godbolt's avatar
Bryan Godbolt committed
277
        receiveDataMutex.lock();
278
        receiveBuffer->enqueue(QByteArray(buffer, len));
Bryan Godbolt's avatar
Bryan Godbolt committed
279
        receiveDataMutex.unlock();
280
        readBytes();
281
282
    }

Bryan Godbolt's avatar
Bryan Godbolt committed
283
284
}

285
void OpalLink::heartbeat()
Bryan Godbolt's avatar
Bryan Godbolt committed
286
{
287

288
    if (m_heartbeatsEnabled) {
289
290
291
292
293
        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
294
}
295
296
void OpalLink::setSignals(double *values)
{
Bryan Godbolt's avatar
Bryan Godbolt committed
297
    unsigned short numSignals = 2;
298
299
300
301
302
    unsigned short logicalId = 1;
    unsigned short signalIndex[] = {0,1};

    int returnValue;
    returnValue =  OpalSetSignals( numSignals, logicalId, signalIndex, values);
303
    if (returnValue != EOK) {
304
        OpalRT::OpalErrorMsg::displayLastErrorMsg();
305
306
    }
}
307
void OpalLink::getSignals()
Bryan Godbolt's avatar
Bryan Godbolt committed
308
{
Bryan Godbolt's avatar
Bryan Godbolt committed
309
310
311
312
    unsigned long  timeout = 0;
    unsigned short acqGroup = 0; //this is actually group 1 in the model
    unsigned short *numSignals = new unsigned short(0);
    double *timestep = new double(0);
Bryan Godbolt's avatar
Bryan Godbolt committed
313
    double values[OpalRT::NUM_OUTPUT_SIGNALS] = {};
Bryan Godbolt's avatar
Bryan Godbolt committed
314
315
316
    unsigned short *lastValues = new unsigned short(false);
    unsigned short *decimation = new unsigned short(0);

317
    while (!(*lastValues)) {
Bryan Godbolt's avatar
Bryan Godbolt committed
318
        int returnVal = OpalGetSignals(timeout, acqGroup, OpalRT::NUM_OUTPUT_SIGNALS, numSignals, timestep,
Bryan Godbolt's avatar
Bryan Godbolt committed
319
320
                                       values, lastValues, decimation);

321
        if (returnVal == EOK ) {
322
            /* Send position info to qgroundcontrol */
323
            if (sendPosition) {
324
325
326
327
328
329
330
331
332
333
334
                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);
            }
335
336
337
338
339
340
341
            /* send attitude info to qgroundcontrol */
            mavlink_message_t attitude;
            mavlink_msg_attitude_pack(systemID, componentID, &attitude,
                                      (*timestep)*1000000,
                                      values[OpalRT::ROLL],
                                      values[OpalRT::PITCH],
                                      values[OpalRT::YAW],
Bryan Godbolt's avatar
Bryan Godbolt committed
342
343
344
                                      values[OpalRT::ROLL_SPEED],
                                      values[OpalRT::PITCH_SPEED],
                                      values[OpalRT::YAW_SPEED]
345
                                     );
346
347
348
349
350
351
352
353
354
355
356
357
            receiveMessage(attitude);

            /* send bias info to qgroundcontrol */
            mavlink_message_t bias;
            mavlink_msg_nav_filter_bias_pack(systemID, componentID, &bias,
                                             (*timestep)*1000000,
                                             values[OpalRT::B_F_0],
                                             values[OpalRT::B_F_1],
                                             values[OpalRT::B_F_2],
                                             values[OpalRT::B_W_0],
                                             values[OpalRT::B_W_1],
                                             values[OpalRT::B_W_2]
358
                                            );
359
            receiveMessage(bias);
Bryan Godbolt's avatar
Bryan Godbolt committed
360
361

            /* send radio outputs */
362
            if (sendRCValues) {
363
                mavlink_message_t rc;
364
                mavlink_msg_rc_channels_raw_pack(systemID, componentID, &rc,
365
366
367
368
369
370
371
372
373
374
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_1]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_2]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_3]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_4]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_5]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]),
                                                 duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]),
                                                 0 //rssi unused
                                                );
375
376
                receiveMessage(rc);
            }
377
            if (sendRawController) {
378
379
                mavlink_message_t rawController;
                mavlink_msg_attitude_controller_output_pack(systemID, componentID, &rawController,
380
381
382
383
384
385
                        1,
                        rescaleControllerOutput(values[OpalRT::CONTROLLER_AILERON]),
                        rescaleControllerOutput(values[OpalRT::CONTROLLER_ELEVATOR]),
                        0, // yaw not used
                        0 // thrust not used
                                                           );
386
387
                receiveMessage(rawController);
            }
388
        } else if (returnVal != EAGAIN) { // if returnVal == EAGAIN => data just wasn't ready
Bryan Godbolt's avatar
Bryan Godbolt committed
389
            getSignalsTimer->stop();
390
            OpalRT::OpalErrorMsg::displayLastErrorMsg();
Bryan Godbolt's avatar
Bryan Godbolt committed
391
392
393
394
395
396
397
398
399
        }
    }

    /* deallocate used memory */

    delete numSignals;
    delete timestep;
    delete lastValues;
    delete decimation;
400

Bryan Godbolt's avatar
Bryan Godbolt committed
401
402
}

403

404
405
406
407
408
409
/*
 *
  Administrative
 *
 */
void OpalLink::run()
Bryan Godbolt's avatar
Bryan Godbolt committed
410
{
Bryan Godbolt's avatar
Bryan Godbolt committed
411
//    qDebug() << "OpalLink::run():: Starting the thread";
Bryan Godbolt's avatar
Bryan Godbolt committed
412
413
}

414
int OpalLink::getId() const
415
416
{
    return id;
Bryan Godbolt's avatar
Bryan Godbolt committed
417
418
}

419
QString OpalLink::getName() const
Bryan Godbolt's avatar
Bryan Godbolt committed
420
{
421
    return name;
Bryan Godbolt's avatar
Bryan Godbolt committed
422
423
}

424
void OpalLink::setName(QString name)
Bryan Godbolt's avatar
Bryan Godbolt committed
425
{
426
427
    this->name = name;
    emit nameChanged(this->name);
Bryan Godbolt's avatar
Bryan Godbolt committed
428
429
}

430
bool OpalLink::isConnected() const
431
{
432
    return connectState;
Bryan Godbolt's avatar
Bryan Godbolt committed
433
434
}

Bryan Godbolt's avatar
Bryan Godbolt committed
435
436
437
438
439
440
uint16_t OpalLink::duty2PulseMicros(double duty)
{
    /* duty cycle assumed to be of a signal at 70 Hz */
    return static_cast<uint16_t>(duty/70*1000000);
}

441
uint8_t OpalLink::rescaleNorm(double norm, int ch)
442
{
443
    switch(ch) {
444
445
446
447
448
449
450
451
452
453
    case OpalRT::NORM_CHANNEL_1:
    case OpalRT::NORM_CHANNEL_2:
    case OpalRT::NORM_CHANNEL_4:
    default:
        // three setpoints
        return static_cast<uint8_t>((norm+1)/2*255);
        break;
    case OpalRT::NORM_CHANNEL_5:
        //two setpoints
    case OpalRT::NORM_CHANNEL_3:
454
    case OpalRT::NORM_CHANNEL_6:
455
456
457
        return static_cast<uint8_t>(norm*255);
        break;
    }
458
}
Bryan Godbolt's avatar
Bryan Godbolt committed
459

460
461
462
463
int8_t OpalLink::rescaleControllerOutput(double raw)
{
    return static_cast<int8_t>((raw>=0?raw*127:raw*128));
}
Bryan Godbolt's avatar
Bryan Godbolt committed
464
465
466

bool OpalLink::connect()
{
Bryan Godbolt's avatar
Bryan Godbolt committed
467
    short modelState;
468

469
    if ((OpalConnect(opalInstID, false, &modelState) == EOK)
470
471
            && (OpalGetSignalControl(0, true) == EOK)
            && (OpalGetParameterControl(true) == EOK)) {
472
        connectState = true;
Bryan Godbolt's avatar
Bryan Godbolt committed
473
474
        if (params)
            delete params;
475
        params = new OpalRT::ParameterList();
476
477
478
        emit connected();
        heartbeatTimer->start(1000/heartbeatRate);
        getSignalsTimer->start(getSignalsPeriod);
479
    } else {
Bryan Godbolt's avatar
Bryan Godbolt committed
480
        connectState = false;
481
        OpalRT::OpalErrorMsg::displayLastErrorMsg();
Bryan Godbolt's avatar
Bryan Godbolt committed
482
    }
483
484
485

    emit connected(connectState);
    return connectState;
Bryan Godbolt's avatar
Bryan Godbolt committed
486
487
488
489
}

bool OpalLink::disconnect()
{
Bryan Godbolt's avatar
Bryan Godbolt committed
490
491
492
493
    // OpalDisconnect returns void so its success or failure cannot be tested
    OpalDisconnect();
    heartbeatTimer->stop();
    getSignalsTimer->stop();
494
495
    connectState = false;
    emit connected(connectState);
Bryan Godbolt's avatar
Bryan Godbolt committed
496
    return true;
Bryan Godbolt's avatar
Bryan Godbolt committed
497
498
}

499

500

501
502
503
504
505
506
507

/*
 *
  Statisctics
 *
 */

508
qint64 OpalLink::getNominalDataRate() const
Bryan Godbolt's avatar
Bryan Godbolt committed
509
{
510
    return 0; //unknown
Bryan Godbolt's avatar
Bryan Godbolt committed
511
}
Bryan Godbolt's avatar
Bryan Godbolt committed
512

513
int OpalLink::getLinkQuality() const
Bryan Godbolt's avatar
Bryan Godbolt committed
514
{
515
    return -1; //not supported
Bryan Godbolt's avatar
Bryan Godbolt committed
516
517
}

518
qint64 OpalLink::getTotalUpstream()
Bryan Godbolt's avatar
Bryan Godbolt committed
519
{
520
521
522
523
524
    statisticsMutex.lock();
    qint64 totalUpstream =  bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
    statisticsMutex.unlock();
    return totalUpstream;
}
Bryan Godbolt's avatar
Bryan Godbolt committed
525

526
527
qint64 OpalLink::getTotalDownstream()
{
528
529
530
531
    statisticsMutex.lock();
    qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
    statisticsMutex.unlock();
    return totalDownstream;
532
533
}

534
qint64 OpalLink::getCurrentUpstream()
535
{
536
537
    return 0; //unknown
}
538

539
540
541
542
qint64 OpalLink::getMaxUpstream()
{
    return 0; //unknown
}
543

544
qint64 OpalLink::getBitsSent() const
545
{
546
    return bitsSentTotal;
547
548
}

549
qint64 OpalLink::getBitsReceived() const
550
{
551
552
    return bitsReceivedTotal;
}
553
554


555
bool OpalLink::isFullDuplex() const
556
557
{
    return false;
Bryan Godbolt's avatar
Bryan Godbolt committed
558
}