UAS.cc 64.4 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
#ifndef __mobile__
135
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage);
Gus Grubba's avatar
Gus Grubba committed
136
    color = UASInterface::getNextColor();
137
#endif
138

139 140 141 142 143 144 145 146 147 148
}

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

149
void UAS::receiveMessage(mavlink_message_t message)
150 151 152 153 154 155 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
{
    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
Gus Grubba's avatar
Gus Grubba committed
207
        if (!componentID.contains(message.msgid))
208 209 210
        {
            // Prefer the first component
            componentID[message.msgid] = message.compid;
Gus Grubba's avatar
Gus Grubba committed
211
            componentMulti[message.msgid] = false;
212 213 214 215 216 217 218 219 220 221 222
        }
        else
        {
            // Got this message already
            if (componentID[message.msgid] != message.compid)
            {
                componentMulti[message.msgid] = true;
                wrongComponent = true;
            }
        }

Gus Grubba's avatar
Gus Grubba committed
223 224 225
        if (componentMulti[message.msgid] == true) {
            multiComponentSourceDetected = true;
        }
226 227 228 229 230 231 232 233 234 235 236 237


        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);
238 239 240

            // 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
241
            quint64 time = getUnixTime();
242 243 244 245 246
            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);

247 248
            // We got the mode
            receivedMode = true;
249 250 251
        }

            break;
252

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

262
            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
263
            quint64 time = getUnixTime();
264 265 266 267 268 269 270
            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);
271 272
            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);

273 274 275 276
            // 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);
        }
277 278 279 280 281 282 283 284 285 286 287 288
            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;
289 290 291
                setRoll(QGC::limitAngleToPMPIf(attitude.roll));
                setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
                setYaw(QGC::limitAngleToPMPIf(attitude.yaw));
292

293 294 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
                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));
352 353

                attitudeKnown = true;
354
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372
            }
        }
            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)
            {
373
                setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
374
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
375 376 377 378 379 380 381 382 383 384 385
            }
        }
            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;
386

387 388
        case MAVLINK_MSG_ID_PARAM_VALUE:
        {
389 390 391
            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);
392 393 394
            // 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);
395 396 397
            mavlink_param_union_t paramVal;
            paramVal.param_float = rawValue.param_value;
            paramVal.type = rawValue.param_type;
398

399 400
            processParamValueMsg(message, parameterName,rawValue,paramVal);
         }
401
            break;
402
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
403
        {
404 405 406 407
            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);
408
            quint64 time = getUnixTimeFromMs(out.time_boot_ms);
409 410

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

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

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

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

443 444 445 446 447 448 449 450 451 452 453 454
        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();
455 456
            imagePacketsArrived = 0;

457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
        }
            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
473
                break;
474 475 476 477 478 479 480 481 482 483 484 485 486
            }

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

            ++imagePacketsArrived;

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

dogmaphobic's avatar
dogmaphobic committed
497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512
        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;

513 514 515 516 517 518
        default:
            break;
        }
    }
}

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

525 526 527 528 529
    int gyroCal = 0;
    int magCal = 0;
    int airspeedCal = 0;
    int radioCal = 0;
    int accelCal = 0;
530
    int pressureCal = 0;
531
    int escCal = 0;
dogmaphobic's avatar
dogmaphobic committed
532

533
    switch (calType) {
534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554
    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;
555 556 557
    case StartCalibrationPressure:
        pressureCal = 1;
        break;
558 559 560 561 562 563 564 565 566
    case StartCalibrationEsc:
        escCal = 1;
        break;
    case StartCalibrationUavcanEsc:
        escCal = 2;
        break;
    case StartCalibrationCompassMot:
        airspeedCal = 1; // ArduPilot, bit of a hack
        break;
567
    }
dogmaphobic's avatar
dogmaphobic committed
568

569 570
    // 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.
571
    mavlink_message_t msg;
572 573 574 575 576 577 578 579 580 581
    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
582
                                       pressureCal,                      // ground pressure
583 584 585 586 587
                                       radioCal,                         // radio cal
                                       accelCal,                         // accel cal
                                       airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot
                                       escCal);                          // esc cal
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
588 589
}

590
void UAS::stopCalibration(void)
591
{
592 593 594
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
595

596 597 598 599 600 601 602 603 604 605
    _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
606 607
}

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

614
   int actuatorCal = 0;
615 616 617 618

    switch (calType) {
        case StartBusConfigActuators:
            actuatorCal = 1;
619 620 621 622
        break;
        case EndBusConfigActuators:
            actuatorCal = 0;
        break;
623 624
    }

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

void UAS::stopBusConfig(void)
{
633 634 635
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
636

637 638 639 640
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_UAVCAN,          // command id
                             true,                              // showError
                             0);                                // cancel
641 642
}

643 644
/**
* Check if time is smaller than 40 years, assuming no system without Unix
645 646 647 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
* 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
696
* the precise time stamp of this measurement augmented to UNIX time, but will
697
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
698
* reason why one would want this, except for system setups where the onboard
699
* clock is not present or broken and datasets should be collected that are still
700
* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE
701 702 703 704 705 706 707 708 709
* 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
710 711 712 713
* 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
714 715 716 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
* 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;
}

771
/**
772 773 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
* 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)
    {
833
        int imgColors = 255;
834 835 836 837 838

        // 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
839
        QByteArray tmpImage(header.toStdString().c_str(), header.length());
840 841 842 843 844 845 846 847 848 849 850 851
        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"))
        {
852
            qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
853 854 855 856 857 858 859 860 861 862 863 864
            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))
        {
865
            qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!";
866
            return QImage();
867 868
        }
    }
869

870 871
    // Restart statemachine
    imagePacketsArrived = 0;
872 873
    imagePackets = 0;
    imageRecBuffer.clear();
874 875 876 877 878
    return image;
}

void UAS::requestImage()
{
879 880 881
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
882

883
   qDebug() << "trying to get an image from the uas...";
884 885 886 887 888

    // check if there is already an image transmission going on
    if (imagePacketsArrived == 0)
    {
        mavlink_message_t msg;
889 890 891 892 893 894 895
        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);
896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918
    }
}


/* MANAGEMENT */

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

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

924
    QVariant paramValue;
925 926

    // Insert with correct type
927

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

933
        case MAV_PARAM_TYPE_UINT8:
Don Gagne's avatar
Don Gagne committed
934
            paramValue = QVariant(paramUnion.param_uint8);
935
            break;
936

937
        case MAV_PARAM_TYPE_INT8:
Don Gagne's avatar
Don Gagne committed
938
            paramValue = QVariant(paramUnion.param_int8);
939
            break;
940

941 942 943 944
        case MAV_PARAM_TYPE_UINT16:
            paramValue = QVariant(paramUnion.param_uint16);
            break;

945
        case MAV_PARAM_TYPE_INT16:
Don Gagne's avatar
Don Gagne committed
946
            paramValue = QVariant(paramUnion.param_int16);
947
            break;
948

949
        case MAV_PARAM_TYPE_UINT32:
Don Gagne's avatar
Don Gagne committed
950
            paramValue = QVariant(paramUnion.param_uint32);
951
            break;
dogmaphobic's avatar
dogmaphobic committed
952

953
        case MAV_PARAM_TYPE_INT32:
Don Gagne's avatar
Don Gagne committed
954
            paramValue = QVariant(paramUnion.param_int32);
955
            break;
956

957 958 959 960 961 962 963 964
        //-- 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

965 966
        default:
            qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
967
    }
968

969
    qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
970

971
    emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue);
972 973
}

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

984 985 986 987
    if (!_vehicle->priorityLink()) {
        return;
    }

988
    // Store the previous manual commands
989 990 991 992 993 994
    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
995

996 997 998 999 1000 1001 1002 1003 1004
    // 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;
1005 1006
    } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
             (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) ||
1007 1008 1009 1010 1011 1012
             buttons != manualButtons) {
        sendCommand = true;

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

1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024
    // 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;

1025
        if (joystickMode == Vehicle::JoystickModeAttitude) {
1026 1027 1028 1029
            // 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
1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042
            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);
1043
        } else if (joystickMode == Vehicle::JoystickModePosition) {
1044 1045 1046 1047 1048 1049 1050 1051 1052
            // 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
1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072
            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);
1073
        } else if (joystickMode == Vehicle::JoystickModeForce) {
1074 1075 1076 1077 1078 1079 1080
            // 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)
1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100
            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);
1101
        } else if (joystickMode == Vehicle::JoystickModeVelocity) {
1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112
            // 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
1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132
            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);
1133
        } else if (joystickMode == Vehicle::JoystickModeRC) {
1134

1135 1136 1137 1138 1139 1140 1141 1142 1143
            // 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
1144

1145 1146 1147 1148 1149 1150 1151
            // 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;

1152
            //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand;
dogmaphobic's avatar
dogmaphobic committed
1153

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

1163
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
1164 1165 1166
    }
}

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

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

Lorenz Meier's avatar
Lorenz Meier committed
1182 1183
        float yawrate = 0.0f;

1184
        // Do not control rates and throttle
1185
        quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates
1186
        mask |= (1 << 6); // ignore throttle
1187 1188 1189 1190 1191 1192 1193
        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
1194
        quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) |
1195
            (1 << 6) | (1 << 7) | (1 << 8);
1196 1197 1198 1199 1200
        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);
1201
        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
1202 1203 1204 1205 1206
    }
    else
    {
        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
    }
1207
}
dogmaphobic's avatar
dogmaphobic committed
1208
#endif
1209

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

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

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

1242
    float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1243 1244 1245
    xacc_var = noise_scaler * 0.2914f;
    yacc_var = noise_scaler * 0.2914f;
    zacc_var = noise_scaler * 0.9577f;
1246 1247 1248
    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
1249 1250 1251
    xmag_var = noise_scaler * 0.0786f;
    ymag_var = noise_scaler * 0.0566f;
    zmag_var = noise_scaler * 0.0333f;
1252 1253 1254 1255
    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;
1256

1257 1258 1259
    // Connect Flight Gear Link
    link = dynamic_cast<QGCFlightGearLink*>(simulation);
    link->setStartupArguments(options);
Thomas Gubler's avatar
Thomas Gubler committed
1260
    link->sensorHilEnabled(sensorHil);
1261 1262
    // 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)));
1263 1264 1265 1266 1267 1268 1269 1270 1271
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1272
#endif
1273 1274 1275 1276

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

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

1317
        float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1318 1319 1320
        xacc_var = noise_scaler * 0.2914f;
        yacc_var = noise_scaler * 0.2914f;
        zacc_var = noise_scaler * 0.9577f;
1321 1322 1323
        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
1324 1325 1326
        xmag_var = noise_scaler * 0.0786f;
        ymag_var = noise_scaler * 0.0566f;
        zmag_var = noise_scaler * 0.0333f;
1327 1328 1329 1330
        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;
1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341
    }
    // Connect X-Plane Link
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1342
#endif
1343

1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361
/**
* @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
1362
#ifndef __mobile__
1363 1364 1365 1366
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)
{
1367 1368 1369 1370
    Q_UNUSED(time_us);
    Q_UNUSED(xacc);
    Q_UNUSED(yacc);
    Q_UNUSED(zacc);
1371

1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391
        // 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
1392
#endif
1393

1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411
/**
* @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
1412
#ifndef __mobile__
1413
void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
Lorenz Meier's avatar
Lorenz Meier committed
1414
                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
1415
                       float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
1416
{
1417 1418 1419
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1420

1421
    if (_vehicle->hilMode())
1422
    {
1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440
        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;
1441 1442 1443 1444 1445 1446 1447
        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);
1448 1449 1450 1451
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1452
        _vehicle->setHilMode(true);
1453 1454 1455
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1456
#endif
1457

1458 1459 1460
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
{
dogmaphobic's avatar
dogmaphobic committed
1461
    /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var.  Calculated according to
1462 1463 1464 1465
    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
1466

1467 1468 1469 1470 1471 1472 1473 1474 1475 1476
    /*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()

1477
    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
1478

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

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

1492 1493 1494 1495
/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure  (hPa)
*/
dogmaphobic's avatar
dogmaphobic committed
1496
#ifndef __mobile__
Lorenz Meier's avatar
Lorenz Meier committed
1497
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
1498
                                    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
1499
{
1500 1501 1502
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1503

1504
    if (_vehicle->hilMode())
Lorenz Meier's avatar
Lorenz Meier committed
1505
    {
1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518
        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);
1519

Lorenz Meier's avatar
Lorenz Meier committed
1520
        mavlink_message_t msg;
1521 1522 1523 1524 1525 1526 1527 1528
        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);
1529
        lastSendTimeSensors = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
1530 1531 1532 1533
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1534
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1535 1536 1537
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1538
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1539

dogmaphobic's avatar
dogmaphobic committed
1540
#ifndef __mobile__
1541 1542 1543
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)
{
1544 1545 1546
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1547

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

Don Gagne's avatar
Don Gagne committed
1550 1551 1552 1553 1554 1555 1556
    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);
1557

1558
    if (_vehicle->hilMode())
1559
    {
Don Gagne's avatar
Don Gagne committed
1560
#if 0
1561
        mavlink_message_t msg;
1562 1563 1564 1565 1566
        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);
1567

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

}
dogmaphobic's avatar
dogmaphobic committed
1580
#endif
1581

dogmaphobic's avatar
dogmaphobic committed
1582
#ifndef __mobile__
1583
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
1584
{
1585 1586 1587
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1588

1589 1590 1591 1592
    // Only send at 10 Hz max rate
    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
        return;

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

Lorenz Meier's avatar
Lorenz Meier committed
1602
        mavlink_message_t msg;
1603 1604 1605 1606 1607
        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);
1608
        lastSendTimeGPS = QGC::groundTimeMilliseconds();
1609
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
Lorenz Meier's avatar
Lorenz Meier committed
1610 1611 1612 1613
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1614
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1615 1616 1617
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1618
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1619

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

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

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

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

1666 1667
    mavlink_message_t message;

1668 1669 1670 1671 1672 1673 1674 1675 1676 1677
    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];
        }
    }

1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691
    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
1692
    //qDebug() << "Mavlink message sent";
1693
}
1694

1695 1696
void UAS::unsetRCToParameterMap()
{
1697 1698 1699
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1700

1701 1702
    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};

1703 1704
    for (int i = 0; i < 3; i++) {
        mavlink_message_t message;
1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718
        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
1719 1720
    }
}
1721 1722 1723

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

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