MAVLinkSimulationLink.cc 36.4 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
/**
 * @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>
39
#include <QFileInfo>
pixhawk's avatar
pixhawk committed
40
#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
    onboardParams.insert("PID_ROLL_K_P", 0.5f);
    onboardParams.insert("PID_PITCH_K_P", 0.5f);
    onboardParams.insert("PID_YAW_K_P", 0.5f);
69
    onboardParams.insert("PID_XY_K_P", 100.0f);
pixhawk's avatar
pixhawk committed
70 71 72
    onboardParams.insert("PID_ALT_K_P", 0.5f);
    onboardParams.insert("SYS_TYPE", 1);
    onboardParams.insert("SYS_ID", systemId);
73 74
    onboardParams.insert("RC4_REV", 0);
    onboardParams.insert("RC5_REV", 1);
LM's avatar
LM committed
75 76 77 78 79 80 81 82
    onboardParams.insert("HDNG2RLL_P", 0.7f);
    onboardParams.insert("HDNG2RLL_I", 0.01f);
    onboardParams.insert("HDNG2RLL_D", 0.02f);
    onboardParams.insert("HDNG2RLL_IMAX", 500.0f);
    onboardParams.insert("RLL2SRV_P", 0.4f);
    onboardParams.insert("RLL2SRV_I", 0.0f);
    onboardParams.insert("RLL2SRV_D", 0.0f);
    onboardParams.insert("RLL2SRV_IMAX", 500.0f);
lm's avatar
lm committed
83

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

pixhawk's avatar
pixhawk committed
86
    simulationFile = new QFile(readFile, this);
Lorenz Meier's avatar
Lorenz Meier committed
87 88
    if (simulationFile->exists() && simulationFile->open(QIODevice::ReadOnly | QIODevice::Text))
    {
pixhawk's avatar
pixhawk committed
89 90 91
        simulationHeader = simulationFile->readLine();
    }
    receiveFile = new QFile(writeFile, this);
92
    lastSent = QGC::groundTimeMilliseconds();
pixhawk's avatar
pixhawk committed
93

Lorenz Meier's avatar
Lorenz Meier committed
94 95
    if (simulationFile->exists())
    {
pixhawk's avatar
pixhawk committed
96
        this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName();
Lorenz Meier's avatar
Lorenz Meier committed
97 98 99
    }
    else
    {
pixhawk's avatar
pixhawk committed
100 101 102
        this->name = "MAVLink simulation link";
    }

103 104


pixhawk's avatar
pixhawk committed
105 106 107
    // Initialize the pseudo-random number generator
    srand(QTime::currentTime().msec());
    maxTimeNoise = 0;
pixhawk's avatar
pixhawk committed
108 109
    this->id = getNextLinkId();
    LinkManager::instance()->add(this);
pixhawk's avatar
pixhawk committed
110 111 112 113 114 115 116
}

MAVLinkSimulationLink::~MAVLinkSimulationLink()
{
    //TODO Check destructor
    //    fileStream->flush();
    //    outStream->flush();
Lorenz Meier's avatar
Lorenz Meier committed
117 118 119 120 121
    // Force termination, there is no
    // need for cleanup since
    // this thread is not manipulating
    // any relevant data
    terminate();
lm's avatar
lm committed
122
    delete simulationFile;
pixhawk's avatar
pixhawk committed
123 124 125 126
}

void MAVLinkSimulationLink::run()
{
pixhawk's avatar
pixhawk committed
127

128
    status.voltage_battery = 0;
129
    status.errors_comm = 0;
130

131 132
    system.base_mode = MAV_MODE_PREFLIGHT;
    system.custom_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED;
133
    system.system_status = MAV_STATE_UNINIT;
134

Lorenz Meier's avatar
Lorenz Meier committed
135 136
    forever
    {
pixhawk's avatar
pixhawk committed
137 138
        static quint64 last = 0;

Lorenz Meier's avatar
Lorenz Meier committed
139 140 141 142
        if (QGC::groundTimeMilliseconds() - last >= rate)
        {
            if (_isConnected)
            {
pixhawk's avatar
pixhawk committed
143
                mainloop();
144
                readBytes();
pixhawk's avatar
pixhawk committed
145
            }
Lorenz Meier's avatar
Lorenz Meier committed
146 147 148 149 150 151
            else
            {
                // Sleep for substantially longer
                // if not connected
                QGC::SLEEP::msleep(500);
            }
152
            last = QGC::groundTimeMilliseconds();
pixhawk's avatar
pixhawk committed
153
        }
154
        QGC::SLEEP::msleep(3);
pixhawk's avatar
pixhawk committed
155 156 157
    }
}

158 159 160 161 162 163 164
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
165
    readyBufferMutex.lock();
Lorenz Meier's avatar
Lorenz Meier committed
166 167
    for (unsigned int i = 0; i < bufferlength; i++)
    {
168 169
        readyBuffer.enqueue(*(buf + i));
    }
pixhawk's avatar
pixhawk committed
170
    readyBufferMutex.unlock();
171 172
}

pixhawk's avatar
pixhawk committed
173 174 175 176
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
177
    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
pixhawk's avatar
pixhawk committed
178 179 180 181 182
    //add data into datastream
    memcpy(stream+(*index),buf, bufferlength);
    (*index) += bufferlength;
}

pixhawk's avatar
pixhawk committed
183 184 185 186 187 188
void MAVLinkSimulationLink::mainloop()
{

    // Test for encoding / decoding packets

    // Test data stream
189
    streampointer = 0;
pixhawk's avatar
pixhawk committed
190 191 192

    // Fake system values

193 194
    static float fullVoltage = 4.2f * 3.0f;
    static float emptyVoltage = 3.35f * 3.0f;
pixhawk's avatar
pixhawk committed
195
    static float voltage = fullVoltage;
196
    static float drainRate = 0.025f; // x.xx% of the capacity is linearly drained per second
pixhawk's avatar
pixhawk committed
197

pixhawk's avatar
pixhawk committed
198
    mavlink_attitude_t attitude;
199
    memset(&attitude, 0, sizeof(mavlink_attitude_t));
200 201 202 203
#ifdef MAVLINK_ENABLED_PIXHAWK
    mavlink_raw_aux_t rawAuxValues;
    memset(&rawAuxValues, 0, sizeof(mavlink_raw_aux_t));
#endif
pixhawk's avatar
pixhawk committed
204
    mavlink_raw_imu_t rawImuValues;
205
    memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t));
pixhawk's avatar
pixhawk committed
206 207 208 209 210 211 212 213 214

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

    // Timers
    static unsigned int rate1hzCounter = 1;
    static unsigned int rate10hzCounter = 1;
    static unsigned int rate50hzCounter = 1;
215
    static unsigned int circleCounter = 0;
pixhawk's avatar
pixhawk committed
216 217 218 219 220 221

    // Vary values

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

pixhawk's avatar
pixhawk committed
224
    static int state = 0;
pixhawk's avatar
pixhawk committed
225

Lorenz Meier's avatar
Lorenz Meier committed
226 227
    if (state == 0)
    {
pixhawk's avatar
pixhawk committed
228 229 230 231 232
        state++;
    }


    // 50 HZ TASKS
Lorenz Meier's avatar
Lorenz Meier committed
233 234 235 236
    if (rate50hzCounter == 1000 / rate / 40)
    {
        if (simulationFile->isOpen())
        {
237
            if (simulationFile->atEnd()) {
pixhawk's avatar
pixhawk committed
238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253
                // 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);
254 255
            // FIXME Remove multiplicaton by 1000
            time *= 1000;
pixhawk's avatar
pixhawk committed
256

257 258
            if (ok) {
                if (timeOffset == 0) {
pixhawk's avatar
pixhawk committed
259 260 261 262
                    timeOffset = time;
                    baseTime = time;
                }

263
                if (lastTime > time) {
pixhawk's avatar
pixhawk committed
264 265 266 267 268 269 270 271 272
                    // 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
273
                for (int i = 1; i < (parts.size() - 1); ++i) {
pixhawk's avatar
pixhawk committed
274 275 276 277 278
                    // Get one data field
                    bool res;
                    double d = QString(parts.at(i)).toDouble(&res);
                    if (!res) d = 0;

279
                    if (keys.value(i, "") == "Accel._X") {
pixhawk's avatar
pixhawk committed
280 281 282
                        rawImuValues.xacc = d;
                    }

283
                    if (keys.value(i, "") == "Accel._Y") {
pixhawk's avatar
pixhawk committed
284 285 286
                        rawImuValues.yacc = d;
                    }

287
                    if (keys.value(i, "") == "Accel._Z") {
pixhawk's avatar
pixhawk committed
288 289
                        rawImuValues.zacc = d;
                    }
290
                    if (keys.value(i, "") == "Gyro_Phi") {
pixhawk's avatar
pixhawk committed
291
                        rawImuValues.xgyro = d;
292
                        attitude.rollspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
293 294
                    }

295
                    if (keys.value(i, "") == "Gyro_Theta") {
pixhawk's avatar
pixhawk committed
296
                        rawImuValues.ygyro = d;
297
                        attitude.pitchspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
298 299
                    }

300
                    if (keys.value(i, "") == "Gyro_Psi") {
pixhawk's avatar
pixhawk committed
301
                        rawImuValues.zgyro = d;
302
                        attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
303
                    }
lm's avatar
lm committed
304
#ifdef MAVLINK_ENABLED_PIXHAWK
305
                    if (keys.value(i, "") == "Pressure") {
pixhawk's avatar
pixhawk committed
306 307 308
                        rawAuxValues.baro = d;
                    }

309
                    if (keys.value(i, "") == "Battery") {
pixhawk's avatar
pixhawk committed
310 311
                        rawAuxValues.vbat = d;
                    }
312
#endif
313
                    if (keys.value(i, "") == "roll_IMU") {
pixhawk's avatar
pixhawk committed
314 315 316
                        attitude.roll = d;
                    }

317
                    if (keys.value(i, "") == "pitch_IMU") {
pixhawk's avatar
pixhawk committed
318 319 320
                        attitude.pitch = d;
                    }

321
                    if (keys.value(i, "") == "yaw_IMU") {
pixhawk's avatar
pixhawk committed
322 323 324 325 326 327 328 329 330 331
                        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
332
                attitude.time_boot_ms = time/1000;
pixhawk's avatar
pixhawk committed
333
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
334
                mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
pixhawk's avatar
pixhawk committed
335
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
336
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
337 338 339 340 341
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

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

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

356
                //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
pixhawk's avatar
pixhawk committed
357 358 359

            }

pixhawk's avatar
pixhawk committed
360
        }
pixhawk's avatar
pixhawk committed
361 362 363 364 365 366

        rate50hzCounter = 1;
    }


    // 10 HZ TASKS
367
    if (rate10hzCounter == 1000 / rate / 9) {
pixhawk's avatar
pixhawk committed
368
        rate10hzCounter = 1;
pixhawk's avatar
pixhawk committed
369

370 371 372 373
        double lastX = x;
        double lastY = y;
        double lastZ = z;
        double hackDt = 0.1f; // 100 ms
374

pixhawk's avatar
pixhawk committed
375
        // Move X Position
376 377 378
        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);
379

380 381 382
        double xSpeed = (x - lastX)/hackDt;
        double ySpeed = (y - lastY)/hackDt;
        double zSpeed = (z - lastZ)/hackDt;
383

384 385


386 387
        circleCounter++;

388

389 390 391
//        x = (x > 5.0f) ? 5.0f : x;
//        y = (y > 5.0f) ? 5.0f : y;
//        z = (z > 3.0f) ? 3.0f : z;
392

393 394 395
//        x = (x < -5.0f) ? -5.0f : x;
//        y = (y < -5.0f) ? -5.0f : y;
//        z = (z < -3.0f) ? -3.0f : z;
396

pixhawk's avatar
pixhawk committed
397 398
        // Send back new setpoint
        mavlink_message_t ret;
LM's avatar
LM committed
399
        mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI);
400 401 402 403 404 405
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        // Send back new position
lm's avatar
lm committed
406
        mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed);
407
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
pixhawk's avatar
pixhawk committed
408 409 410
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
411

412 413 414 415 416 417
//        // 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;
418

pixhawk's avatar
pixhawk committed
419
        // GLOBAL POSITION
lm's avatar
lm committed
420
        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, 0, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
pixhawk's avatar
pixhawk committed
421 422 423 424 425
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

426 427 428 429 430 431
        // GLOBAL POSITION VEHICLE 2
        mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
432

433 434 435
//        // 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
436 437


438
//        // GLOBAL POSITION VEHICLE 3
439
//        mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (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);
440 441 442 443
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
444

445
        static int rcCounter = 0;
446
        if (rcCounter == 2) {
447
            mavlink_rc_channels_raw_t chan;
lm's avatar
lm committed
448
            chan.time_boot_ms = 0;
lm's avatar
lm committed
449
            chan.port = 0;
450 451 452 453 454 455 456
            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;
457
            chan.chan8_raw = 0;
pixhawk's avatar
pixhawk committed
458
            chan.rssi = 100;
459
            mavlink_msg_rc_channels_raw_encode(systemId, componentId, &msg, &chan);
460 461 462 463 464 465 466 467 468
            // 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
469 470 471
    }

    // 1 HZ TASKS
472
    if (rate1hzCounter == 1000 / rate / 1) {
pixhawk's avatar
pixhawk committed
473 474
        // STATE
        static int statusCounter = 0;
475
        if (statusCounter == 100) {
476
            system.base_mode = (system.base_mode + 1) % MAV_MODE_ENUM_END;
pixhawk's avatar
pixhawk committed
477 478 479 480
            statusCounter = 0;
        }
        statusCounter++;

481
        static int detectionCounter = 6;
482
        if (detectionCounter % 10 == 0) {
lm's avatar
lm committed
483
#ifdef MAVLINK_ENABLED_PIXHAWK
484 485 486
            mavlink_pattern_detected_t detected;
            detected.confidence = 5.0f;

487
            if (detectionCounter == 10) {
488 489 490
                char fileName[] = "patterns/face5.png";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
491
            } else if (detectionCounter == 20) {
492 493 494
                char fileName[] = "7";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
495
            } else if (detectionCounter == 30) {
496 497 498
                char fileName[] = "patterns/einstein.bmp";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
499
            } else if (detectionCounter == 40) {
500 501 502
                char fileName[] = "F";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
503
            } else if (detectionCounter == 50) {
504 505 506
                char fileName[] = "patterns/face2.png";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 0; // 0: Pattern, 1: Letter
507
            } else if (detectionCounter == 60) {
508 509 510 511 512 513
                char fileName[] = "H";
                memcpy(detected.file, fileName, sizeof(fileName));
                detected.type = 1; // 0: Pattern, 1: Letter
                detectionCounter = 0;
            }
            detected.detected = 1;
514
            mavlink_msg_pattern_detected_encode(systemId, componentId, &msg, &detected);
515 516 517 518 519 520 521 522 523 524
            // 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++;

525
        status.voltage_battery = voltage * 1000; // millivolts
526
        status.load = 33 * detectionCounter % 1000;
pixhawk's avatar
pixhawk committed
527 528

        // Pack message and get size of encoded byte string
529
        mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
530
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
531
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
532 533 534 535
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

536
        // Pack debug text message
pixhawk's avatar
pixhawk committed
537
        mavlink_statustext_t text;
538
        text.severity = 0;
539
        strcpy((char*)(text.text), "Text message from system 32");
pixhawk's avatar
pixhawk committed
540 541
        mavlink_msg_statustext_encode(systemId, componentId, &msg, &text);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
542 543 544
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
545 546
        /*
        // Pack message and get size of encoded byte string
547
        mavlink_msg_boot_pack(systemId, componentId, &msg, version);
pixhawk's avatar
pixhawk committed
548
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
549
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
550 551 552 553
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;*/

pixhawk's avatar
pixhawk committed
554 555
        // HEARTBEAT

pixhawk's avatar
pixhawk committed
556
        static int typeCounter = 0;
557
        uint8_t mavType;
lm's avatar
lm committed
558 559
        if (typeCounter < 10)
        {
lm's avatar
lm committed
560
            mavType = MAV_TYPE_QUADROTOR;
lm's avatar
lm committed
561 562 563 564
        }
        else
        {
            mavType = typeCounter % (MAV_TYPE_GCS);
565
        }
pixhawk's avatar
pixhawk committed
566 567
        typeCounter++;

pixhawk's avatar
pixhawk committed
568
        // Pack message and get size of encoded byte string
569
        mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK, system.base_mode, system.custom_mode, system.system_status);
pixhawk's avatar
pixhawk committed
570
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
571
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
lm's avatar
lm committed
572
        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
pixhawk's avatar
pixhawk committed
573 574 575 576
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

577
        // Pack message and get size of encoded byte string
578
        mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
579 580 581 582 583 584 585
        // Allocate buffer with packet data
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
586

pixhawk's avatar
pixhawk committed
587 588 589 590 591 592 593
        // Send controller states

        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;


594

595
//        // HEARTBEAT VEHICLE 2
pixhawk's avatar
pixhawk committed
596

597
//        // Pack message and get size of encoded byte string
598
//        mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA);
599 600 601 602 603
//        // 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
604

605
//        // HEARTBEAT VEHICLE 3
606

607
//        // Pack message and get size of encoded byte string
608
//        mavlink_msg_heartbeat_pack(60, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
609 610 611 612 613
//        // Allocate buffer with packet data
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
614

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

pixhawk's avatar
pixhawk committed
623
        rate1hzCounter = 1;
pixhawk's avatar
pixhawk committed
624 625 626 627 628
    }

    // FULL RATE TASKS
    // Default is 50 Hz

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

pixhawk's avatar
pixhawk committed
634
        //streampointer = 0;
pixhawk's avatar
pixhawk committed
635 636 637 638

        // Attitude

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

        rate50hzCounter = 1;
pixhawk's avatar
pixhawk committed
647
    }*/
pixhawk's avatar
pixhawk committed
648 649

    readyBufferMutex.lock();
650
    for (unsigned int i = 0; i < streampointer; i++) {
pixhawk's avatar
pixhawk committed
651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671
        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)
{
pixhawk's avatar
pixhawk committed
672 673
    // Parse bytes
    mavlink_message_t msg;
674
    mavlink_status_t comm;
pixhawk's avatar
pixhawk committed
675

676 677 678 679 680
    uint8_t stream[2048];
    int streampointer = 0;
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength = 0;

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

LM's avatar
LM committed
690 691
            switch (msg.msgid)
            {
pixhawk's avatar
pixhawk committed
692
                // SET THE SYSTEM MODE
LM's avatar
LM committed
693 694
            case MAVLINK_MSG_ID_SET_MODE:
            {
695 696 697
                mavlink_set_mode_t mode;
                mavlink_msg_set_mode_decode(&msg, &mode);
                // Set mode indepent of mode.target
698
                system.base_mode = mode.base_mode;
699 700
            }
            break;
LM's avatar
LM committed
701 702
            case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
            {
703 704
                mavlink_set_local_position_setpoint_t set;
                mavlink_msg_set_local_position_setpoint_decode(&msg, &set);
705 706 707 708 709 710 711
                spX = set.x;
                spY = set.y;
                spZ = set.z;
                spYaw = set.yaw;

                // Send back new setpoint
                mavlink_message_t ret;
lm's avatar
lm committed
712
                mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw);
713 714 715 716 717 718 719
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;
            }
            break;
            // EXECUTE OPERATOR ACTIONS
LM's avatar
LM committed
720 721
            case MAVLINK_MSG_ID_COMMAND_LONG:
            {
pixhawk's avatar
pixhawk committed
722 723
                mavlink_command_long_t action;
                mavlink_msg_command_long_decode(&msg, &action);
724

725
//                qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
726

lm's avatar
lm committed
727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752
                // 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;
//                }
753 754
            }
            break;
lm's avatar
lm committed
755
#ifdef MAVLINK_ENABLED_PIXHAWK
756 757 758
            case MAVLINK_MSG_ID_MANUAL_CONTROL: {
                mavlink_manual_control_t control;
                mavlink_msg_manual_control_decode(&msg, &control);
759
//                qDebug() << "\n" << "ROLL:" << control.x << "PITCH:" << control.y;
760 761
            }
            break;
762
#endif
LM's avatar
LM committed
763 764
            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
            {
765
//                qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
766 767
                mavlink_param_request_list_t read;
                mavlink_msg_param_request_list_decode(&msg, &read);
LM's avatar
LM committed
768 769 770 771 772 773 774 775 776 777
                if (read.target_system == systemId)
                {
                    // 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) {
                            // Pack message and get size of encoded byte string
778
                            mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAV_PARAM_TYPE_REAL32, onboardParams.size(), j);
LM's avatar
LM committed
779 780 781 782 783 784 785
                            // Allocate buffer with packet data
                            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                            //add data into datastream
                            memcpy(stream+streampointer,buffer, bufferlength);
                            streampointer+=bufferlength;
                        }
                        j++;
786
                    }
787

788
//                    qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
LM's avatar
LM committed
789
                }
790
            }
LM's avatar
LM committed
791 792 793
                break;
            case MAVLINK_MSG_ID_PARAM_SET:
            {
794
                // Drop on even milliseconds
LM's avatar
LM committed
795 796
                if (QGC::groundTimeMilliseconds() % 2 == 0)
                {
797
//                    qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER";
798 799 800 801 802
                    mavlink_param_set_t set;
                    mavlink_msg_param_set_decode(&msg, &set);
                    //                    if (set.target_system == systemId)
                    //                    {
                    QString key = QString((char*)set.param_id);
LM's avatar
LM committed
803 804
                    if (onboardParams.contains(key))
                    {
805 806
                        onboardParams.remove(key);
                        onboardParams.insert(key, set.param_value);
807 808

                        // Pack message and get size of encoded byte string
809
                        mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
810 811 812 813 814 815
                        // Allocate buffer with packet data
                        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                        //add data into datastream
                        memcpy(stream+streampointer,buffer, bufferlength);
                        streampointer+=bufferlength;
                    }
816 817 818 819
                    //                    }
                }
            }
            break;
LM's avatar
LM committed
820 821
            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
            {
822
//                qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
823 824 825 826
                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);
LM's avatar
LM committed
827 828
                if (onboardParams.contains(key))
                {
829 830 831
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
832
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
833 834 835 836 837 838
                    // 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;
LM's avatar
LM committed
839
                }
840
                else if (read.param_index >= 0 && read.param_index < onboardParams.keys().size())
LM's avatar
LM committed
841
                {
842 843 844 845
                    key = onboardParams.keys().at(read.param_index);
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
846
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
847 848 849 850 851 852
                    // 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;
853
                }
854 855
            }
            break;
pixhawk's avatar
pixhawk committed
856 857
            }
        }
pixhawk's avatar
pixhawk committed
858
    }
pixhawk's avatar
pixhawk committed
859

860
    readyBufferMutex.lock();
LM's avatar
LM committed
861 862
    for (int i = 0; i < streampointer; i++)
    {
863 864 865 866
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

pixhawk's avatar
pixhawk committed
867
    // Update comm status
868
    status.errors_comm = comm.packet_rx_drop_count;
pixhawk's avatar
pixhawk committed
869

pixhawk's avatar
pixhawk committed
870 871 872
}


873 874
void MAVLinkSimulationLink::readBytes()
{
pixhawk's avatar
pixhawk committed
875 876
    // Lock concurrent resource readyBuffer
    readyBufferMutex.lock();
877 878
    const qint64 maxLength = 2048;
    char data[maxLength];
pixhawk's avatar
pixhawk committed
879
    qint64 len = qMin((qint64)readyBuffer.size(), maxLength);
pixhawk's avatar
pixhawk committed
880

881
    for (unsigned int i = 0; i < len; i++) {
pixhawk's avatar
pixhawk committed
882 883 884 885 886 887 888 889
        *(data + i) = readyBuffer.takeFirst();
    }

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

    readyBufferMutex.unlock();

890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905
//    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
906 907 908 909 910 911 912 913
}

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

Lorenz Meier's avatar
Lorenz Meier committed
917 918
    if(isConnected())
    {
pixhawk's avatar
pixhawk committed
919 920 921 922 923
        //        timer->stop();

        _isConnected = false;

        emit disconnected();
924
        emit connected(false);
pixhawk's avatar
pixhawk committed
925

926
        //exit();
pixhawk's avatar
pixhawk committed
927 928 929 930 931 932 933 934 935 936 937 938 939 940
    }

    return true;
}

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

    start(LowPriority);
LM's avatar
LM committed
945
    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883);
946
    Q_UNUSED(mav1);
947 948
//    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
//    Q_UNUSED(mav2);
pixhawk's avatar
pixhawk committed
949 950 951 952
    //    timer->start(rate);
    return true;
}

953 954 955 956 957 958 959 960 961 962 963 964
/**
 * 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();
}

965 966 967 968 969 970 971 972 973 974 975
/**
 * 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;

976
    if(connect) {
977 978 979 980 981 982
        this->connect();
    }

    return true;
}

pixhawk's avatar
pixhawk committed
983 984 985 986 987
/**
 * Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
988
bool MAVLinkSimulationLink::isConnected() const
989
{
pixhawk's avatar
pixhawk committed
990 991 992
    return _isConnected;
}

993
int MAVLinkSimulationLink::getId() const
pixhawk's avatar
pixhawk committed
994 995 996 997
{
    return id;
}

998
QString MAVLinkSimulationLink::getName() const
pixhawk's avatar
pixhawk committed
999 1000 1001 1002
{
    return name;
}

1003
qint64 MAVLinkSimulationLink::getNominalDataRate() const
1004
{
pixhawk's avatar
pixhawk committed
1005 1006 1007 1008
    /* 100 Mbit is reasonable fast and sufficient for all embedded applications */
    return 100000000;
}

1009 1010
qint64 MAVLinkSimulationLink::getTotalUpstream()
{
pixhawk's avatar
pixhawk committed
1011 1012 1013 1014 1015
    return 0;
    //TODO Add functionality here
    // @todo Add functionality here
}

1016 1017
qint64 MAVLinkSimulationLink::getShortTermUpstream()
{
pixhawk's avatar
pixhawk committed
1018 1019 1020
    return 0;
}

1021 1022
qint64 MAVLinkSimulationLink::getCurrentUpstream()
{
pixhawk's avatar
pixhawk committed
1023 1024 1025
    return 0;
}

1026 1027
qint64 MAVLinkSimulationLink::getMaxUpstream()
{
pixhawk's avatar
pixhawk committed
1028 1029 1030
    return 0;
}

1031
qint64 MAVLinkSimulationLink::getBitsSent() const
1032
{
pixhawk's avatar
pixhawk committed
1033 1034 1035
    return 0;
}

1036
qint64 MAVLinkSimulationLink::getBitsReceived() const
1037
{
pixhawk's avatar
pixhawk committed
1038 1039 1040
    return 0;
}

1041 1042
qint64 MAVLinkSimulationLink::getTotalDownstream()
{
pixhawk's avatar
pixhawk committed
1043 1044 1045
    return 0;
}

1046 1047
qint64 MAVLinkSimulationLink::getShortTermDownstream()
{
pixhawk's avatar
pixhawk committed
1048 1049 1050
    return 0;
}

1051 1052
qint64 MAVLinkSimulationLink::getCurrentDownstream()
{
pixhawk's avatar
pixhawk committed
1053 1054 1055
    return 0;
}

1056 1057
qint64 MAVLinkSimulationLink::getMaxDownstream()
{
pixhawk's avatar
pixhawk committed
1058 1059 1060
    return 0;
}

1061
bool MAVLinkSimulationLink::isFullDuplex() const
1062
{
pixhawk's avatar
pixhawk committed
1063 1064 1065 1066
    /* Full duplex is no problem when running in pure software, but this is a serial simulation */
    return false;
}

1067
int MAVLinkSimulationLink::getLinkQuality() const
1068
{
pixhawk's avatar
pixhawk committed
1069 1070 1071
    /* The Link quality is always perfect when running in software */
    return 100;
}