MAVLinkSimulationLink.cc 36.2 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
/*=====================================================================
pixhawk's avatar
pixhawk committed
2

lm's avatar
lm committed
3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

lm's avatar
lm committed
5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

lm's avatar
lm committed
7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

lm's avatar
lm committed
9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12
    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.
pixhawk's avatar
pixhawk committed
13

lm's avatar
lm committed
14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17
    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.
pixhawk's avatar
pixhawk committed
18

pixhawk's avatar
pixhawk committed
19
    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21

pixhawk's avatar
pixhawk committed
22
======================================================================*/
pixhawk's avatar
pixhawk committed
23

pixhawk's avatar
pixhawk committed
24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40
/**
 * @file
 *   @brief Implementation of simulated system link
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <cmath>
#include <QTime>
#include <QImage>
#include <QDebug>
#include "MG.h"
#include "LinkManager.h"
41
#include "MAVLinkProtocol.h"
pixhawk's avatar
pixhawk committed
42
#include "MAVLinkSimulationLink.h"
43
#include "QGCMAVLink.h"
pixhawk's avatar
pixhawk committed
44
#include "QGC.h"
45
#include "MAVLinkSimulationMAV.h"
pixhawk's avatar
pixhawk committed
46 47 48 49 50 51 52 53 54 55 56 57

/**
 * Create a simulated link. This link is connected to an input and output file.
 * The link sends one line at a time at the specified sendrate. The timing of
 * the sendrate is free of drift, which means it is stable on the long run.
 * However, small deviations are mixed in to vary the sendrate slightly
 * in order to simulate disturbances on a real communication link.
 *
 * @param readFile The file with the messages to read (must be in ASCII format, line breaks can be Unix or Windows style)
 * @param writeFile The received messages are written to that file
 * @param rate The rate at which the messages are sent (in intervals of milliseconds)
 **/
pixhawk's avatar
pixhawk committed
58
MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile, int rate, QObject* parent) : LinkInterface(parent),
59 60
    readyBytes(0),
    timeOffset(0)
pixhawk's avatar
pixhawk committed
61 62 63 64
{
    this->rate = rate;
    _isConnected = false;

lm's avatar
lm committed
65
    onboardParams = QMap<QString, float>();
pixhawk's avatar
pixhawk committed
66 67 68 69 70 71 72
    onboardParams.insert("PID_ROLL_K_P", 0.5f);
    onboardParams.insert("PID_PITCH_K_P", 0.5f);
    onboardParams.insert("PID_YAW_K_P", 0.5f);
    onboardParams.insert("PID_XY_K_P", 0.5f);
    onboardParams.insert("PID_ALT_K_P", 0.5f);
    onboardParams.insert("SYS_TYPE", 1);
    onboardParams.insert("SYS_ID", systemId);
lm's avatar
lm committed
73

pixhawk's avatar
pixhawk committed
74 75
    // Comments on the variables can be found in the header file

pixhawk's avatar
pixhawk committed
76
    simulationFile = new QFile(readFile, this);
77
    if (simulationFile->exists() && simulationFile->open(QIODevice::ReadOnly | QIODevice::Text)) {
pixhawk's avatar
pixhawk committed
78 79 80
        simulationHeader = simulationFile->readLine();
    }
    receiveFile = new QFile(writeFile, this);
81
    lastSent = MG::TIME::getGroundTimeNow() * 1000;
pixhawk's avatar
pixhawk committed
82

83
    if (simulationFile->exists()) {
pixhawk's avatar
pixhawk committed
84
        this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName();
85
    } else {
pixhawk's avatar
pixhawk committed
86 87 88
        this->name = "MAVLink simulation link";
    }

89 90


pixhawk's avatar
pixhawk committed
91 92 93
    // Initialize the pseudo-random number generator
    srand(QTime::currentTime().msec());
    maxTimeNoise = 0;
pixhawk's avatar
pixhawk committed
94 95
    this->id = getNextLinkId();
    LinkManager::instance()->add(this);
96
    QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*)));
pixhawk's avatar
pixhawk committed
97 98 99 100 101 102 103
}

MAVLinkSimulationLink::~MAVLinkSimulationLink()
{
    //TODO Check destructor
    //    fileStream->flush();
    //    outStream->flush();
lm's avatar
lm committed
104
    delete simulationFile;
pixhawk's avatar
pixhawk committed
105 106 107 108
}

void MAVLinkSimulationLink::run()
{
pixhawk's avatar
pixhawk committed
109 110 111 112 113 114

    status.mode = MAV_MODE_UNINIT;
    status.status = MAV_STATE_UNINIT;
    status.vbat = 0;
    status.packet_drop = 0;

115 116


117
    forever {
pixhawk's avatar
pixhawk committed
118 119 120

        static quint64 last = 0;

121 122
        if (MG::TIME::getGroundTimeNow() - last >= rate) {
            if (_isConnected) {
pixhawk's avatar
pixhawk committed
123
                mainloop();
124 125 126 127 128 129 130 131 132

                // FIXME Hacky code to read from packet log file
//                readyBufferMutex.lock();
//                for (int i = 0; i < streampointer; i++)
//                {
//                    readyBuffer.enqueue(*(stream + i));
//                }
//                readyBufferMutex.unlock();

133
                readBytes();
pixhawk's avatar
pixhawk committed
134 135 136
            }
            last = MG::TIME::getGroundTimeNow();
        }
137
        QGC::SLEEP::msleep(3);
pixhawk's avatar
pixhawk committed
138 139 140 141

    }
}

142 143 144 145 146 147 148
void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
{
    // Allocate buffer with packet data
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);

    // Pack to link buffer
pixhawk's avatar
pixhawk committed
149
    readyBufferMutex.lock();
150
    for (unsigned int i = 0; i < bufferlength; i++) {
151 152
        readyBuffer.enqueue(*(buf + i));
    }
pixhawk's avatar
pixhawk committed
153
    readyBufferMutex.unlock();
154 155
}

pixhawk's avatar
pixhawk committed
156 157 158 159
void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
{
    // Allocate buffer with packet data
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
pixhawk's avatar
pixhawk committed
160
    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
pixhawk's avatar
pixhawk committed
161 162 163 164 165
    //add data into datastream
    memcpy(stream+(*index),buf, bufferlength);
    (*index) += bufferlength;
}

pixhawk's avatar
pixhawk committed
166 167 168 169 170 171
void MAVLinkSimulationLink::mainloop()
{

    // Test for encoding / decoding packets

    // Test data stream
172
    streampointer = 0;
pixhawk's avatar
pixhawk committed
173 174 175

    // Fake system values

176 177
    static float fullVoltage = 4.2f * 3.0f;
    static float emptyVoltage = 3.35f * 3.0f;
pixhawk's avatar
pixhawk committed
178
    static float voltage = fullVoltage;
179
    static float drainRate = 0.025f; // x.xx% of the capacity is linearly drained per second
pixhawk's avatar
pixhawk committed
180

pixhawk's avatar
pixhawk committed
181
    mavlink_attitude_t attitude;
182
    memset(&attitude, 0, sizeof(mavlink_attitude_t));
183 184 185 186
#ifdef MAVLINK_ENABLED_PIXHAWK
    mavlink_raw_aux_t rawAuxValues;
    memset(&rawAuxValues, 0, sizeof(mavlink_raw_aux_t));
#endif
pixhawk's avatar
pixhawk committed
187
    mavlink_raw_imu_t rawImuValues;
188
    memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t));
pixhawk's avatar
pixhawk committed
189 190 191 192 193 194 195 196 197 198

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength;
    int messageSize;
    mavlink_message_t msg;

    // Timers
    static unsigned int rate1hzCounter = 1;
    static unsigned int rate10hzCounter = 1;
    static unsigned int rate50hzCounter = 1;
199
    static unsigned int circleCounter = 0;
pixhawk's avatar
pixhawk committed
200 201 202 203 204 205

    // Vary values

    // VOLTAGE
    // The battery is drained constantly
    voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate);
206
    if (voltage < 3.550f * 3.0f) voltage = 3.550f * 3.0f;
pixhawk's avatar
pixhawk committed
207

pixhawk's avatar
pixhawk committed
208
    static int state = 0;
pixhawk's avatar
pixhawk committed
209

210
    if (state == 0) {
pixhawk's avatar
pixhawk committed
211 212
        // BOOT
        // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
213
        messageSize = mavlink_msg_boot_pack(systemId, componentId, &msg, version);
pixhawk's avatar
pixhawk committed
214
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
215
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
216 217 218 219 220 221 222 223
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
        state++;
    }


    // 50 HZ TASKS
224 225 226
    if (rate50hzCounter == 1000 / rate / 40) {
        if (simulationFile->isOpen()) {
            if (simulationFile->atEnd()) {
pixhawk's avatar
pixhawk committed
227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242
                // We reached the end of the file, start from scratch
                simulationFile->reset();
                simulationHeader = simulationFile->readLine();
            }

            // Data was made available, read one line
            // first entry is the timestamp
            QString values = QString(simulationFile->readLine());
            QStringList parts = values.split("\t");
            QStringList keys = simulationHeader.split("\t");
            //qDebug() << simulationHeader;
            //qDebug() << values;
            bool ok;
            static quint64 lastTime = 0;
            static quint64 baseTime = 0;
            quint64 time = QString(parts.first()).toLongLong(&ok, 10);
243 244
            // FIXME Remove multiplicaton by 1000
            time *= 1000;
pixhawk's avatar
pixhawk committed
245

246 247
            if (ok) {
                if (timeOffset == 0) {
pixhawk's avatar
pixhawk committed
248 249 250 251
                    timeOffset = time;
                    baseTime = time;
                }

252
                if (lastTime > time) {
pixhawk's avatar
pixhawk committed
253 254 255 256 257 258 259 260 261
                    // We have wrapped around in the logfile
                    // Add the measurement time interval to the base time
                    baseTime += lastTime - timeOffset;
                }
                lastTime = time;

                time = time - timeOffset + baseTime;

                // Gather individual measurement values
262
                for (int i = 1; i < (parts.size() - 1); ++i) {
pixhawk's avatar
pixhawk committed
263 264 265 266 267 268 269 270
                    // Get one data field
                    bool res;
                    double d = QString(parts.at(i)).toDouble(&res);
                    if (!res) d = 0;

                    //qDebug() << "TIME" << time << "VALUE" << d;
                    //emit valueChanged(220, keys.at(i), d, MG::TIME::getGroundTimeNow());

271
                    if (keys.value(i, "") == "Accel._X") {
pixhawk's avatar
pixhawk committed
272 273 274
                        rawImuValues.xacc = d;
                    }

275
                    if (keys.value(i, "") == "Accel._Y") {
pixhawk's avatar
pixhawk committed
276 277 278
                        rawImuValues.yacc = d;
                    }

279
                    if (keys.value(i, "") == "Accel._Z") {
pixhawk's avatar
pixhawk committed
280 281
                        rawImuValues.zacc = d;
                    }
282
                    if (keys.value(i, "") == "Gyro_Phi") {
pixhawk's avatar
pixhawk committed
283
                        rawImuValues.xgyro = d;
284
                        attitude.rollspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
285 286
                    }

287
                    if (keys.value(i, "") == "Gyro_Theta") {
pixhawk's avatar
pixhawk committed
288
                        rawImuValues.ygyro = d;
289
                        attitude.pitchspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
290 291
                    }

292
                    if (keys.value(i, "") == "Gyro_Psi") {
pixhawk's avatar
pixhawk committed
293
                        rawImuValues.zgyro = d;
294
                        attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
295
                    }
lm's avatar
lm committed
296
#ifdef MAVLINK_ENABLED_PIXHAWK
297
                    if (keys.value(i, "") == "Pressure") {
pixhawk's avatar
pixhawk committed
298 299 300
                        rawAuxValues.baro = d;
                    }

301
                    if (keys.value(i, "") == "Battery") {
pixhawk's avatar
pixhawk committed
302 303
                        rawAuxValues.vbat = d;
                    }
304
#endif
305
                    if (keys.value(i, "") == "roll_IMU") {
pixhawk's avatar
pixhawk committed
306 307 308
                        attitude.roll = d;
                    }

309
                    if (keys.value(i, "") == "pitch_IMU") {
pixhawk's avatar
pixhawk committed
310 311 312
                        attitude.pitch = d;
                    }

313
                    if (keys.value(i, "") == "yaw_IMU") {
pixhawk's avatar
pixhawk committed
314 315 316 317 318 319 320 321 322 323
                        attitude.yaw = d;
                    }

                    //Accel._X	Accel._Y	Accel._Z	Battery	Bottom_Rotor	CPU_Load	Ground_Dist.	Gyro_Phi	Gyro_Psi	Gyro_Theta	Left_Servo	Mag._X	Mag._Y	Mag._Z	Pressure	Right_Servo	Temperature	Top_Rotor	pitch_IMU	roll_IMU	yaw_IMU

                }
                // Send out packets


                // ATTITUDE
324
                attitude.usec = time;
pixhawk's avatar
pixhawk committed
325
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
326
                mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
pixhawk's avatar
pixhawk committed
327
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
328
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
329 330 331 332 333
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

                // IMU
334
                rawImuValues.usec = time;
pixhawk's avatar
pixhawk committed
335 336 337 338
                rawImuValues.xmag = 0;
                rawImuValues.ymag = 0;
                rawImuValues.zmag = 0;
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
339
                mavlink_msg_raw_imu_encode(systemId, componentId, &msg, &rawImuValues);
pixhawk's avatar
pixhawk committed
340
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
341
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
342 343 344 345 346 347 348 349 350 351
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

                //qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;

                //qDebug() << "REALTIME" << MG::TIME::getGroundTimeNow() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;

            }

pixhawk's avatar
pixhawk committed
352
        }
pixhawk's avatar
pixhawk committed
353 354 355 356 357 358

        rate50hzCounter = 1;
    }


    // 10 HZ TASKS
359
    if (rate10hzCounter == 1000 / rate / 9) {
pixhawk's avatar
pixhawk committed
360
        rate10hzCounter = 1;
pixhawk's avatar
pixhawk committed
361

362 363 364 365
        float lastX = x;
        float lastY = y;
        float lastZ = z;
        float hackDt = 0.1f; // 100 ms
366

pixhawk's avatar
pixhawk committed
367
        // Move X Position
368 369 370
        x = 12.0*sin(((double)circleCounter)/200.0);
        y = 5.0*cos(((double)circleCounter)/200.0);
        z = 1.8 + 1.2*sin(((double)circleCounter)/200.0);
371

372 373 374 375
        float xSpeed = (x - lastX)/hackDt;
        float ySpeed = (y - lastY)/hackDt;
        float zSpeed = (z - lastZ)/hackDt;

376 377


378 379
        circleCounter++;

380

381 382 383
//        x = (x > 5.0f) ? 5.0f : x;
//        y = (y > 5.0f) ? 5.0f : y;
//        z = (z > 3.0f) ? 3.0f : z;
384

385 386 387
//        x = (x < -5.0f) ? -5.0f : x;
//        y = (y < -5.0f) ? -5.0f : y;
//        z = (z < -3.0f) ? -3.0f : z;
388

pixhawk's avatar
pixhawk committed
389 390 391
        // Send back new setpoint
        mavlink_message_t ret;
        mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw);
392 393 394 395 396 397
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        // Send back new position
398
        mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed);
399
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
pixhawk's avatar
pixhawk committed
400 401 402
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
403

404 405 406 407 408 409
//        // GPS RAW
//        mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
410

pixhawk's avatar
pixhawk committed
411
        // GLOBAL POSITION
412
        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed);
pixhawk's avatar
pixhawk committed
413 414 415 416 417
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

418 419 420 421 422 423
//        // GLOBAL POSITION VEHICLE 2
//        mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
424

425 426 427
//        // ATTITUDE VEHICLE 2
//        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
//        sendMAVLinkMessage(&ret);
pixhawk's avatar
pixhawk committed
428 429


430 431 432 433 434 435
//        // GLOBAL POSITION VEHICLE 3
//        mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
436

437
        static int rcCounter = 0;
438
        if (rcCounter == 2) {
439
            mavlink_rc_channels_raw_t chan;
440 441 442 443 444 445 446
            chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000);
            chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000);
            chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000);
            chan.chan4_raw = (chan.chan1_raw + chan.chan2_raw) / 2.0f;
            chan.chan5_raw = (chan.chan3_raw + chan.chan4_raw) / 2.0f;
            chan.chan6_raw = (chan.chan3_raw + chan.chan2_raw) / 2.0f;
            chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f;
447
            chan.chan8_raw = 0;
pixhawk's avatar
pixhawk committed
448
            chan.rssi = 100;
449
            messageSize = mavlink_msg_rc_channels_raw_encode(systemId, componentId, &msg, &chan);
450 451 452 453 454 455 456 457 458
            // Allocate buffer with packet data
            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
            //add data into datastream
            memcpy(stream+streampointer,buffer, bufferlength);
            streampointer += bufferlength;
            rcCounter = 0;
        }
        rcCounter++;

pixhawk's avatar
pixhawk committed
459 460 461
    }

    // 1 HZ TASKS
462
    if (rate1hzCounter == 1000 / rate / 1) {
pixhawk's avatar
pixhawk committed
463 464
        // STATE
        static int statusCounter = 0;
465
        if (statusCounter == 100) {
pixhawk's avatar
pixhawk committed
466 467 468 469 470
            status.mode = (status.mode + 1) % MAV_MODE_TEST3;
            statusCounter = 0;
        }
        statusCounter++;

471
        static int detectionCounter = 6;
472
        if (detectionCounter % 10 == 0) {
lm's avatar
lm committed
473
#ifdef MAVLINK_ENABLED_PIXHAWK
474 475 476
            mavlink_pattern_detected_t detected;
            detected.confidence = 5.0f;

477
            if (detectionCounter == 10) {
478 479 480
                char fileName[] = "patterns/face5.png";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
481
            } else if (detectionCounter == 20) {
482 483 484
                char fileName[] = "7";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
485
            } else if (detectionCounter == 30) {
486 487 488
                char fileName[] = "patterns/einstein.bmp";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
489
            } else if (detectionCounter == 40) {
490 491 492
                char fileName[] = "F";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
493
            } else if (detectionCounter == 50) {
494 495 496
                char fileName[] = "patterns/face2.png";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
497
            } else if (detectionCounter == 60) {
498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514
                char fileName[] = "H";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
                detectionCounter = 0;
            }
            detected.detected = 1;
            messageSize = mavlink_msg_pattern_detected_encode(systemId, componentId, &msg, &detected);
            // Allocate buffer with packet data
            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
            //add data into datastream
            memcpy(stream+streampointer,buffer, bufferlength);
            streampointer += bufferlength;
            //detectionCounter = 0;
#endif
        }
        detectionCounter++;

515
        status.vbat = voltage * 1000; // millivolts
516
        status.load = 33 * detectionCounter % 1000;
pixhawk's avatar
pixhawk committed
517 518

        // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
519
        messageSize = mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
520
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
521
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
522 523 524 525
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

526
        // Pack debug text message
pixhawk's avatar
pixhawk committed
527
        mavlink_statustext_t text;
528
        text.severity = 0;
529
        strcpy((char*)(text.text), "Text message from system 32");
pixhawk's avatar
pixhawk committed
530 531
        mavlink_msg_statustext_encode(systemId, componentId, &msg, &text);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
532 533 534
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
535 536
        /*
        // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
537
        messageSize = mavlink_msg_boot_pack(systemId, componentId, &msg, version);
pixhawk's avatar
pixhawk committed
538
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
539
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
540 541 542 543
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;*/

pixhawk's avatar
pixhawk committed
544 545
        // HEARTBEAT

pixhawk's avatar
pixhawk committed
546
        static int typeCounter = 0;
547
        uint8_t mavType;
548
        if (typeCounter < 10) {
lm's avatar
lm committed
549
            mavType = MAV_TYPE_QUADROTOR;
550
        } else {
lm's avatar
lm committed
551
            mavType = typeCounter % (MAV_TYPE_OCU);
552
        }
pixhawk's avatar
pixhawk committed
553 554
        typeCounter++;

pixhawk's avatar
pixhawk committed
555
        // Pack message and get size of encoded byte string
lm's avatar
lm committed
556
        messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_CLASS_PIXHAWK);
pixhawk's avatar
pixhawk committed
557
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
558
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
559 560 561 562
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
563

pixhawk's avatar
pixhawk committed
564 565 566
        // Send controller states


lm's avatar
lm committed
567 568 569 570 571 572 573 574 575 576
#ifdef MAVLINK_ENABLED_PIXHAWK
        uint8_t attControl = 1;
        uint8_t posXYControl = 1;
        uint8_t posZControl = 0;
        uint8_t posYawControl = 1;

        uint8_t gpsLock = 2;
        uint8_t visLock = 3;
        uint8_t ahrsHealth = 200;
        uint8_t posLock = qMax(gpsLock, visLock);
577
        messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, ahrsHealth, attControl, posXYControl, posZControl, posYawControl);
lm's avatar
lm committed
578
#endif
579

pixhawk's avatar
pixhawk committed
580 581 582 583 584
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;


585

586
//        // HEARTBEAT VEHICLE 2
pixhawk's avatar
pixhawk committed
587

588 589 590 591 592 593 594
//        // Pack message and get size of encoded byte string
//        messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA);
//        // Allocate buffer with packet data
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
pixhawk's avatar
pixhawk committed
595

596
//        // HEARTBEAT VEHICLE 3
597

598 599 600 601 602 603 604
//        // Pack message and get size of encoded byte string
//        messageSize = mavlink_msg_heartbeat_pack(60, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
//        // Allocate buffer with packet data
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
605

pixhawk's avatar
pixhawk committed
606
        // STATUS VEHICLE 2
607
        mavlink_sys_status_t status2;
pixhawk's avatar
pixhawk committed
608 609
        status2.mode = MAV_MODE_LOCKED;
        status2.vbat = voltage;
610
        status2.load = 120;
pixhawk's avatar
pixhawk committed
611 612 613
        status2.status = MAV_STATE_STANDBY;

        // Pack message and get size of encoded byte string
614
        messageSize = mavlink_msg_sys_status_encode(54, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
615
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
616
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
617 618 619
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
pixhawk's avatar
pixhawk committed
620

pixhawk's avatar
pixhawk committed
621
        rate1hzCounter = 1;
pixhawk's avatar
pixhawk committed
622 623 624 625 626
    }

    // FULL RATE TASKS
    // Default is 50 Hz

pixhawk's avatar
pixhawk committed
627
    /*
pixhawk's avatar
pixhawk committed
628 629 630 631
    // 50 HZ TASKS
    if (rate50hzCounter == 1000 / rate / 50)
    {

pixhawk's avatar
pixhawk committed
632
        //streampointer = 0;
pixhawk's avatar
pixhawk committed
633 634 635 636

        // Attitude

        // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
637
        messageSize = mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0);
pixhawk's avatar
pixhawk committed
638
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
639
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
640 641 642 643 644
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        rate50hzCounter = 1;
pixhawk's avatar
pixhawk committed
645
    }*/
pixhawk's avatar
pixhawk committed
646 647

    readyBufferMutex.lock();
648
    for (unsigned int i = 0; i < streampointer; i++) {
pixhawk's avatar
pixhawk committed
649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

    // Increment counters after full main loop
    rate1hzCounter++;
    rate10hzCounter++;
    rate50hzCounter++;
}


qint64 MAVLinkSimulationLink::bytesAvailable()
{
    readyBufferMutex.lock();
    qint64 size = readyBuffer.size();
    readyBufferMutex.unlock();
    return size;
}

void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
{
670
    //qDebug() << "Simulation received " << size << " bytes from groundstation: ";
pixhawk's avatar
pixhawk committed
671 672 673 674

    // Increase write counter
    //bitsSentTotal += size * 8;

pixhawk's avatar
pixhawk committed
675 676
    // Parse bytes
    mavlink_message_t msg;
lm's avatar
lm committed
677
    mavlink_status_t comm;
pixhawk's avatar
pixhawk committed
678

679 680 681 682 683
    uint8_t stream[2048];
    int streampointer = 0;
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength = 0;

pixhawk's avatar
pixhawk committed
684 685
    // Output all bytes as hex digits
    int i;
686 687
    for (i=0; i<size; i++) {
        if (mavlink_parse_char(this->id, data[i], &msg, &comm)) {
pixhawk's avatar
pixhawk committed
688
            // MESSAGE RECEIVED!
689 690
            qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
            emit messageReceived(msg);
pixhawk's avatar
pixhawk committed
691

692
            switch (msg.msgid) {
pixhawk's avatar
pixhawk committed
693
                // SET THE SYSTEM MODE
694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724
            case MAVLINK_MSG_ID_SET_MODE: {
                mavlink_set_mode_t mode;
                mavlink_msg_set_mode_decode(&msg, &mode);
                // Set mode indepent of mode.target
                status.mode = mode.mode;
            }
            break;
            case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: {
                mavlink_local_position_setpoint_set_t set;
                mavlink_msg_local_position_setpoint_set_decode(&msg, &set);
                spX = set.x;
                spY = set.y;
                spZ = set.z;
                spYaw = set.yaw;

                // Send back new setpoint
                mavlink_message_t ret;
                mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw);
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;
            }
            break;
            // EXECUTE OPERATOR ACTIONS
            case MAVLINK_MSG_ID_ACTION: {
                mavlink_action_t action;
                mavlink_msg_action_decode(&msg, &action);

                qDebug() << "SIM" << "received action" << action.action << "for system" << action.target;

lm's avatar
lm committed
725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750
                // FIXME MAVLINKV10PORTINGNEEDED
//                switch (action.action) {
//                case MAV_ACTION_LAUNCH:
//                    status.status = MAV_STATE_ACTIVE;
//                    status.mode = MAV_MODE_AUTO;
//                    break;
//                case MAV_ACTION_RETURN:
//                    status.status = MAV_STATE_ACTIVE;
//                    break;
//                case MAV_ACTION_MOTORS_START:
//                    status.status = MAV_STATE_ACTIVE;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                case MAV_ACTION_MOTORS_STOP:
//                    status.status = MAV_STATE_STANDBY;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                case MAV_ACTION_EMCY_KILL:
//                    status.status = MAV_STATE_EMERGENCY;
//                    status.mode = MAV_MODE_MANUAL;
//                    break;
//                case MAV_ACTION_SHUTDOWN:
//                    status.status = MAV_STATE_POWEROFF;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                }
751 752
            }
            break;
lm's avatar
lm committed
753
#ifdef MAVLINK_ENABLED_PIXHAWK
754 755 756 757 758 759
            case MAVLINK_MSG_ID_MANUAL_CONTROL: {
                mavlink_manual_control_t control;
                mavlink_msg_manual_control_decode(&msg, &control);
                qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch;
            }
            break;
760
#endif
761 762 763 764
            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
                qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
                mavlink_param_request_list_t read;
                mavlink_msg_param_request_list_decode(&msg, &read);
765 766
//                    if (read.target_system == systemId)
//                    {
767 768 769 770 771 772 773
                // Output all params
                // Iterate through all components, through all parameters and emit them
                QMap<QString, float>::iterator i;
                // Iterate through all components / subsystems
                int j = 0;
                for (i = onboardParams.begin(); i != onboardParams.end(); ++i) {
                    if (j != 5) {
774
                        // Pack message and get size of encoded byte string
lm's avatar
lm committed
775
                        mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
776 777 778 779 780 781
                        // Allocate buffer with packet data
                        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                        //add data into datastream
                        memcpy(stream+streampointer,buffer, bufferlength);
                        streampointer+=bufferlength;
                    }
782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800
                    j++;
                }

                qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
//                    }
            }
            break;
            case MAVLINK_MSG_ID_PARAM_SET: {
                // Drop on even milliseconds
                if (QGC::groundTimeMilliseconds() % 2 == 0) {
                    qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER";
                    mavlink_param_set_t set;
                    mavlink_msg_param_set_decode(&msg, &set);
                    //                    if (set.target_system == systemId)
                    //                    {
                    QString key = QString((char*)set.param_id);
                    if (onboardParams.contains(key)) {
                        onboardParams.remove(key);
                        onboardParams.insert(key, set.param_value);
801 802

                        // Pack message and get size of encoded byte string
lm's avatar
lm committed
803
                        mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
804 805 806 807 808 809
                        // Allocate buffer with packet data
                        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                        //add data into datastream
                        memcpy(stream+streampointer,buffer, bufferlength);
                        streampointer+=bufferlength;
                    }
810 811 812 813 814 815 816 817 818 819 820 821 822 823
                    //                    }
                }
            }
            break;
            case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
                qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
                mavlink_param_request_read_t read;
                mavlink_msg_param_request_read_decode(&msg, &read);
                QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
                QString key = QString(bytes);
                if (onboardParams.contains(key)) {
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
lm's avatar
lm committed
824
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
825 826 827 828 829 830 831 832 833 834 835
                    // Allocate buffer with packet data
                    bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                    //add data into datastream
                    memcpy(stream+streampointer,buffer, bufferlength);
                    streampointer+=bufferlength;
                    //qDebug() << "Sending PARAM" << key;
                } else if (read.param_index < onboardParams.size()) {
                    key = onboardParams.keys().at(read.param_index);
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
lm's avatar
lm committed
836
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
837 838 839 840 841 842
                    // Allocate buffer with packet data
                    bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                    //add data into datastream
                    memcpy(stream+streampointer,buffer, bufferlength);
                    streampointer+=bufferlength;
                    //qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key;
843
                }
844 845
            }
            break;
pixhawk's avatar
pixhawk committed
846 847 848 849
            }


        }
850 851
        unsigned char v=data[i];
        fprintf(stderr,"%02x ", v);
pixhawk's avatar
pixhawk committed
852
    }
853
    fprintf(stderr,"\n");
pixhawk's avatar
pixhawk committed
854

855
    readyBufferMutex.lock();
856
    for (int i = 0; i < streampointer; i++) {
857 858 859 860
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

pixhawk's avatar
pixhawk committed
861 862 863
    // Update comm status
    status.packet_drop = comm.packet_rx_drop_count;

pixhawk's avatar
pixhawk committed
864 865 866
}


867 868
void MAVLinkSimulationLink::readBytes()
{
pixhawk's avatar
pixhawk committed
869 870
    // Lock concurrent resource readyBuffer
    readyBufferMutex.lock();
871 872
    const qint64 maxLength = 2048;
    char data[maxLength];
pixhawk's avatar
pixhawk committed
873
    qint64 len = qMin((qint64)readyBuffer.size(), maxLength);
pixhawk's avatar
pixhawk committed
874

875
    for (unsigned int i = 0; i < len; i++) {
pixhawk's avatar
pixhawk committed
876 877 878 879 880 881 882 883
        *(data + i) = readyBuffer.takeFirst();
    }

    QByteArray b(data, len);
    emit bytesReceived(this, b);

    readyBufferMutex.unlock();

884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899
//    if (len > 0)
//    {
//        qDebug() << "Simulation sent " << len << " bytes to groundstation: ";

//        /* Increase write counter */
//        //bitsSentTotal += size * 8;

//        //Output all bytes as hex digits
//        int i;
//        for (i=0; i<len; i++)
//        {
//            unsigned int v=data[i];
//            fprintf(stderr,"%02x ", v);
//        }
//        fprintf(stderr,"\n");
//    }
pixhawk's avatar
pixhawk committed
900 901 902 903 904 905 906 907
}

/**
 * Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection
 * couldn't be disconnected.
 **/
908 909
bool MAVLinkSimulationLink::disconnect()
{
pixhawk's avatar
pixhawk committed
910 911 912 913 914 915 916

    if(isConnected()) {
        //        timer->stop();

        _isConnected = false;

        emit disconnected();
917
        emit connected(false);
pixhawk's avatar
pixhawk committed
918

919
        //exit();
pixhawk's avatar
pixhawk committed
920 921 922 923 924 925 926 927 928 929 930 931 932 933
    }

    return true;
}

/**
 * Connect the link.
 *
 * @return True if connection has been established, false if connection
 * couldn't be established.
 **/
bool MAVLinkSimulationLink::connect()
{
    _isConnected = true;
934 935
    emit connected();
    emit connected(true);
pixhawk's avatar
pixhawk committed
936 937

    start(LowPriority);
LM's avatar
LM committed
938
    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883);
939
    Q_UNUSED(mav1);
940 941
//    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
//    Q_UNUSED(mav2);
pixhawk's avatar
pixhawk committed
942 943 944 945
    //    timer->start(rate);
    return true;
}

946 947 948 949 950 951 952 953 954 955 956 957
/**
 * Connect the link.
 *
 * @param connect true connects the link, false disconnects it
 * @return True if connection has been established, false if connection
 * couldn't be established.
 **/
void MAVLinkSimulationLink::connectLink()
{
    this->connect();
}

958 959 960 961 962 963 964 965 966 967 968
/**
 * Connect the link.
 *
 * @param connect true connects the link, false disconnects it
 * @return True if connection has been established, false if connection
 * couldn't be established.
 **/
bool MAVLinkSimulationLink::connectLink(bool connect)
{
    _isConnected = connect;

969
    if(connect) {
970 971 972 973 974 975
        this->connect();
    }

    return true;
}

pixhawk's avatar
pixhawk committed
976 977 978 979 980
/**
 * Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
981 982
bool MAVLinkSimulationLink::isConnected()
{
pixhawk's avatar
pixhawk committed
983 984 985 986 987 988 989 990 991 992 993 994 995
    return _isConnected;
}

int MAVLinkSimulationLink::getId()
{
    return id;
}

QString MAVLinkSimulationLink::getName()
{
    return name;
}

996 997
qint64 MAVLinkSimulationLink::getNominalDataRate()
{
pixhawk's avatar
pixhawk committed
998 999 1000 1001
    /* 100 Mbit is reasonable fast and sufficient for all embedded applications */
    return 100000000;
}

1002 1003
qint64 MAVLinkSimulationLink::getTotalUpstream()
{
pixhawk's avatar
pixhawk committed
1004 1005 1006 1007 1008
    return 0;
    //TODO Add functionality here
    // @todo Add functionality here
}

1009 1010
qint64 MAVLinkSimulationLink::getShortTermUpstream()
{
pixhawk's avatar
pixhawk committed
1011 1012 1013
    return 0;
}

1014 1015
qint64 MAVLinkSimulationLink::getCurrentUpstream()
{
pixhawk's avatar
pixhawk committed
1016 1017 1018
    return 0;
}

1019 1020
qint64 MAVLinkSimulationLink::getMaxUpstream()
{
pixhawk's avatar
pixhawk committed
1021 1022 1023
    return 0;
}

1024 1025
qint64 MAVLinkSimulationLink::getBitsSent()
{
pixhawk's avatar
pixhawk committed
1026 1027 1028
    return 0;
}

1029 1030
qint64 MAVLinkSimulationLink::getBitsReceived()
{
pixhawk's avatar
pixhawk committed
1031 1032 1033
    return 0;
}

1034 1035
qint64 MAVLinkSimulationLink::getTotalDownstream()
{
pixhawk's avatar
pixhawk committed
1036 1037 1038
    return 0;
}

1039 1040
qint64 MAVLinkSimulationLink::getShortTermDownstream()
{
pixhawk's avatar
pixhawk committed
1041 1042 1043
    return 0;
}

1044 1045
qint64 MAVLinkSimulationLink::getCurrentDownstream()
{
pixhawk's avatar
pixhawk committed
1046 1047 1048
    return 0;
}

1049 1050
qint64 MAVLinkSimulationLink::getMaxDownstream()
{
pixhawk's avatar
pixhawk committed
1051 1052 1053
    return 0;
}

1054 1055
bool MAVLinkSimulationLink::isFullDuplex()
{
pixhawk's avatar
pixhawk committed
1056 1057 1058 1059
    /* Full duplex is no problem when running in pure software, but this is a serial simulation */
    return false;
}

1060 1061
int MAVLinkSimulationLink::getLinkQuality()
{
pixhawk's avatar
pixhawk committed
1062 1063 1064
    /* The Link quality is always perfect when running in software */
    return 100;
}