UAS.cc 59.3 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
#include "UAS.h"
#include "LinkInterface.h"
#include "QGC.h"
29
#include "AudioOutput.h"
30 31 32
#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

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

Don Gagne's avatar
Don Gagne committed
78 79 80
    blockHomePositionChanges(false),
    receivedMode(false),

81 82
    // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
    // TODO: calibrate stand-still pixhawk variances
83
    xacc_var(0.6457f),
dogmaphobic's avatar
dogmaphobic committed
84
    yacc_var(0.7048f),
85
    zacc_var(0.97885f),
dogmaphobic's avatar
dogmaphobic committed
86 87 88
    rollspeed_var(0.8126f),
    pitchspeed_var(0.6145f),
    yawspeed_var(0.5852f),
89 90 91 92 93 94 95
    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),
96
    /*
97 98 99 100 101 102 103 104 105 106 107 108 109
    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),
110
    */
111

dogmaphobic's avatar
dogmaphobic committed
112
#ifndef __mobile__
113
    simulation(0),
dogmaphobic's avatar
dogmaphobic committed
114
#endif
115 116

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

130
#ifndef __mobile__
131
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage);
132
#endif
133

134 135 136 137 138 139 140 141 142 143
}

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

144
void UAS::receiveMessage(mavlink_message_t message)
145 146 147 148 149 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
{
    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
182
    if (message.sysid == uasId && (!attitudeStamped || lastAttitude != 0 || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198
    {
        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
199
        if (!componentID.contains(message.msgid))
200 201 202
        {
            // Prefer the first component
            componentID[message.msgid] = message.compid;
Gus Grubba's avatar
Gus Grubba committed
203
            componentMulti[message.msgid] = false;
204 205 206 207 208 209 210 211 212 213 214
        }
        else
        {
            // Got this message already
            if (componentID[message.msgid] != message.compid)
            {
                componentMulti[message.msgid] = true;
                wrongComponent = true;
            }
        }

Gus Grubba's avatar
Gus Grubba committed
215 216 217
        if (componentMulti[message.msgid] == true) {
            multiComponentSourceDetected = true;
        }
218 219 220 221 222 223 224 225 226 227 228 229


        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);
230 231 232

            // 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
233
            quint64 time = getUnixTime();
234 235 236 237 238
            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);

239 240
            // We got the mode
            receivedMode = true;
241 242 243
        }

            break;
244

245 246 247 248 249 250 251 252 253
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_sys_status_t state;
            mavlink_msg_sys_status_decode(&message, &state);

254
            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
255
            quint64 time = getUnixTime();
256 257 258 259 260 261 262
            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);
263 264
            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);

265 266 267 268
            // 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);
        }
269 270 271 272 273 274 275 276
            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;
277

278 279
        case MAVLINK_MSG_ID_PARAM_VALUE:
        {
280 281 282
            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);
283 284 285
            // 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);
286 287 288
            mavlink_param_union_t paramVal;
            paramVal.param_float = rawValue.param_value;
            paramVal.type = rawValue.param_type;
289

290 291
            processParamValueMsg(message, parameterName,rawValue,paramVal);
         }
292
            break;
293
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
294
        {
295 296 297 298
            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);
299
            quint64 time = getUnixTimeFromMs(out.time_boot_ms);
300 301

            // For plotting emit roll sp, pitch sp and yaw sp values
302 303 304
            emit valueChanged(uasId, "roll sp", "rad", roll, time);
            emit valueChanged(uasId, "pitch sp", "rad", pitch, time);
            emit valueChanged(uasId, "yaw sp", "rad", yaw, time);
305 306
        }
            break;
dogmaphobic's avatar
dogmaphobic committed
307

308 309 310 311 312 313 314 315 316 317 318 319
        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();
320 321
            imagePacketsArrived = 0;

322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337
        }
            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
338
                break;
339 340 341 342 343 344 345 346 347 348 349 350 351
            }

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

            ++imagePacketsArrived;

            // emit signal if all packets arrived
352
            if (imagePacketsArrived >= imagePackets)
353 354
            {
                // Restart statemachine
Don Gagne's avatar
Don Gagne committed
355 356
                imagePackets = 0;
                imagePacketsArrived = 0;
357 358 359 360 361
                emit imageReady(this);
            }
        }
            break;

dogmaphobic's avatar
dogmaphobic committed
362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377
        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;

378 379 380 381 382 383
        default:
            break;
        }
    }
}

384
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
385
{
386 387 388
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
389

390 391 392 393 394
    int gyroCal = 0;
    int magCal = 0;
    int airspeedCal = 0;
    int radioCal = 0;
    int accelCal = 0;
395
    int pressureCal = 0;
396
    int escCal = 0;
dogmaphobic's avatar
dogmaphobic committed
397

398
    switch (calType) {
399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419
    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;
420 421 422
    case StartCalibrationPressure:
        pressureCal = 1;
        break;
423 424 425 426 427 428 429 430 431
    case StartCalibrationEsc:
        escCal = 1;
        break;
    case StartCalibrationUavcanEsc:
        escCal = 2;
        break;
    case StartCalibrationCompassMot:
        airspeedCal = 1; // ArduPilot, bit of a hack
        break;
432
    }
dogmaphobic's avatar
dogmaphobic committed
433

434 435
    // 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.
436
    mavlink_message_t msg;
437 438 439 440 441 442 443 444 445 446
    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
447
                                       pressureCal,                      // ground pressure
448 449 450 451 452
                                       radioCal,                         // radio cal
                                       accelCal,                         // accel cal
                                       airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot
                                       escCal);                          // esc cal
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
453 454
}

455
void UAS::stopCalibration(void)
456
{
457 458 459
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
460

461 462 463 464 465 466 467 468 469 470
    _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
471 472
}

473 474
void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{
475 476 477
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
478

479
   int actuatorCal = 0;
480 481 482 483

    switch (calType) {
        case StartBusConfigActuators:
            actuatorCal = 1;
484 485 486 487
        break;
        case EndBusConfigActuators:
            actuatorCal = 0;
        break;
488 489
    }

490 491 492 493
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_UAVCAN,          // command id
                             true,                              // showError
                             actuatorCal);                      // actuators
494 495 496 497
}

void UAS::stopBusConfig(void)
{
498 499 500
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
501

502 503 504 505
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                             MAV_CMD_PREFLIGHT_UAVCAN,          // command id
                             true,                              // showError
                             0);                                // cancel
506 507
}

508 509
/**
* Check if time is smaller than 40 years, assuming no system without Unix
510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560
* 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
561
* the precise time stamp of this measurement augmented to UNIX time, but will
562
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
563
* reason why one would want this, except for system setups where the onboard
564
* clock is not present or broken and datasets should be collected that are still
565
* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE
566 567 568 569 570 571 572 573 574
* 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
575 576 577 578
* 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
579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635
* 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;
}

636
/**
637 638 639 640 641 642 643 644 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 696 697
* 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)
    {
698
        int imgColors = 255;
699 700 701 702 703

        // 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
704
        QByteArray tmpImage(header.toStdString().c_str(), header.length());
705 706 707 708 709 710 711 712 713 714 715 716
        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"))
        {
717
            qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
718 719 720 721 722 723 724 725 726 727 728 729
            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))
        {
730
            qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!";
731
            return QImage();
732 733
        }
    }
734

735 736
    // Restart statemachine
    imagePacketsArrived = 0;
737 738
    imagePackets = 0;
    imageRecBuffer.clear();
739 740 741 742 743
    return image;
}

void UAS::requestImage()
{
744 745 746
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
747

748
   qDebug() << "trying to get an image from the uas...";
749 750 751 752 753

    // check if there is already an image transmission going on
    if (imagePacketsArrived == 0)
    {
        mavlink_message_t msg;
754 755 756 757 758 759 760
        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);
761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783
    }
}


/* MANAGEMENT */

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

784
//TODO update this to use the parameter manager / param data model instead
785
void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue,  mavlink_param_union_t& paramUnion)
786 787 788
{
    int compId = msg.compid;

789
    QVariant paramValue;
790 791

    // Insert with correct type
792

793 794
    switch (rawValue.param_type) {
        case MAV_PARAM_TYPE_REAL32:
Don Gagne's avatar
Don Gagne committed
795
            paramValue = QVariant(paramUnion.param_float);
796
            break;
797

798
        case MAV_PARAM_TYPE_UINT8:
Don Gagne's avatar
Don Gagne committed
799
            paramValue = QVariant(paramUnion.param_uint8);
800
            break;
801

802
        case MAV_PARAM_TYPE_INT8:
Don Gagne's avatar
Don Gagne committed
803
            paramValue = QVariant(paramUnion.param_int8);
804
            break;
805

806 807 808 809
        case MAV_PARAM_TYPE_UINT16:
            paramValue = QVariant(paramUnion.param_uint16);
            break;

810
        case MAV_PARAM_TYPE_INT16:
Don Gagne's avatar
Don Gagne committed
811
            paramValue = QVariant(paramUnion.param_int16);
812
            break;
813

814
        case MAV_PARAM_TYPE_UINT32:
Don Gagne's avatar
Don Gagne committed
815
            paramValue = QVariant(paramUnion.param_uint32);
816
            break;
dogmaphobic's avatar
dogmaphobic committed
817

818
        case MAV_PARAM_TYPE_INT32:
Don Gagne's avatar
Don Gagne committed
819
            paramValue = QVariant(paramUnion.param_int32);
820
            break;
821

822 823 824 825 826 827 828 829
        //-- 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

830 831
        default:
            qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
832
    }
833

834
    qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
835

836
    emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue);
837 838
}

839 840
/**
* Set the manual control commands.
841 842
* This can only be done if the system has manual inputs enabled and is armed.
*/
843
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
844
{
845 846 847
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
848

849 850 851 852
    if (!_vehicle->priorityLink()) {
        return;
    }

853
    // Store the previous manual commands
854 855 856 857 858 859
    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
860

861 862 863 864 865 866 867 868 869
    // 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;
870 871
    } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
             (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) ||
872 873 874 875 876 877
             buttons != manualButtons) {
        sendCommand = true;

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

879 880 881 882 883 884 885 886 887 888 889
    // 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;

890
        if (joystickMode == Vehicle::JoystickModeAttitude) {
891 892 893 894
            // 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
895 896 897 898 899 900 901 902 903 904 905 906 907
            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);
908
        } else if (joystickMode == Vehicle::JoystickModePosition) {
909 910 911 912 913 914 915 916 917
            // 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
918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937
            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);
938
        } else if (joystickMode == Vehicle::JoystickModeForce) {
939 940 941 942 943 944 945
            // 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)
946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965
            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);
966
        } else if (joystickMode == Vehicle::JoystickModeVelocity) {
967 968 969 970 971 972 973 974 975 976 977
            // 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
978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997
            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);
998
        } else if (joystickMode == Vehicle::JoystickModeRC) {
999

1000 1001 1002 1003 1004 1005 1006 1007 1008
            // 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
1009

1010 1011 1012 1013 1014 1015 1016
            // 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;

1017
            //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand;
dogmaphobic's avatar
dogmaphobic committed
1018

1019
            // Send the MANUAL_COMMAND message
1020 1021 1022 1023 1024 1025
            mavlink_msg_manual_control_pack_chan(mavlink->getSystemId(),
                                                 mavlink->getComponentId(),
                                                 _vehicle->priorityLink()->mavlinkChannel(),
                                                 &message,
                                                 this->uasId,
                                                 newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
1026
        }
1027

1028
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
1029 1030 1031
    }
}

dogmaphobic's avatar
dogmaphobic committed
1032
#ifndef __mobile__
1033 1034
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{
1035 1036 1037
    if (!_vehicle) {
        return;
    }
1038
    const uint8_t base_mode = _vehicle->baseMode();
dogmaphobic's avatar
dogmaphobic committed
1039

1040
   // If system has manual inputs enabled and is armed
1041
    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
1042 1043
    {
        mavlink_message_t message;
1044 1045
        float q[4];
        mavlink_euler_to_quaternion(roll, pitch, yaw, q);
1046

Lorenz Meier's avatar
Lorenz Meier committed
1047 1048
        float yawrate = 0.0f;

1049
        // Do not control rates and throttle
1050
        quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates
1051
        mask |= (1 << 6); // ignore throttle
1052 1053 1054 1055 1056 1057 1058
        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
1059
        quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) |
1060
            (1 << 6) | (1 << 7) | (1 << 8);
1061 1062 1063 1064 1065
        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);
1066
        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
1067 1068 1069 1070 1071
    }
    else
    {
        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
    }
1072
}
dogmaphobic's avatar
dogmaphobic committed
1073
#endif
1074

1075
/**
Jean Cyr's avatar
Jean Cyr committed
1076
* Order the robot to start receiver pairing
1077 1078 1079
*/
void UAS::pairRX(int rxType, int rxSubType)
{
1080 1081 1082 1083 1084 1085
    if (_vehicle) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                                 MAV_CMD_START_RX_PAIR,             // command id
                                 true,                              // showError
                                 rxType,
                                 rxSubType);
1086
    }
1087
}
dogmaphobic's avatar
dogmaphobic committed
1088

1089 1090 1091
/**
* If enabled, connect the flight gear link.
*/
dogmaphobic's avatar
dogmaphobic committed
1092
#ifndef __mobile__
1093
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration)
1094
{
1095
    Q_UNUSED(configuration);
1096

1097
    QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
1098
    if (!link) {
1099 1100 1101 1102 1103
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
1104
        simulation = new QGCFlightGearLink(_vehicle, options);
1105
    }
1106

1107
    float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1108 1109 1110
    xacc_var = noise_scaler * 0.2914f;
    yacc_var = noise_scaler * 0.2914f;
    zacc_var = noise_scaler * 0.9577f;
1111 1112 1113
    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
1114 1115 1116
    xmag_var = noise_scaler * 0.0786f;
    ymag_var = noise_scaler * 0.0566f;
    zmag_var = noise_scaler * 0.0333f;
1117 1118 1119 1120
    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;
1121

1122 1123 1124
    // Connect Flight Gear Link
    link = dynamic_cast<QGCFlightGearLink*>(simulation);
    link->setStartupArguments(options);
Thomas Gubler's avatar
Thomas Gubler committed
1125
    link->sensorHilEnabled(sensorHil);
1126 1127
    // 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)));
1128 1129 1130 1131 1132 1133 1134 1135 1136
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1137
#endif
1138 1139 1140 1141

/**
* If enabled, connect the JSBSim link.
*/
dogmaphobic's avatar
dogmaphobic committed
1142
#ifndef __mobile__
1143 1144 1145
void UAS::enableHilJSBSim(bool enable, QString options)
{
    QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
1146
    if (!link) {
1147 1148 1149 1150 1151
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
1152
        simulation = new QGCJSBSimLink(_vehicle, options);
1153 1154 1155 1156
    }
    // Connect Flight Gear Link
    link = dynamic_cast<QGCJSBSimLink*>(simulation);
    link->setStartupArguments(options);
1157 1158 1159 1160 1161 1162 1163 1164 1165
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1166
#endif
1167 1168 1169 1170

/**
* If enabled, connect the X-plane gear link.
*/
dogmaphobic's avatar
dogmaphobic committed
1171
#ifndef __mobile__
1172 1173 1174
void UAS::enableHilXPlane(bool enable)
{
    QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
1175
    if (!link) {
1176 1177 1178 1179
        if (simulation) {
            stopHil();
            delete simulation;
        }
1180
        simulation = new QGCXPlaneLink(_vehicle);
1181

1182
        float noise_scaler = 0.0001f;
Lorenz Meier's avatar
Lorenz Meier committed
1183 1184 1185
        xacc_var = noise_scaler * 0.2914f;
        yacc_var = noise_scaler * 0.2914f;
        zacc_var = noise_scaler * 0.9577f;
1186 1187 1188
        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
1189 1190 1191
        xmag_var = noise_scaler * 0.0786f;
        ymag_var = noise_scaler * 0.0566f;
        zmag_var = noise_scaler * 0.0333f;
1192 1193 1194 1195
        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;
1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206
    }
    // Connect X-Plane Link
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}
dogmaphobic's avatar
dogmaphobic committed
1207
#endif
1208

1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226
/**
* @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
1227
#ifndef __mobile__
1228 1229 1230 1231
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)
{
1232 1233 1234 1235
    Q_UNUSED(time_us);
    Q_UNUSED(xacc);
    Q_UNUSED(yacc);
    Q_UNUSED(zacc);
1236

1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256
        // 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
1257
#endif
1258

1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276
/**
* @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
1277
#ifndef __mobile__
1278
void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
Lorenz Meier's avatar
Lorenz Meier committed
1279
                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
1280
                       float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
1281
{
1282 1283 1284
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1285

1286
    if (_vehicle->hilMode())
1287
    {
1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305
        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;
1306 1307 1308 1309 1310 1311 1312
        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);
1313 1314 1315 1316
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1317
        _vehicle->setHilMode(true);
1318 1319 1320
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1321
#endif
1322

1323 1324 1325
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
{
dogmaphobic's avatar
dogmaphobic committed
1326
    /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var.  Calculated according to
1327 1328 1329 1330
    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
1331

1332 1333 1334 1335 1336 1337 1338 1339 1340 1341
    /*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()

1342
    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
1343

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

Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
1348
    //Finally guard against any case where the noise is not real
1349
    if(std::isfinite(noise)) {
1350
            return truth_meas + noise;
1351
    } else {
1352 1353 1354 1355 1356
        return truth_meas;
    }
}
#endif

1357 1358 1359 1360
/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure  (hPa)
*/
dogmaphobic's avatar
dogmaphobic committed
1361
#ifndef __mobile__
Lorenz Meier's avatar
Lorenz Meier committed
1362
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
1363
                                    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
1364
{
1365 1366 1367
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1368

1369
    if (_vehicle->hilMode())
Lorenz Meier's avatar
Lorenz Meier committed
1370
    {
1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383
        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);
1384

Lorenz Meier's avatar
Lorenz Meier committed
1385
        mavlink_message_t msg;
1386 1387 1388 1389 1390 1391 1392 1393
        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);
1394
        lastSendTimeSensors = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
1395 1396 1397 1398
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1399
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1400 1401 1402
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1403
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1404

dogmaphobic's avatar
dogmaphobic committed
1405
#ifndef __mobile__
1406 1407 1408
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)
{
1409 1410 1411
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1412

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

Don Gagne's avatar
Don Gagne committed
1415 1416 1417 1418 1419 1420 1421
    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);
1422

1423
    if (_vehicle->hilMode())
1424
    {
Don Gagne's avatar
Don Gagne committed
1425
#if 0
1426
        mavlink_message_t msg;
1427 1428 1429 1430 1431
        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);
1432

1433
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
1434
        lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
Don Gagne's avatar
Don Gagne committed
1435
#endif
1436 1437 1438 1439
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1440
        _vehicle->setHilMode(true);
1441 1442 1443 1444
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }

}
dogmaphobic's avatar
dogmaphobic committed
1445
#endif
1446

dogmaphobic's avatar
dogmaphobic committed
1447
#ifndef __mobile__
1448
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
1449
{
1450 1451 1452
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1453

1454 1455 1456 1457
    // Only send at 10 Hz max rate
    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
        return;

1458
    if (_vehicle->hilMode())
Lorenz Meier's avatar
Lorenz Meier committed
1459
    {
Lorenz Meier's avatar
Lorenz Meier committed
1460 1461 1462
        float course = cog;
        // map to 0..2pi
        if (course < 0)
1463
            course += 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
1464 1465 1466
        // scale from radians to degrees
        course = (course / M_PI) * 180.0f;

Lorenz Meier's avatar
Lorenz Meier committed
1467
        mavlink_message_t msg;
1468 1469 1470 1471 1472
        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);
1473
        lastSendTimeGPS = QGC::groundTimeMilliseconds();
1474
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
Lorenz Meier's avatar
Lorenz Meier committed
1475 1476 1477 1478
    }
    else
    {
        // Attempt to set HIL mode
Don Gagne's avatar
Don Gagne committed
1479
        _vehicle->setHilMode(true);
Lorenz Meier's avatar
Lorenz Meier committed
1480 1481 1482
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}
dogmaphobic's avatar
dogmaphobic committed
1483
#endif
Lorenz Meier's avatar
Lorenz Meier committed
1484

1485 1486 1487
/**
* Connect flight gear link.
**/
dogmaphobic's avatar
dogmaphobic committed
1488
#ifndef __mobile__
1489 1490 1491 1492
void UAS::startHil()
{
    if (hilEnabled) return;
    hilEnabled = true;
1493
    sensorHil = false;
Don Gagne's avatar
Don Gagne committed
1494
    _vehicle->setHilMode(true);
1495
    qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";