MAVLinkSimulationLink.cc 30.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)
 **/
58
MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile, int rate) :
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));
pixhawk's avatar
pixhawk committed
200
    mavlink_raw_imu_t rawImuValues;
201
    memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t));
pixhawk's avatar
pixhawk committed
202 203 204 205 206 207 208 209 210

    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;
211
    static unsigned int circleCounter = 0;
pixhawk's avatar
pixhawk committed
212 213 214 215 216 217

    // Vary values

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

pixhawk's avatar
pixhawk committed
220
    static int state = 0;
pixhawk's avatar
pixhawk committed
221

Lorenz Meier's avatar
Lorenz Meier committed
222 223
    if (state == 0)
    {
pixhawk's avatar
pixhawk committed
224 225 226 227 228
        state++;
    }


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

253 254
            if (ok) {
                if (timeOffset == 0) {
pixhawk's avatar
pixhawk committed
255 256 257 258
                    timeOffset = time;
                    baseTime = time;
                }

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

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

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

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

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

296
                    if (keys.value(i, "") == "Gyro_Psi") {
pixhawk's avatar
pixhawk committed
297
                        rawImuValues.zgyro = d;
298
                        attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
299
                    }
300
                    if (keys.value(i, "") == "roll_IMU") {
pixhawk's avatar
pixhawk committed
301 302 303
                        attitude.roll = d;
                    }

304
                    if (keys.value(i, "") == "pitch_IMU") {
pixhawk's avatar
pixhawk committed
305 306 307
                        attitude.pitch = d;
                    }

308
                    if (keys.value(i, "") == "yaw_IMU") {
pixhawk's avatar
pixhawk committed
309 310 311 312 313 314 315 316 317 318
                        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
319
                attitude.time_boot_ms = time/1000;
pixhawk's avatar
pixhawk committed
320
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
321
                mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
pixhawk's avatar
pixhawk committed
322
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
323
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
324 325 326 327 328
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

                // IMU
329
                rawImuValues.time_usec = time;
pixhawk's avatar
pixhawk committed
330 331 332 333
                rawImuValues.xmag = 0;
                rawImuValues.ymag = 0;
                rawImuValues.zmag = 0;
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
334
                mavlink_msg_raw_imu_encode(systemId, componentId, &msg, &rawImuValues);
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 342
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

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

343
                //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
pixhawk's avatar
pixhawk committed
344 345 346

            }

pixhawk's avatar
pixhawk committed
347
        }
pixhawk's avatar
pixhawk committed
348 349 350 351 352 353

        rate50hzCounter = 1;
    }


    // 10 HZ TASKS
354
    if (rate10hzCounter == 1000 / rate / 9) {
pixhawk's avatar
pixhawk committed
355
        rate10hzCounter = 1;
pixhawk's avatar
pixhawk committed
356

357 358 359 360
        double lastX = x;
        double lastY = y;
        double lastZ = z;
        double hackDt = 0.1f; // 100 ms
361

pixhawk's avatar
pixhawk committed
362
        // Move X Position
363 364 365
        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);
366

367 368 369
        double xSpeed = (x - lastX)/hackDt;
        double ySpeed = (y - lastY)/hackDt;
        double zSpeed = (z - lastZ)/hackDt;
370

371 372


373 374
        circleCounter++;

375

376 377 378
//        x = (x > 5.0f) ? 5.0f : x;
//        y = (y > 5.0f) ? 5.0f : y;
//        z = (z > 3.0f) ? 3.0f : z;
379

380 381 382
//        x = (x < -5.0f) ? -5.0f : x;
//        y = (y < -5.0f) ? -5.0f : y;
//        z = (z < -3.0f) ? -3.0f : z;
383

pixhawk's avatar
pixhawk committed
384
        mavlink_message_t ret;
385 386

        // Send back new position
lm's avatar
lm committed
387
        mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed);
388
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
pixhawk's avatar
pixhawk committed
389 390 391
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
392

393 394 395 396 397 398
//        // 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;
399

pixhawk's avatar
pixhawk committed
400
        // GLOBAL POSITION
lm's avatar
lm committed
401
        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
402 403 404 405 406
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

407 408 409 410 411 412
        // 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;
413

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


419
//        // GLOBAL POSITION VEHICLE 3
420
//        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);
421 422 423 424
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
425

426
        static int rcCounter = 0;
427
        if (rcCounter == 2) {
428
            mavlink_rc_channels_t chan;
lm's avatar
lm committed
429
            chan.time_boot_ms = 0;
430 431 432 433 434 435 436
            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;
437
            chan.chan8_raw = 0;
pixhawk's avatar
pixhawk committed
438
            chan.rssi = 100;
439
            mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &chan);
440 441 442 443 444 445 446 447 448
            // 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
449 450 451
    }

    // 1 HZ TASKS
452
    if (rate1hzCounter == 1000 / rate / 1) {
pixhawk's avatar
pixhawk committed
453 454
        // STATE
        static int statusCounter = 0;
455
        if (statusCounter == 100) {
456
            system.base_mode = (system.base_mode + 1) % MAV_MODE_ENUM_END;
pixhawk's avatar
pixhawk committed
457 458 459 460
            statusCounter = 0;
        }
        statusCounter++;

461
        static int detectionCounter = 6;
462
        if (detectionCounter % 10 == 0) {
463 464 465
        }
        detectionCounter++;

466
        status.voltage_battery = voltage * 1000; // millivolts
467
        status.load = 33 * detectionCounter % 1000;
pixhawk's avatar
pixhawk committed
468 469

        // Pack message and get size of encoded byte string
470
        mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
471
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
472
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
473 474 475 476
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

477
        // Pack debug text message
pixhawk's avatar
pixhawk committed
478
        mavlink_statustext_t text;
479
        text.severity = 0;
480
        strcpy((char*)(text.text), "Text message from system 32");
pixhawk's avatar
pixhawk committed
481 482
        mavlink_msg_statustext_encode(systemId, componentId, &msg, &text);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
483 484 485
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
486 487
        /*
        // Pack message and get size of encoded byte string
488
        mavlink_msg_boot_pack(systemId, componentId, &msg, version);
pixhawk's avatar
pixhawk committed
489
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
490
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
491 492 493 494
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;*/

pixhawk's avatar
pixhawk committed
495 496
        // HEARTBEAT

pixhawk's avatar
pixhawk committed
497
        static int typeCounter = 0;
498
        uint8_t mavType;
lm's avatar
lm committed
499 500
        if (typeCounter < 10)
        {
lm's avatar
lm committed
501
            mavType = MAV_TYPE_QUADROTOR;
lm's avatar
lm committed
502 503 504 505
        }
        else
        {
            mavType = typeCounter % (MAV_TYPE_GCS);
506
        }
pixhawk's avatar
pixhawk committed
507 508
        typeCounter++;

pixhawk's avatar
pixhawk committed
509
        // Pack message and get size of encoded byte string
510
        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
511
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
512
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
lm's avatar
lm committed
513
        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
pixhawk's avatar
pixhawk committed
514 515 516 517
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

518
        // Pack message and get size of encoded byte string
519
        mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
520 521 522 523 524 525 526
        // 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
527

pixhawk's avatar
pixhawk committed
528 529 530 531 532 533
        // Send controller states

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

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

pixhawk's avatar
pixhawk committed
542
        rate1hzCounter = 1;
pixhawk's avatar
pixhawk committed
543 544 545 546 547
    }

    // FULL RATE TASKS
    // Default is 50 Hz

pixhawk's avatar
pixhawk committed
548
    /*
pixhawk's avatar
pixhawk committed
549 550 551 552
    // 50 HZ TASKS
    if (rate50hzCounter == 1000 / rate / 50)
    {

pixhawk's avatar
pixhawk committed
553
        //streampointer = 0;
pixhawk's avatar
pixhawk committed
554 555 556 557

        // Attitude

        // Pack message and get size of encoded byte string
558
        mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0);
pixhawk's avatar
pixhawk committed
559
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
560
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
561 562 563 564 565
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        rate50hzCounter = 1;
pixhawk's avatar
pixhawk committed
566
    }*/
pixhawk's avatar
pixhawk committed
567 568

    readyBufferMutex.lock();
569
    for (unsigned int i = 0; i < streampointer; i++) {
pixhawk's avatar
pixhawk committed
570 571 572 573 574 575 576 577 578 579 580 581 582
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

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


void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
{
pixhawk's avatar
pixhawk committed
583 584
    // Parse bytes
    mavlink_message_t msg;
585
    mavlink_status_t comm;
pixhawk's avatar
pixhawk committed
586

587 588 589 590
    uint8_t stream[2048];
    int streampointer = 0;
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength = 0;
591 592 593
    
    // Initialize drop count to 0 so it isn't referenced uninitialized when returned at the bottom of this function
    comm.packet_rx_drop_count = 0;
594

pixhawk's avatar
pixhawk committed
595
    // Output all bytes as hex digits
LM's avatar
LM committed
596
    for (int i=0; i<size; i++)
lm's avatar
lm committed
597 598 599
    {
        if (mavlink_parse_char(this->id, data[i], &msg, &comm))
        {
pixhawk's avatar
pixhawk committed
600
            // MESSAGE RECEIVED!
601
//            qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
602
            emit messageReceived(msg);
pixhawk's avatar
pixhawk committed
603

LM's avatar
LM committed
604 605
            switch (msg.msgid)
            {
pixhawk's avatar
pixhawk committed
606
                // SET THE SYSTEM MODE
LM's avatar
LM committed
607 608
            case MAVLINK_MSG_ID_SET_MODE:
            {
609 610 611
                mavlink_set_mode_t mode;
                mavlink_msg_set_mode_decode(&msg, &mode);
                // Set mode indepent of mode.target
612
                system.base_mode = mode.base_mode;
613 614 615
            }
            break;
            // EXECUTE OPERATOR ACTIONS
LM's avatar
LM committed
616 617
            case MAVLINK_MSG_ID_COMMAND_LONG:
            {
pixhawk's avatar
pixhawk committed
618 619
                mavlink_command_long_t action;
                mavlink_msg_command_long_decode(&msg, &action);
620

621
//                qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
622

lm's avatar
lm committed
623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648
                // 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;
//                }
649 650
            }
            break;
LM's avatar
LM committed
651 652
            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
            {
653
//                qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
654 655
                mavlink_param_request_list_t read;
                mavlink_msg_param_request_list_decode(&msg, &read);
LM's avatar
LM committed
656 657 658 659 660 661 662 663 664 665
                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
666
                            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
667 668 669 670 671 672 673
                            // 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++;
674
                    }
675

676
//                    qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
LM's avatar
LM committed
677
                }
678
            }
LM's avatar
LM committed
679 680 681
                break;
            case MAVLINK_MSG_ID_PARAM_SET:
            {
682
                // Drop on even milliseconds
LM's avatar
LM committed
683 684
                if (QGC::groundTimeMilliseconds() % 2 == 0)
                {
685
//                    qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER";
686 687 688 689 690
                    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
691 692
                    if (onboardParams.contains(key))
                    {
693 694
                        onboardParams.remove(key);
                        onboardParams.insert(key, set.param_value);
695 696

                        // Pack message and get size of encoded byte string
697
                        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));
698 699 700 701 702 703
                        // Allocate buffer with packet data
                        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                        //add data into datastream
                        memcpy(stream+streampointer,buffer, bufferlength);
                        streampointer+=bufferlength;
                    }
704 705 706 707
                    //                    }
                }
            }
            break;
LM's avatar
LM committed
708 709
            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
            {
710
//                qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
711 712 713 714
                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
715 716
                if (onboardParams.contains(key))
                {
717 718 719
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
720
                    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));
721 722 723 724 725 726
                    // 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
727
                }
728
                else if (read.param_index >= 0 && read.param_index < onboardParams.keys().size())
LM's avatar
LM committed
729
                {
730 731 732 733
                    key = onboardParams.keys().at(read.param_index);
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
734
                    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));
735 736 737 738 739 740
                    // 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;
741
                }
742 743
            }
            break;
pixhawk's avatar
pixhawk committed
744 745
            }
        }
pixhawk's avatar
pixhawk committed
746
    }
pixhawk's avatar
pixhawk committed
747

748 749 750 751 752 753
    // Log the amount and time written out for future data rate calculations.
    // While this interface doesn't actually write any data to external systems,
    // this data "transmit" here should still count towards the outgoing data rate.
    QMutexLocker dataRateLocker(&dataRateMutex);
    logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());

754
    readyBufferMutex.lock();
LM's avatar
LM committed
755 756
    for (int i = 0; i < streampointer; i++)
    {
757 758 759 760
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

pixhawk's avatar
pixhawk committed
761
    // Update comm status
762
    status.errors_comm = comm.packet_rx_drop_count;
pixhawk's avatar
pixhawk committed
763

pixhawk's avatar
pixhawk committed
764 765 766
}


767 768
void MAVLinkSimulationLink::readBytes()
{
pixhawk's avatar
pixhawk committed
769 770
    // Lock concurrent resource readyBuffer
    readyBufferMutex.lock();
771 772
    const qint64 maxLength = 2048;
    char data[maxLength];
pixhawk's avatar
pixhawk committed
773
    qint64 len = qMin((qint64)readyBuffer.size(), maxLength);
pixhawk's avatar
pixhawk committed
774

775
    for (unsigned int i = 0; i < len; i++) {
pixhawk's avatar
pixhawk committed
776 777 778 779 780 781 782
        *(data + i) = readyBuffer.takeFirst();
    }

    QByteArray b(data, len);
    emit bytesReceived(this, b);
    readyBufferMutex.unlock();

783 784 785 786
    // Log the amount and time received for future data rate calculations.
    QMutexLocker dataRateLocker(&dataRateMutex);
    logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, len, QDateTime::currentMSecsSinceEpoch());

pixhawk's avatar
pixhawk committed
787 788 789 790 791 792 793 794
}

/**
 * Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection
 * couldn't be disconnected.
 **/
795
bool MAVLinkSimulationLink::_disconnect(void)
796
{
pixhawk's avatar
pixhawk committed
797

Lorenz Meier's avatar
Lorenz Meier committed
798 799
    if(isConnected())
    {
pixhawk's avatar
pixhawk committed
800 801 802 803 804
        //        timer->stop();

        _isConnected = false;

        emit disconnected();
805
        emit connected(false);
pixhawk's avatar
pixhawk committed
806

807
        //exit();
pixhawk's avatar
pixhawk committed
808 809 810 811 812 813 814 815 816 817 818
    }

    return true;
}

/**
 * Connect the link.
 *
 * @return True if connection has been established, false if connection
 * couldn't be established.
 **/
819
bool MAVLinkSimulationLink::_connect(void)
pixhawk's avatar
pixhawk committed
820 821
{
    _isConnected = true;
822 823
    emit connected();
    emit connected(true);
pixhawk's avatar
pixhawk committed
824 825

    start(LowPriority);
LM's avatar
LM committed
826
    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883);
827
    Q_UNUSED(mav1);
828 829
//    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
//    Q_UNUSED(mav2);
pixhawk's avatar
pixhawk committed
830 831 832 833 834 835 836 837 838
    //    timer->start(rate);
    return true;
}

/**
 * Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
839
bool MAVLinkSimulationLink::isConnected() const
840
{
pixhawk's avatar
pixhawk committed
841 842 843
    return _isConnected;
}

844
int MAVLinkSimulationLink::getId() const
pixhawk's avatar
pixhawk committed
845 846 847 848
{
    return id;
}

849
QString MAVLinkSimulationLink::getName() const
pixhawk's avatar
pixhawk committed
850 851 852 853
{
    return name;
}

854
qint64 MAVLinkSimulationLink::getConnectionSpeed() const
855
{
pixhawk's avatar
pixhawk committed
856 857
    /* 100 Mbit is reasonable fast and sufficient for all embedded applications */
    return 100000000;
858 859 860 861 862 863 864 865 866 867 868
}

qint64 MAVLinkSimulationLink::getCurrentInDataRate() const
{
    return 0;
}

qint64 MAVLinkSimulationLink::getCurrentOutDataRate() const
{
    return 0;
}