OpalLink.cc 21.7 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 */
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]));
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]));
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);
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)) {
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],
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";
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));
}
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;
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;
486 487 488 489
}

bool OpalLink::disconnect()
{
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);
496
    return true;
497 498
}

499 500 501 502 503
// Data rate functions
qint64 OpalLink::getConnectionSpeed() const
{
    return 0; //unknown
}
504

505 506 507 508
qint64 OpalLink::getCurrentInDataRate() const
{
    return 0;
}
509

510
qint64 OpalLink::getCurrentOutDataRate() const
511
{
512 513
    return 0;
}