UAS.cc 64.5 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Donald Gagne's avatar
Donald Gagne committed
10 11 12
// NO NEW CODE HERE
// UASInterface, UAS.h/cc are deprecated. All new functionality should go into Vehicle.h/cc
//
13 14 15 16 17 18

#include <QList>
#include <QTimer>
#include <QSettings>
#include <iostream>
#include <QDebug>
Don Gagne's avatar
Don Gagne committed
19

20 21
#include <cmath>
#include <qmath.h>
Don Gagne's avatar
Don Gagne committed
22

23 24 25
#include <limits>
#include <cstdlib>

26 27 28 29 30 31 32
#include "UAS.h"
#include "LinkInterface.h"
#include "QGC.h"
#include "GAudioOutput.h"
#include "MAVLinkProtocol.h"
#include "QGCMAVLink.h"
#include "LinkManager.h"
Gus Grubba's avatar
Gus Grubba committed
33
#ifndef NO_SERIAL_LINK
34
#include "SerialLink.h"
dogmaphobic's avatar
dogmaphobic committed
35
#endif
Don Gagne's avatar
Don Gagne committed
36
#include "FirmwarePluginManager.h"
37
#include "QGCLoggingCategory.h"
38
#include "Vehicle.h"
39
#include "Joystick.h"
40
#include "QGCApplication.h"
41

42
QGC_LOGGING_CATEGORY(UASLog, "UASLog")
43

Don Gagne's avatar
Don Gagne committed
44
// THIS CLASS IS DEPRECATED. ALL NEW FUNCTIONALITY SHOULD GO INTO Vehicle class
45
UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager) : UASInterface(),
46 47
    lipoFull(4.2f),
    lipoEmpty(3.5f),
48
    uasId(vehicle->id()),
49 50
    unknownPackets(),
    mavlink(protocol),
51 52 53 54 55 56
    receiveDropRate(0),
    sendDropRate(0),

    status(-1),

    startTime(QGC::groundTimeMilliseconds()),
57
    onboardTimeOffset(0),
58

59 60 61 62 63 64 65 66
    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),
67

68
#ifndef __mobile__
69
    fileManager(this, vehicle),
70
#endif
Don Gagne's avatar
Don Gagne committed
71

72 73 74
    attitudeKnown(false),
    attitudeStamped(false),
    lastAttitude(0),
75

76 77 78 79
    roll(0.0),
    pitch(0.0),
    yaw(0.0),

Don Gagne's avatar
Don Gagne committed
80 81
    imagePackets(0),    // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images

Don Gagne's avatar
Don Gagne committed
82 83 84
    blockHomePositionChanges(false),
    receivedMode(false),

85 86
    // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
    // TODO: calibrate stand-still pixhawk variances
87
    xacc_var(0.6457f),
dogmaphobic's avatar
dogmaphobic committed
88
    yacc_var(0.7048f),
89
    zacc_var(0.97885f),
dogmaphobic's avatar
dogmaphobic committed
90 91 92
    rollspeed_var(0.8126f),
    pitchspeed_var(0.6145f),
    yawspeed_var(0.5852f),
93 94 95 96 97 98 99
    xmag_var(0.2393f),
    ymag_var(0.2283f),
    zmag_var(0.1665f),
    abs_pressure_var(0.5802f),
    diff_pressure_var(0.5802f),
    pressure_alt_var(0.5802f),
    temperature_var(0.7145f),
100
    /*
101 102 103 104 105 106 107 108 109 110 111 112 113
    xacc_var(0.0f),
    yacc_var(0.0f),
    zacc_var(0.0f),
    rollspeed_var(0.0f),
    pitchspeed_var(0.0f),
    yawspeed_var(0.0f),
    xmag_var(0.0f),
    ymag_var(0.0f),
    zmag_var(0.0f),
    abs_pressure_var(0.0f),
    diff_pressure_var(0.0f),
    pressure_alt_var(0.0f),
    temperature_var(0.0f),
114
    */
115

dogmaphobic's avatar
dogmaphobic committed
116
#ifndef __mobile__
117
    simulation(0),
dogmaphobic's avatar
dogmaphobic committed
118
#endif
119 120

    // The protected members.
121 122 123 124
    connectionLost(false),
    lastVoltageWarning(0),
    lastNonNullTime(0),
    onboardTimeOffsetInvalidCount(0),
125
    hilEnabled(false),
126 127
    sensorHil(false),
    lastSendTimeGPS(0),
128
    lastSendTimeSensors(0),
129
    lastSendTimeOpticalFlow(0),
130 131
    _vehicle(vehicle),
    _firmwarePluginManager(firmwarePluginManager)
132
{
dogmaphobic's avatar
dogmaphobic committed
133

134 135 136 137 138
    for (unsigned int i = 0; i<255;++i)
    {
        componentID[i] = -1;
        componentMulti[i] = false;
    }
139

140
#ifndef __mobile__
141
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage);
142
#endif
143

144 145 146 147 148 149 150 151 152 153 154
    color = UASInterface::getNextColor();
}

/**
* @ return the id of the uas
*/
int UAS::getUASID() const
{
    return uasId;
}

155
void UAS::receiveMessage(mavlink_message_t message)
156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240
{
    if (!components.contains(message.compid))
    {
        QString componentName;

        switch (message.compid)
        {
        case MAV_COMP_ID_ALL:
        {
            componentName = "ANONYMOUS";
            break;
        }
        case MAV_COMP_ID_IMU:
        {
            componentName = "IMU #1";
            break;
        }
        case MAV_COMP_ID_CAMERA:
        {
            componentName = "CAMERA";
            break;
        }
        case MAV_COMP_ID_MISSIONPLANNER:
        {
            componentName = "MISSIONPLANNER";
            break;
        }
        }

        components.insert(message.compid, componentName);
    }

    //    qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;

    // Only accept messages from this system (condition 1)
    // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled
    // and we already got one attitude packet
    if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
    {
        QString uasState;
        QString stateDescription;

        bool multiComponentSourceDetected = false;
        bool wrongComponent = false;

        switch (message.compid)
        {
        case MAV_COMP_ID_IMU_2:
            // Prefer IMU 2 over IMU 1 (FIXME)
            componentID[message.msgid] = MAV_COMP_ID_IMU_2;
            break;
        default:
            // Do nothing
            break;
        }

        // Store component ID
        if (componentID[message.msgid] == -1)
        {
            // Prefer the first component
            componentID[message.msgid] = message.compid;
        }
        else
        {
            // Got this message already
            if (componentID[message.msgid] != message.compid)
            {
                componentMulti[message.msgid] = true;
                wrongComponent = true;
            }
        }

        if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true;


        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);
241 242 243

            // Send the base_mode and system_status values to the plotter. This uses the ground time
            // so the Ground Time checkbox must be ticked for these values to display
244
            quint64 time = getUnixTime();
245 246 247 248 249
            QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid);
            emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time);
            emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time);
            emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time);

250 251
            // We got the mode
            receivedMode = true;
252 253 254
        }

            break;
255

256 257 258 259 260 261 262 263 264
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_sys_status_t state;
            mavlink_msg_sys_status_decode(&message, &state);

265
            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
266
            quint64 time = getUnixTime();
267 268 269 270 271 272 273
            QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid);
            emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time);
            emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time);
            emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time);
            emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time);
            emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time);
            emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time);
274 275
            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);

276 277 278 279
            // Process CPU load.
            emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
            emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
        }
280 281 282 283 284 285 286 287 288 289 290 291
            break;
        case MAVLINK_MSG_ID_ATTITUDE:
        {
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&message, &attitude);
            quint64 time = getUnixReferenceTime(attitude.time_boot_ms);

            emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time);

            if (!wrongComponent)
            {
                lastAttitude = time;
292 293 294
                setRoll(QGC::limitAngleToPMPIf(attitude.roll));
                setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
                setYaw(QGC::limitAngleToPMPIf(attitude.yaw));
295

296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354
                attitudeKnown = true;
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
            }
        }
            break;
        case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        {
            mavlink_attitude_quaternion_t attitude;
            mavlink_msg_attitude_quaternion_decode(&message, &attitude);
            quint64 time = getUnixReferenceTime(attitude.time_boot_ms);

            double a = attitude.q1;
            double b = attitude.q2;
            double c = attitude.q3;
            double d = attitude.q4;

            double aSq = a * a;
            double bSq = b * b;
            double cSq = c * c;
            double dSq = d * d;
            float dcm[3][3];
            dcm[0][0] = aSq + bSq - cSq - dSq;
            dcm[0][1] = 2.0 * (b * c - a * d);
            dcm[0][2] = 2.0 * (a * c + b * d);
            dcm[1][0] = 2.0 * (b * c + a * d);
            dcm[1][1] = aSq - bSq + cSq - dSq;
            dcm[1][2] = 2.0 * (c * d - a * b);
            dcm[2][0] = 2.0 * (b * d - a * c);
            dcm[2][1] = 2.0 * (a * b + c * d);
            dcm[2][2] = aSq - bSq - cSq + dSq;

            float phi, theta, psi;
            theta = asin(-dcm[2][0]);

            if (fabs(theta - M_PI_2) < 1.0e-3f) {
                phi = 0.0f;
                psi = (atan2(dcm[1][2] - dcm[0][1],
                        dcm[0][2] + dcm[1][1]) + phi);

            } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
                phi = 0.0f;
                psi = atan2f(dcm[1][2] - dcm[0][1],
                          dcm[0][2] + dcm[1][1] - phi);

            } else {
                phi = atan2f(dcm[2][1], dcm[2][2]);
                psi = atan2f(dcm[1][0], dcm[0][0]);
            }

            emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi),
                                 QGC::limitAngleToPMPIf(theta),
                                 QGC::limitAngleToPMPIf(psi), time);

            if (!wrongComponent)
            {
                lastAttitude = time;
                setRoll(QGC::limitAngleToPMPIf(phi));
                setPitch(QGC::limitAngleToPMPIf(theta));
                setYaw(QGC::limitAngleToPMPIf(psi));
355 356

                attitudeKnown = true;
357
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375
            }
        }
            break;
        case MAVLINK_MSG_ID_HIL_CONTROLS:
        {
            mavlink_hil_controls_t hil;
            mavlink_msg_hil_controls_decode(&message, &hil);
            emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
        }
            break;
        case MAVLINK_MSG_ID_VFR_HUD:
        {
            mavlink_vfr_hud_t hud;
            mavlink_msg_vfr_hud_decode(&message, &hud);
            quint64 time = getUnixTime();

            if (!attitudeKnown)
            {
376
                setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
377
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
378 379 380 381 382 383 384 385 386 387 388
            }
        }
            break;
        case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
        {
            mavlink_global_vision_position_estimate_t pos;
            mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
            quint64 time = getUnixTime(pos.usec);
            emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
        }
            break;
389

390 391
        case MAVLINK_MSG_ID_PARAM_VALUE:
        {
392 393 394
            mavlink_param_value_t rawValue;
            mavlink_msg_param_value_decode(&message, &rawValue);
            QByteArray bytes(rawValue.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
395 396 397
            // Construct a string stopping at the first NUL (0) character, else copy the whole
            // byte array (max MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN, so safe)
            QString parameterName(bytes);
398 399 400
            mavlink_param_union_t paramVal;
            paramVal.param_float = rawValue.param_value;
            paramVal.type = rawValue.param_type;
401

402 403
            processParamValueMsg(message, parameterName,rawValue,paramVal);
         }
404
            break;
405
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
406
        {
407 408 409 410
            mavlink_attitude_target_t out;
            mavlink_msg_attitude_target_decode(&message, &out);
            float roll, pitch, yaw;
            mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw);
411
            quint64 time = getUnixTimeFromMs(out.time_boot_ms);
412 413

            // For plotting emit roll sp, pitch sp and yaw sp values
414 415 416
            emit valueChanged(uasId, "roll sp", "rad", roll, time);
            emit valueChanged(uasId, "pitch sp", "rad", pitch, time);
            emit valueChanged(uasId, "yaw sp", "rad", yaw, time);
417 418
        }
            break;
dogmaphobic's avatar
dogmaphobic committed
419

420 421 422
        case MAVLINK_MSG_ID_STATUSTEXT:
        {
            QByteArray b;
423
            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
424
            mavlink_msg_statustext_get_text(&message, b.data());
dogmaphobic's avatar
dogmaphobic committed
425

426 427
            // Ensure NUL-termination
            b[b.length()-1] = '\0';
428 429 430
            QString text = QString(b);
            int severity = mavlink_msg_statustext_get_severity(&message);

dogmaphobic's avatar
dogmaphobic committed
431 432
        // If the message is NOTIFY or higher severity, or starts with a '#',
        // then read it aloud.
433
            if (text.startsWith("#") || severity <= MAV_SEVERITY_NOTICE)
434
            {
435
                text.remove("#");
436
                emit textMessageReceived(uasId, message.compid, severity, text);
437
                _say(text.toLower(), severity);
438 439 440 441 442 443 444
            }
            else
            {
                emit textMessageReceived(uasId, message.compid, severity, text);
            }
        }
            break;
445

446 447 448 449 450 451 452 453 454 455 456 457
        case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
        {
            mavlink_data_transmission_handshake_t p;
            mavlink_msg_data_transmission_handshake_decode(&message, &p);
            imageSize = p.size;
            imagePackets = p.packets;
            imagePayload = p.payload;
            imageQuality = p.jpg_quality;
            imageType = p.type;
            imageWidth = p.width;
            imageHeight = p.height;
            imageStart = QGC::groundTimeMilliseconds();
458 459
            imagePacketsArrived = 0;

460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475
        }
            break;

        case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
        {
            mavlink_encapsulated_data_t img;
            mavlink_msg_encapsulated_data_decode(&message, &img);
            int seq = img.seqnr;
            int pos = seq * imagePayload;

            // Check if we have a valid transaction
            if (imagePackets == 0)
            {
                // NO VALID TRANSACTION - ABORT
                // Restart statemachine
                imagePacketsArrived = 0;
Don Gagne's avatar
Don Gagne committed
476
                break;
477 478 479 480 481 482 483 484 485 486 487 488 489
            }

            for (int i = 0; i < imagePayload; ++i)
            {
                if (pos <= imageSize) {
                    imageRecBuffer[pos] = img.data[i];
                }
                ++pos;
            }

            ++imagePacketsArrived;

            // emit signal if all packets arrived
490
            if (imagePacketsArrived >= imagePackets)
491 492
            {
                // Restart statemachine
Don Gagne's avatar
Don Gagne committed
493 494
                imagePackets = 0;
                imagePacketsArrived = 0;
495 496 497 498 499
                emit imageReady(this);
            }
        }
            break;

dogmaphobic's avatar
dogmaphobic committed
500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515
        case MAVLINK_MSG_ID_LOG_ENTRY:
        {
            mavlink_log_entry_t log;
            mavlink_msg_log_entry_decode(&message, &log);
            emit logEntry(this, log.time_utc, log.size, log.id, log.num_logs, log.last_log_num);
        }
            break;

        case MAVLINK_MSG_ID_LOG_DATA:
        {
            mavlink_log_data_t log;
            mavlink_msg_log_data_decode(&message, &log);
            emit logData(this, log.ofs, log.id, log.count, log.data);
        }
            break;

516 517 518 519 520 521
        default:
            break;
        }
    }
}

522
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
523
{
524 525 526
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
527

528 529 530 531 532
    int gyroCal = 0;
    int magCal = 0;
    int airspeedCal = 0;
    int radioCal = 0;
    int accelCal = 0;
533
    int pressureCal = 0;
534
    int escCal = 0;
dogmaphobic's avatar
dogmaphobic committed
535

536
    switch (calType) {
537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557
    case StartCalibrationGyro:
        gyroCal = 1;
        break;
    case StartCalibrationMag:
        magCal = 1;
        break;
    case StartCalibrationAirspeed:
        airspeedCal = 1;
        break;
    case StartCalibrationRadio:
        radioCal = 1;
        break;
    case StartCalibrationCopyTrims:
        radioCal = 2;
        break;
    case StartCalibrationAccel:
        accelCal = 1;
        break;
    case StartCalibrationLevel:
        accelCal = 2;
        break;
558 559 560
    case StartCalibrationPressure:
        pressureCal = 1;
        break;
561 562 563 564 565 566 567 568 569
    case StartCalibrationEsc:
        escCal = 1;
        break;
    case StartCalibrationUavcanEsc:
        escCal = 2;
        break;
    case StartCalibrationCompassMot:
        airspeedCal = 1; // ArduPilot, bit of a hack
        break;
570
    }
dogmaphobic's avatar
dogmaphobic committed
571

572 573
    // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
    // causes the retry logic to break down.
574
    mavlink_message_t msg;
575 576 577 578 579 580 581 582 583 584
    mavlink_msg_command_long_pack_chan(mavlink->getSystemId(),
                                       mavlink->getComponentId(),
                                       _vehicle->priorityLink()->mavlinkChannel(),
                                       &msg,
                                       uasId,
                                       _vehicle->defaultComponentId(),   // target component
                                       MAV_CMD_PREFLIGHT_CALIBRATION,    // command id
                                       0,                                // 0=first transmission of command
                                       gyroCal,                          // gyro cal
                                       magCal,                           // mag cal
585
                                       pressureCal,                      // ground pressure
586 587 588 589 590
                                       radioCal,                         // radio cal
                                       accelCal,                         // accel cal
                                       airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot
                                       escCal);                          // esc cal
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
591 592
}

593
void UAS::stopCalibration(void)
594
{
595 596 597
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
598

599 600 601 602 603 604 605 606 607 608
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_CALIBRATION,     // command id
                             true,                              // showError
                             0,                                 // gyro cal
                             0,                                 // mag cal
                             0,                                 // ground pressure
                             0,                                 // radio cal
                             0,                                 // accel cal
                             0,                                 // airspeed cal
                             0);                                // unused
609 610
}

611 612
void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{
613 614 615
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
616

617
   int actuatorCal = 0;
618 619 620 621

    switch (calType) {
        case StartBusConfigActuators:
            actuatorCal = 1;
622 623 624 625
        break;
        case EndBusConfigActuators:
            actuatorCal = 0;
        break;
626 627
    }

628 629 630 631
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_UAVCAN,          // command id
                             true,                              // showError
                             actuatorCal);                      // actuators
632 633 634 635
}

void UAS::stopBusConfig(void)
{
636 637 638
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
639

640 641 642 643
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_UAVCAN,          // command id
                             true,                              // showError
                             0);                                // cancel
644 645
}

646 647
/**
* Check if time is smaller than 40 years, assuming no system without Unix
648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698
* timestamp runs longer than 40 years continuously without reboot. In worst case
* this will add/subtract the communication delay between GCS and MAV, it will
* never alter the timestamp in a safety critical way.
*/
quint64 UAS::getUnixReferenceTime(quint64 time)
{
    // Same as getUnixTime, but does not react to attitudeStamped mode
    if (time == 0)
    {
        //        qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
        return QGC::groundTimeMilliseconds();
    }
    // Check if time is smaller than 40 years,
    // assuming no system without Unix timestamp
    // runs longer than 40 years continuously without
    // reboot. In worst case this will add/subtract the
    // communication delay between GCS and MAV,
    // it will never alter the timestamp in a safety
    // critical way.
    //
    // Calculation:
    // 40 years
    // 365 days
    // 24 hours
    // 60 minutes
    // 60 seconds
    // 1000 milliseconds
    // 1000 microseconds
#ifndef _MSC_VER
    else if (time < 1261440000000000LLU)
#else
    else if (time < 1261440000000000)
#endif
    {
        //        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
        if (onboardTimeOffset == 0)
        {
            onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
        }
        return time/1000 + onboardTimeOffset;
    }
    else
    {
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
        return time/1000;
    }
}

/**
* @warning If attitudeStamped is enabled, this function will not actually return
699
* the precise time stamp of this measurement augmented to UNIX time, but will
700
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
701
* reason why one would want this, except for system setups where the onboard
702
* clock is not present or broken and datasets should be collected that are still
703
* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE
704 705 706 707 708 709 710 711 712
* SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
*/
quint64 UAS::getUnixTimeFromMs(quint64 time)
{
    return getUnixTime(time*1000);
}

/**
* @warning If attitudeStamped is enabled, this function will not actually return
713 714 715 716
* the precise time stam of this measurement augmented to UNIX time, but will
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
* reason why one would want this, except for system setups where the onboard
* clock is not present or broken and datasets should be collected that are
717 718 719 720 721 722 723 724 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 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773
* still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED
* RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
*/
quint64 UAS::getUnixTime(quint64 time)
{
    quint64 ret = 0;
    if (attitudeStamped)
    {
        ret = lastAttitude;
    }

    if (time == 0)
    {
        ret = QGC::groundTimeMilliseconds();
    }
    // Check if time is smaller than 40 years,
    // assuming no system without Unix timestamp
    // runs longer than 40 years continuously without
    // reboot. In worst case this will add/subtract the
    // communication delay between GCS and MAV,
    // it will never alter the timestamp in a safety
    // critical way.
    //
    // Calculation:
    // 40 years
    // 365 days
    // 24 hours
    // 60 minutes
    // 60 seconds
    // 1000 milliseconds
    // 1000 microseconds
#ifndef _MSC_VER
    else if (time < 1261440000000000LLU)
#else
    else if (time < 1261440000000000)
#endif
    {
        //        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
        if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100))
        {
            lastNonNullTime = time;
            onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
        }
        if (time > lastNonNullTime) lastNonNullTime = time;

        ret = time/1000 + onboardTimeOffset;
    }
    else
    {
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
        ret = time/1000;
    }

    return ret;
}

774
/**
775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835
* Get the status of the code and a description of the status.
* Status can be unitialized, booting up, calibrating sensors, active
* standby, cirtical, emergency, shutdown or unknown.
*/
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
    switch (statusCode)
    {
    case MAV_STATE_UNINIT:
        uasState = tr("UNINIT");
        stateDescription = tr("Unitialized, booting up.");
        break;
    case MAV_STATE_BOOT:
        uasState = tr("BOOT");
        stateDescription = tr("Booting system, please wait.");
        break;
    case MAV_STATE_CALIBRATING:
        uasState = tr("CALIBRATING");
        stateDescription = tr("Calibrating sensors, please wait.");
        break;
    case MAV_STATE_ACTIVE:
        uasState = tr("ACTIVE");
        stateDescription = tr("Active, normal operation.");
        break;
    case MAV_STATE_STANDBY:
        uasState = tr("STANDBY");
        stateDescription = tr("Standby mode, ready for launch.");
        break;
    case MAV_STATE_CRITICAL:
        uasState = tr("CRITICAL");
        stateDescription = tr("FAILURE: Continuing operation.");
        break;
    case MAV_STATE_EMERGENCY:
        uasState = tr("EMERGENCY");
        stateDescription = tr("EMERGENCY: Land Immediately!");
        break;
        //case MAV_STATE_HILSIM:
        //uasState = tr("HIL SIM");
        //stateDescription = tr("HIL Simulation, Sensors read from SIM");
        //break;

    case MAV_STATE_POWEROFF:
        uasState = tr("SHUTDOWN");
        stateDescription = tr("Powering off system.");
        break;

    default:
        uasState = tr("UNKNOWN");
        stateDescription = tr("Unknown system state");
        break;
    }
}

QImage UAS::getImage()
{

//    qDebug() << "IMAGE TYPE:" << imageType;

    // RAW greyscale
    if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U)
    {
836
        int imgColors = 255;
837 838 839 840 841

        // Construct PGM header
        QString header("P5\n%1 %2\n%3\n");
        header = header.arg(imageWidth).arg(imageHeight).arg(imgColors);

Don Gagne's avatar
Don Gagne committed
842
        QByteArray tmpImage(header.toStdString().c_str(), header.length());
843 844 845 846 847 848 849 850 851 852 853 854
        tmpImage.append(imageRecBuffer);

        //qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header;

        if (imageRecBuffer.isNull())
        {
            qDebug()<< "could not convertToPGM()";
            return QImage();
        }

        if (!image.loadFromData(tmpImage, "PGM"))
        {
855
            qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
856 857 858 859 860 861 862 863 864 865 866 867
            return QImage();
        }

    }
    // BMP with header
    else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
             imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
             imageType == MAVLINK_DATA_STREAM_IMG_PGM ||
             imageType == MAVLINK_DATA_STREAM_IMG_PNG)
    {
        if (!image.loadFromData(imageRecBuffer))
        {
868
            qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!";
869
            return QImage();
870 871
        }
    }
872

873 874
    // Restart statemachine
    imagePacketsArrived = 0;
875 876
    imagePackets = 0;
    imageRecBuffer.clear();
877 878 879 880 881
    return image;
}

void UAS::requestImage()
{
882 883 884
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
885

886
   qDebug() << "trying to get an image from the uas...";
887 888 889 890 891

    // check if there is already an image transmission going on
    if (imagePacketsArrived == 0)
    {
        mavlink_message_t msg;
892 893 894 895 896 897 898
        mavlink_msg_data_transmission_handshake_pack_chan(mavlink->getSystemId(),
                                                          mavlink->getComponentId(),
                                                          _vehicle->priorityLink()->mavlinkChannel(),
                                                          &msg,
                                                          MAVLINK_DATA_STREAM_IMG_JPEG,
                                                          0, 0, 0, 0, 0, 50);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921
    }
}


/* MANAGEMENT */

/**
 *
 * @return The uptime in milliseconds
 *
 */
quint64 UAS::getUptime() const
{
    if(startTime == 0)
    {
        return 0;
    }
    else
    {
        return QGC::groundTimeMilliseconds() - startTime;
    }
}

922
//TODO update this to use the parameter manager / param data model instead
923
void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue,  mavlink_param_union_t& paramUnion)
924 925 926
{
    int compId = msg.compid;

927
    QVariant paramValue;
928 929

    // Insert with correct type
930

931 932
    switch (rawValue.param_type) {
        case MAV_PARAM_TYPE_REAL32:
Don Gagne's avatar
Don Gagne committed
933
            paramValue = QVariant(paramUnion.param_float);
934
            break;
935

936
        case MAV_PARAM_TYPE_UINT8:
Don Gagne's avatar
Don Gagne committed
937
            paramValue = QVariant(paramUnion.param_uint8);
938
            break;
939

940
        case MAV_PARAM_TYPE_INT8:
Don Gagne's avatar
Don Gagne committed
941
            paramValue = QVariant(paramUnion.param_int8);
942
            break;
943

944 945 946 947
        case MAV_PARAM_TYPE_UINT16:
            paramValue = QVariant(paramUnion.param_uint16);
            break;

948
        case MAV_PARAM_TYPE_INT16:
Don Gagne's avatar
Don Gagne committed
949
            paramValue = QVariant(paramUnion.param_int16);
950
            break;
951

952
        case MAV_PARAM_TYPE_UINT32:
Don Gagne's avatar
Don Gagne committed
953
            paramValue = QVariant(paramUnion.param_uint32);
954
            break;
dogmaphobic's avatar
dogmaphobic committed
955

956
        case MAV_PARAM_TYPE_INT32:
Don Gagne's avatar
Don Gagne committed
957
            paramValue = QVariant(paramUnion.param_int32);
958
            break;
959

960 961 962 963 964 965 966 967
        //-- Note: These are not handled above:
        //
        //   MAV_PARAM_TYPE_UINT64
        //   MAV_PARAM_TYPE_INT64
        //   MAV_PARAM_TYPE_REAL64
        //
        //   No space in message (the only storage allocation is a "float") and not present in mavlink_param_union_t

968 969
        default:
            qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
970
    }
971

972
    qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
973

974
    emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue);
975 976
}

977 978
/**
* Set the manual control commands.
979 980
* This can only be done if the system has manual inputs enabled and is armed.
*/
981
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
982
{
983 984 985
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
986

987 988 989 990
    if (!_vehicle->priorityLink()) {
        return;
    }

991
    // Store the previous manual commands
992 993 994 995 996 997
    static float manualRollAngle = 0.0;
    static float manualPitchAngle = 0.0;
    static float manualYawAngle = 0.0;
    static float manualThrust = 0.0;
    static quint16 manualButtons = 0;
    static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
998

999 1000 1001 1002 1003 1004 1005 1006 1007
    // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
    // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
    // if no command inputs have changed.

    // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
    bool sendCommand = false;
    if (countSinceLastTransmission++ >= 5) {
        sendCommand = true;
        countSinceLastTransmission = 0;
1008 1009
    } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
             (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) ||
1010 1011 1012 1013 1014 1015
             buttons != manualButtons) {
        sendCommand = true;

        // Ensure that another message will be sent the next time this function is called
        countSinceLastTransmission = 10;
    }
1016

1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027
    // Now if we should trigger an update, let's do that
    if (sendCommand) {
        // Save the new manual control inputs
        manualRollAngle = roll;
        manualPitchAngle = pitch;
        manualYawAngle = yaw;
        manualThrust = thrust;
        manualButtons = buttons;

        mavlink_message_t message;

1028
        if (joystickMode == Vehicle::JoystickModeAttitude) {
1029 1030 1031 1032
            // send an external attitude setpoint command (rate control disabled)
            float attitudeQuaternion[4];
            mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
            uint8_t typeMask = 0x7; // disable rate control
1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045
            mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(),
                                                      mavlink->getComponentId(),
                                                      _vehicle->priorityLink()->mavlinkChannel(),
                                                      &message,
                                                      QGC::groundTimeUsecs(),
                                                      this->uasId,
                                                      0,
                                                      typeMask,
                                                      attitudeQuaternion,
                                                      0,
                                                      0,
                                                      0,
                                                      thrust);
1046
        } else if (joystickMode == Vehicle::JoystickModePosition) {
1047 1048 1049 1050 1051 1052 1053 1054 1055
            // Send the the local position setpoint (local pos sp external message)
            static float px = 0;
            static float py = 0;
            static float pz = 0;
            //XXX: find decent scaling
            px -= pitch;
            py += roll;
            pz -= 2.0f*(thrust-0.5);
            uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075
            mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(),
                                                                mavlink->getComponentId(),
                                                                _vehicle->priorityLink()->mavlinkChannel(),
                                                                &message,
                                                                QGC::groundTimeUsecs(),
                                                                this->uasId,
                                                                0,
                                                                MAV_FRAME_LOCAL_NED,
                                                                typeMask,
                                                                px,
                                                                py,
                                                                pz,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                yaw,
                                                                0);
1076
        } else if (joystickMode == Vehicle::JoystickModeForce) {
1077 1078 1079 1080 1081 1082 1083
            // Send the the force setpoint (local pos sp external message)
            float dcm[3][3];
            mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
            const float fx = -dcm[0][2] * thrust;
            const float fy = -dcm[1][2] * thrust;
            const float fz = -dcm[2][2] * thrust;
            uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else)
1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103
            mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(),
                                                                mavlink->getComponentId(),
                                                                _vehicle->priorityLink()->mavlinkChannel(),
                                                                &message,
                                                                QGC::groundTimeUsecs(),
                                                                this->uasId,
                                                                0,
                                                                MAV_FRAME_LOCAL_NED,
                                                                typeMask,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                fx,
                                                                fy,
                                                                fz,
                                                                0,
                                                                0);
1104
        } else if (joystickMode == Vehicle::JoystickModeVelocity) {
1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115
            // Send the the local velocity setpoint (local pos sp external message)
            static float vx = 0;
            static float vy = 0;
            static float vz = 0;
            static float yawrate = 0;
            //XXX: find decent scaling
            vx -= pitch;
            vy += roll;
            vz -= 2.0f*(thrust-0.5);
            yawrate += yaw; //XXX: not sure what scale to apply here
            uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control
1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135
            mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(),
                                                                mavlink->getComponentId(),
                                                                _vehicle->priorityLink()->mavlinkChannel(),
                                                                &message,
                                                                QGC::groundTimeUsecs(),
                                                                this->uasId,
                                                                0,
                                                                MAV_FRAME_LOCAL_NED,
                                                                typeMask,
                                                                0,
                                                                0,
                                                                0,
                                                                vx,
                                                                vy,
                                                                vz,
                                                                0,
                                                                0,
                                                                0,
                                                                0,
                                                                yawrate);
1136
        } else if (joystickMode == Vehicle::JoystickModeRC) {
1137

1138 1139 1140 1141 1142 1143 1144 1145 1146
            // Save the new manual control inputs
            manualRollAngle = roll;
            manualPitchAngle = pitch;
            manualYawAngle = yaw;
            manualThrust = thrust;
            manualButtons = buttons;

            // Store scaling values for all 3 axes
            const float axesScaling = 1.0 * 1000.0;
dogmaphobic's avatar
dogmaphobic committed
1147

1148 1149 1150 1151 1152 1153 1154
            // Calculate the new commands for roll, pitch, yaw, and thrust
            const float newRollCommand = roll * axesScaling;
            // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward
            const float newPitchCommand = -pitch * axesScaling;
            const float newYawCommand = yaw * axesScaling;
            const float newThrustCommand = thrust * axesScaling;

1155
            //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand;
dogmaphobic's avatar
dogmaphobic committed
1156

1157
            // Send the MANUAL_COMMAND message
1158 1159 1160 1161 1162 1163
            mavlink_msg_manual_control_pack_chan(mavlink->getSystemId(),
                                                 mavlink->getComponentId(),
                                                 _vehicle->priorityLink()->mavlinkChannel(),
                                                 &message,
                                                 this->uasId,
                                                 newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
1164
        }
1165

1166
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
1167 1168 1169
    }
}

dogmaphobic's avatar
dogmaphobic committed
1170
#ifndef __mobile__
1171 1172
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{
1173 1174 1175
    if (!_vehicle) {
        return;
    }
1176
    const uint8_t base_mode = _vehicle->baseMode();
dogmaphobic's avatar
dogmaphobic committed
1177

1178
   // If system has manual inputs enabled and is armed
1179
    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
1180 1181
    {
        mavlink_message_t message;
1182 1183
        float q[4];
        mavlink_euler_to_quaternion(roll, pitch, yaw, q);
1184

Lorenz Meier's avatar
Lorenz Meier committed
1185 1186
        float yawrate = 0.0f;

1187
        // Do not control rates and throttle
1188
        quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates
1189
        mask |= (1 << 6); // ignore throttle
1190 1191 1192 1193 1194 1195 1196
        mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(),
                                                  mavlink->getComponentId(),
                                                  _vehicle->priorityLink()->mavlinkChannel(),
                                                  &message,
                                                  QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(),
                                                  mask, q, 0, 0, 0, 0);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
Lorenz Meier's avatar
Lorenz Meier committed
1197
        quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) |
1198
            (1 << 6) | (1 << 7) | (1 << 8);
1199 1200 1201 1202 1203
        mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), mavlink->getComponentId(),
                                                            _vehicle->priorityLink()->mavlinkChannel(),
                                                            &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(),
                                                            MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
1204
        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
1205 1206 1207 1208 1209
    }
    else
    {
        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
    }
1210
}
dogmaphobic's avatar
dogmaphobic committed
1211
#endif
1212

1213
/**
Jean Cyr's avatar
Jean Cyr committed
1214
* Order the robot to start receiver pairing
1215 1216 1217
*/
void UAS::pairRX(int rxType, int rxSubType)
{
1218 1219 1220 1221 1222 1223
    if (_vehicle) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                                 MAV_CMD_START_RX_PAIR,             // command id
                                 true,                              // showError
                                 rxType,
                                 rxSubType);
1224
    }
1225
}
dogmaphobic's avatar
dogmaphobic committed
1226

1227 1228 1229
/**
* If enabled, connect the flight gear link.
*/
dogmaphobic's avatar
dogmaphobic committed
1230
#ifndef __mobile__
1231
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration)
1232
{
1233
    Q_UNUSED(configuration);
1234

1235
    QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
1236
    if (!link) {
1237 1238 1239 1240 1241
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
1242
        simulation = new QGCFlightGearLink(_vehicle, options);
1243
    }
1244

1245
    float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1246 1247 1248
    xacc_var = noise_scaler * 0.2914f;
    yacc_var = noise_scaler * 0.2914f;
    zacc_var = noise_scaler * 0.9577f;
1249 1250 1251
    rollspeed_var = noise_scaler * 0.8126f;
    pitchspeed_var = noise_scaler * 0.6145f;
    yawspeed_var = noise_scaler * 0.5852f;
Lorenz Meier's avatar
Lorenz Meier committed
1252 1253 1254
    xmag_var = noise_scaler * 0.0786f;
    ymag_var = noise_scaler * 0.0566f;
    zmag_var = noise_scaler * 0.0333f;
1255 1256 1257 1258
    abs_pressure_var = noise_scaler * 0.5604f;
    diff_pressure_var = noise_scaler * 0.2604f;
    pressure_alt_var = noise_scaler * 0.5604f;
    temperature_var = noise_scaler * 0.7290f;
1259

1260 1261 1262
    // Connect Flight Gear Link
    link = dynamic_cast<QGCFlightGearLink*>(simulation);
    link->setStartupArguments(options);
Thomas Gubler's avatar
Thomas Gubler committed
1263
    link->sensorHilEnabled(sensorHil);
1264 1265
    // FIXME: this signal is not on the base hil configuration widget, only on the FG widget
    //QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float)));
1266 1267 1268 1269 1270 1271 1272 1273 1274
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1275
#endif
1276 1277 1278 1279

/**
* If enabled, connect the JSBSim link.
*/
dogmaphobic's avatar
dogmaphobic committed
1280
#ifndef __mobile__
1281 1282 1283
void UAS::enableHilJSBSim(bool enable, QString options)
{
    QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
1284
    if (!link) {
1285 1286 1287 1288 1289
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
1290
        simulation = new QGCJSBSimLink(_vehicle, options);
1291 1292 1293 1294
    }
    // Connect Flight Gear Link
    link = dynamic_cast<QGCJSBSimLink*>(simulation);
    link->setStartupArguments(options);
1295 1296 1297 1298 1299 1300 1301 1302 1303
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1304
#endif
1305 1306 1307 1308

/**
* If enabled, connect the X-plane gear link.
*/
dogmaphobic's avatar
dogmaphobic committed
1309
#ifndef __mobile__
1310 1311 1312
void UAS::enableHilXPlane(bool enable)
{
    QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
1313
    if (!link) {
1314 1315 1316 1317
        if (simulation) {
            stopHil();
            delete simulation;
        }
1318
        simulation = new QGCXPlaneLink(_vehicle);
1319

1320
        float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1321 1322 1323
        xacc_var = noise_scaler * 0.2914f;
        yacc_var = noise_scaler * 0.2914f;
        zacc_var = noise_scaler * 0.9577f;
1324 1325 1326
        rollspeed_var = noise_scaler * 0.8126f;
        pitchspeed_var = noise_scaler * 0.6145f;
        yawspeed_var = noise_scaler * 0.5852f;
Lorenz Meier's avatar
Lorenz Meier committed
1327 1328 1329
        xmag_var = noise_scaler * 0.0786f;
        ymag_var = noise_scaler * 0.0566f;
        zmag_var = noise_scaler * 0.0333f;
1330 1331 1332 1333
        abs_pressure_var = noise_scaler * 0.5604f;
        diff_pressure_var = noise_scaler * 0.2604f;
        pressure_alt_var = noise_scaler * 0.5604f;
        temperature_var = noise_scaler * 0.7290f;
1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344
    }
    // Connect X-Plane Link
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1345
#endif
1346

1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364
/**
* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
dogmaphobic's avatar
dogmaphobic committed
1365
#ifndef __mobile__
1366 1367 1368 1369
void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
                       float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{
1370 1371 1372 1373
    Q_UNUSED(time_us);
    Q_UNUSED(xacc);
    Q_UNUSED(yacc);
    Q_UNUSED(zacc);
1374

1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394
        // Emit attitude for cross-check
        emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime());
        emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime());
        emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime());

        emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime());
        emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
        emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());

        emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime());
        emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime());
        emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime());

        emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime());
        emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime());
        emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime());

        emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime());
        emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime());
}
dogmaphobic's avatar
dogmaphobic committed
1395
#endif
1396

1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414
/**
* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
dogmaphobic's avatar
dogmaphobic committed
1415
#ifndef __mobile__
1416
void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
Lorenz Meier's avatar
Lorenz Meier committed
1417
                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
1418
                       float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
1419
{
1420 1421 1422
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1423

1424
    if (_vehicle->hilMode())
1425
    {
1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443
        float q[4];

        double cosPhi_2 = cos(double(roll) / 2.0);
        double sinPhi_2 = sin(double(roll) / 2.0);
        double cosTheta_2 = cos(double(pitch) / 2.0);
        double sinTheta_2 = sin(double(pitch) / 2.0);
        double cosPsi_2 = cos(double(yaw) / 2.0);
        double sinPsi_2 = sin(double(yaw) / 2.0);
        q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
                sinPhi_2 * sinTheta_2 * sinPsi_2);
        q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
                cosPhi_2 * sinTheta_2 * sinPsi_2);
        q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
                sinPhi_2 * cosTheta_2 * sinPsi_2);
        q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
                sinPhi_2 * sinTheta_2 * cosPsi_2);

        mavlink_message_t msg;
1444 1445 1446 1447 1448 1449 1450
        mavlink_msg_hil_state_quaternion_pack_chan(mavlink->getSystemId(),
                                                   mavlink->getComponentId(),
                                                   _vehicle->priorityLink()->mavlinkChannel(),
                                                   &msg,
                                                   time_us, q, rollspeed, pitchspeed, yawspeed,
                                                   lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
1451 1452 1453 1454
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1455
        _vehicle->setHilMode(true);
1456 1457 1458
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1459
#endif
1460

1461 1462 1463
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
{
dogmaphobic's avatar
dogmaphobic committed
1464
    /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var.  Calculated according to
1465 1466 1467 1468
    Box-Muller transform */
    static const float epsilon = std::numeric_limits<float>::min(); //used to ensure non-zero uniform numbers
    static float z0; //calculated normal distribution random variables with mu = 0, var = 1;
    float u1, u2;        //random variables generated from c++ rand();
dogmaphobic's avatar
dogmaphobic committed
1469

1470 1471 1472 1473 1474 1475 1476 1477 1478 1479
    /*Generate random variables in range (0 1] */
    do
    {
        //TODO seed rand() with srand(time) but srand(time should be called once on startup)
        //currently this will generate repeatable random noise
        u1 = rand() * (1.0 / RAND_MAX);
        u2 = rand() * (1.0 / RAND_MAX);
    }
    while ( u1 <= epsilon );  //Have a catch to ensure non-zero for log()

1480
    z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1
dogmaphobic's avatar
dogmaphobic committed
1481

1482 1483
    //TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these
    //as well
1484
    float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2
dogmaphobic's avatar
dogmaphobic committed
1485

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
1486
    //Finally guard against any case where the noise is not real
1487
    if(std::isfinite(noise)) {
1488
            return truth_meas + noise;
1489
    } else {
1490 1491 1492 1493 1494
        return truth_meas;
    }
}
#endif

1495 1496 1497 1498
/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure  (hPa)
*/
dogmaphobic's avatar
dogmaphobic committed
1499
#ifndef __mobile__
Lorenz Meier's avatar
Lorenz Meier committed
1500
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
1501
                                    float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
Lorenz Meier's avatar
Lorenz Meier committed
1502
{
1503 1504 1505
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1506

1507
    if (_vehicle->hilMode())
Lorenz Meier's avatar
Lorenz Meier committed
1508
    {
1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521
        float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
        float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
        float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
        float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
        float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
        float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
        float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
        float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
        float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var);
        float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var);
        float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
        float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var);
        float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var);
1522

Lorenz Meier's avatar
Lorenz Meier committed
1523
        mavlink_message_t msg;
1524 1525 1526 1527 1528 1529 1530 1531
        mavlink_msg_hil_sensor_pack_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         _vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt,
                                         yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt,
                                         diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
1532
        lastSendTimeSensors = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
1533 1534 1535 1536
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1537
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1538 1539 1540
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1541
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1542

dogmaphobic's avatar
dogmaphobic committed
1543
#ifndef __mobile__
1544 1545 1546
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
                    float flow_comp_m_y, quint8 quality, float ground_distance)
{
1547 1548 1549
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1550

Don Gagne's avatar
Don Gagne committed
1551
    // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api
1552

Don Gagne's avatar
Don Gagne committed
1553 1554 1555 1556 1557 1558 1559
    Q_UNUSED(time_us);
    Q_UNUSED(flow_x);
    Q_UNUSED(flow_y);
    Q_UNUSED(flow_comp_m_x);
    Q_UNUSED(flow_comp_m_y);
    Q_UNUSED(quality);
    Q_UNUSED(ground_distance);
1560

1561
    if (_vehicle->hilMode())
1562
    {
Don Gagne's avatar
Don Gagne committed
1563
#if 0
1564
        mavlink_message_t msg;
1565 1566 1567 1568 1569
        mavlink_msg_hil_optical_flow_pack_chan(mavlink->getSystemId(),
                                               mavlink->getComponentId(),
                                               _vehicle->priorityLink()->mavlinkChannel(),
                                               &msg,
                                               time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance);
1570

1571
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
1572
        lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
Don Gagne's avatar
Don Gagne committed
1573
#endif
1574 1575 1576 1577
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1578
        _vehicle->setHilMode(true);
1579 1580 1581 1582
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }

}
dogmaphobic's avatar
dogmaphobic committed
1583
#endif
1584

dogmaphobic's avatar
dogmaphobic committed
1585
#ifndef __mobile__
1586
void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites)
Lorenz Meier's avatar
Lorenz Meier committed
1587
{
1588 1589 1590
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1591

1592 1593 1594 1595
    // Only send at 10 Hz max rate
    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
        return;

1596
    if (_vehicle->hilMode())
Lorenz Meier's avatar
Lorenz Meier committed
1597
    {
Lorenz Meier's avatar
Lorenz Meier committed
1598 1599 1600
        float course = cog;
        // map to 0..2pi
        if (course < 0)
1601
            course += 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
1602 1603 1604
        // scale from radians to degrees
        course = (course / M_PI) * 180.0f;

Lorenz Meier's avatar
Lorenz Meier committed
1605
        mavlink_message_t msg;
1606 1607 1608 1609 1610
        mavlink_msg_hil_gps_pack_chan(mavlink->getSystemId(),
                                      mavlink->getComponentId(),
                                      _vehicle->priorityLink()->mavlinkChannel(),
                                      &msg,
                                      time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites);
1611
        lastSendTimeGPS = QGC::groundTimeMilliseconds();
1612
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
Lorenz Meier's avatar
Lorenz Meier committed
1613 1614 1615 1616
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1617
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1618 1619 1620
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1621
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1622

1623 1624 1625
/**
* Connect flight gear link.
**/
dogmaphobic's avatar
dogmaphobic committed
1626
#ifndef __mobile__
1627 1628 1629 1630
void UAS::startHil()
{
    if (hilEnabled) return;
    hilEnabled = true;
1631
    sensorHil = false;
Don Gagne's avatar
Don Gagne committed
1632
    _vehicle->setHilMode(true);
1633
    qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
1634 1635
    // Connect HIL simulation link
    simulation->connectSimulation();
1636
}
dogmaphobic's avatar
dogmaphobic committed
1637
#endif
1638 1639 1640 1641

/**
* disable flight gear link.
*/
dogmaphobic's avatar
dogmaphobic committed
1642
#ifndef __mobile__
1643 1644
void UAS::stopHil()
{
1645 1646 1647 1648 1649
   if (simulation && simulation->isConnected()) {
       simulation->disconnectSimulation();
       _vehicle->setHilMode(false);
       qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
   }
1650
    hilEnabled = false;
1651
    sensorHil = false;
1652
}
dogmaphobic's avatar
dogmaphobic committed
1653
#endif
1654 1655 1656 1657 1658 1659 1660 1661 1662

/**
* @rerturn the map of the components
*/
QMap<int, QString> UAS::getComponents()
{
    return components;
}

1663
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
1664
{
1665 1666 1667
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1668

1669 1670
    mavlink_message_t message;

1671 1672 1673 1674 1675 1676 1677 1678 1679 1680
    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
    // Copy string into buffer, ensuring not to exceed the buffer size
    for (unsigned int i = 0; i < sizeof(param_id_cstr); i++)
    {
        if ((int)i < param_id.length())
        {
            param_id_cstr[i] = param_id.toLatin1()[i];
        }
    }

1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694
    mavlink_msg_param_map_rc_pack_chan(mavlink->getSystemId(),
                                       mavlink->getComponentId(),
                                       _vehicle->priorityLink()->mavlinkChannel(),
                                       &message,
                                       this->uasId,
                                       _vehicle->defaultComponentId(),
                                       param_id_cstr,
                                       -1,
                                       param_rc_channel_index,
                                       value0,
                                       scale,
                                       valueMin,
                                       valueMax);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
dogmaphobic's avatar
dogmaphobic committed
1695
    //qDebug() << "Mavlink message sent";
1696
}
1697

1698 1699
void UAS::unsetRCToParameterMap()
{
1700 1701 1702
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1703

1704 1705
    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};

1706 1707
    for (int i = 0; i < 3; i++) {
        mavlink_message_t message;
1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721
        mavlink_msg_param_map_rc_pack_chan(mavlink->getSystemId(),
                                           mavlink->getComponentId(),
                                           _vehicle->priorityLink()->mavlinkChannel(),
                                           &message,
                                           this->uasId,
                                           _vehicle->defaultComponentId(),
                                           param_id_cstr,
                                           -2,
                                           i,
                                           0.0f,
                                           0.0f,
                                           0.0f,
                                           0.0f);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
Don Gagne's avatar
Don Gagne committed
1722 1723
    }
}
1724 1725 1726

void UAS::_say(const QString& text, int severity)
{
1727 1728
    Q_UNUSED(severity);
    qgcApp()->toolbox()->audioOutput()->say(text);
1729
}
Don Gagne's avatar
Don Gagne committed
1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743

void UAS::shutdownVehicle(void)
{
#ifndef __mobile__
    stopHil();
    if (simulation) {
        // wait for the simulator to exit
        simulation->wait();
        simulation->disconnectSimulation();
        simulation->deleteLater();
    }
#endif
    _vehicle = NULL;
}