UAS.cc 127 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
/*===================================================================
======================================================================*/

/**
 * @file
 *   @brief Represents one unmanned aerial vehicle
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

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

18 19
#include <cmath>
#include <qmath.h>
Don Gagne's avatar
Don Gagne committed
20

21 22 23 24 25 26 27 28 29
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
#include "QGC.h"
#include "GAudioOutput.h"
#include "MAVLinkProtocol.h"
#include "QGCMAVLink.h"
#include "LinkManager.h"
#include "SerialLink.h"
tstellanova's avatar
tstellanova committed
30
#include "UASParameterCommsMgr.h"
31
#include <Eigen/Geometry>
32
#include "AutoPilotPluginManager.h"
Don Gagne's avatar
Don Gagne committed
33
#include "QGCMessageBox.h"
34

35 36
Q_LOGGING_CATEGORY(UASLog, "UASLog")

37 38
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
39
* by calling readSettings. This means the new UAS will have the same settings
40
* as the previous one created unless one calls deleteSettings in the code after
41
* creating the UAS.
42
*/
43

44
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
45 46
    lipoFull(4.2f),
    lipoEmpty(3.5f),
47 48 49
    uasId(id),
    unknownPackets(),
    mavlink(protocol),
50 51 52 53 54 55 56 57 58
    commStatus(COMM_DISCONNECTED),
    receiveDropRate(0),
    sendDropRate(0),

    name(""),
    type(MAV_TYPE_GENERIC),
    airframe(QGC_AIRFRAME_GENERIC),
    autopilot(-1),
    systemIsArmed(false),
59 60
    base_mode(0),
    custom_mode(0),
61 62 63 64 65 66 67 68 69
    // custom_mode not initialized
    status(-1),
    // shortModeText not initialized
    // shortStateText not initialized

    // actuatorValues not initialized
    // actuatorNames not initialized
    // motorValues not initialized
    // motorNames mnot initialized
70 71
    thrustSum(0),
    thrustMax(10),
72 73 74 75 76

    // batteryType not initialized
    // cells not initialized
    // fullVoltage not initialized
    // emptyVoltage not initialized
77 78 79 80 81 82 83 84
    startVoltage(-1.0f),
    tickVoltage(10.5f),
    lastTickVoltageValue(13.0f),
    tickLowpassVoltage(12.0f),
    warnVoltage(9.5f),
    warnLevelPercent(20.0f),
    currentVoltage(12.6f),
    lpVoltage(12.0f),
dongfang's avatar
dongfang committed
85
    currentCurrent(0.4f),
86
    batteryRemainingEstimateEnabled(false),
87 88
    chargeLevel(-1),
    timeRemaining(0),
89 90 91
    lowBattAlarm(false),

    startTime(QGC::groundTimeMilliseconds()),
92
    onboardTimeOffset(0),
93

94 95 96 97 98 99 100 101
    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),
102

103
    positionLock(false),
104 105 106
    isLocalPositionKnown(false),
    isGlobalPositionKnown(false),

107 108 109
    localX(0.0),
    localY(0.0),
    localZ(0.0),
110 111 112 113

    latitude(0.0),
    longitude(0.0),
    altitudeAMSL(0.0),
114 115
    altitudeAMSLFT(0.0),
    altitudeWGS84(0.0),
116 117
    altitudeRelative(0.0),

Don Gagne's avatar
Don Gagne committed
118 119 120 121 122
    globalEstimatorActive(false),

    latitude_gps(0.0),
    longitude_gps(0.0),
    altitude_gps(0.0),
123 124 125 126 127

    speedX(0.0),
    speedY(0.0),
    speedZ(0.0),

128 129 130
    nedPosGlobalOffset(0,0,0),
    nedAttGlobalOffset(0,0,0),

Don Gagne's avatar
Don Gagne committed
131 132
    airSpeed(std::numeric_limits<double>::quiet_NaN()),
    groundSpeed(std::numeric_limits<double>::quiet_NaN()),
Lorenz Meier's avatar
Lorenz Meier committed
133
    waypointManager(this),
134
    fileManager(this, this),
Don Gagne's avatar
Don Gagne committed
135

136 137 138
    attitudeKnown(false),
    attitudeStamped(false),
    lastAttitude(0),
139

140 141 142 143
    roll(0.0),
    pitch(0.0),
    yaw(0.0),

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

Don Gagne's avatar
Don Gagne committed
146 147 148 149
    blockHomePositionChanges(false),
    receivedMode(false),


150
    paramsOnceRequested(false),
Lorenz Meier's avatar
Lorenz Meier committed
151
    paramMgr(this),
152
    simulation(0),
153 154

    // The protected members.
155 156 157 158
    connectionLost(false),
    lastVoltageWarning(0),
    lastNonNullTime(0),
    onboardTimeOffsetInvalidCount(0),
159
    hilEnabled(false),
160 161
    sensorHil(false),
    lastSendTimeGPS(0),
162 163
    lastSendTimeSensors(0),
    lastSendTimeOpticalFlow(0)
164
{
165

166 167 168 169 170
    for (unsigned int i = 0; i<255;++i)
    {
        componentID[i] = -1;
        componentMulti[i] = false;
    }
171

172 173
    connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));

174
    // Store a list of available actions for this UAS.
175
    // Basically everything exposed as a SLOT with no return value or arguments.
176

177
    QAction* newAction = new QAction(tr("Arm"), this);
178 179 180 181
    newAction->setToolTip(tr("Enable the UAS so that all actuators are online"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem()));
    actions.append(newAction);

182
    newAction = new QAction(tr("Disarm"), this);
183 184 185 186
    newAction->setToolTip(tr("Disable the UAS so that all actuators are offline"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem()));
    actions.append(newAction);

187
    newAction = new QAction(tr("Toggle armed"), this);
188 189 190 191
    newAction->setToolTip(tr("Toggle between armed and disarmed"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
    actions.append(newAction);

192
    newAction = new QAction(tr("Go home"), this);
193 194 195 196
    newAction->setToolTip(tr("Command the UAS to return to its home position"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(home()));
    actions.append(newAction);

197
    newAction = new QAction(tr("Land"), this);
198 199 200 201
    newAction->setToolTip(tr("Command the UAS to land"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(land()));
    actions.append(newAction);

202
    newAction = new QAction(tr("Launch"), this);
203 204 205 206
    newAction->setToolTip(tr("Command the UAS to launch itself and begin its mission"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(launch()));
    actions.append(newAction);

207
    newAction = new QAction(tr("Resume"), this);
208 209 210 211
    newAction->setToolTip(tr("Command the UAS to continue its mission"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(go()));
    actions.append(newAction);

212
    newAction = new QAction(tr("Stop"), this);
213 214 215 216
    newAction->setToolTip(tr("Command the UAS to halt and hold position"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(halt()));
    actions.append(newAction);

217
    newAction = new QAction(tr("Go autonomous"), this);
218 219 220 221
    newAction->setToolTip(tr("Set the UAS into an autonomous control mode"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(goAutonomous()));
    actions.append(newAction);

222
    newAction = new QAction(tr("Go manual"), this);
223 224 225 226
    newAction->setToolTip(tr("Set the UAS into a manual control mode"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(goManual()));
    actions.append(newAction);

227
    newAction = new QAction(tr("Toggle autonomy"), this);
228 229 230 231
    newAction->setToolTip(tr("Toggle between manual and full-autonomy"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
    actions.append(newAction);

232
    color = UASInterface::getNextColor();
233
    setBatterySpecs(QString(""));
234
    connect(&statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
235
    connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings()));
236
    statusTimeout.start(500);
237
    readSettings();
238 239
    //need to init paramMgr after readSettings have been loaded, to properly set autopilot and so forth
    paramMgr.initWithUAS(this);
240 241
    // Initial signals
    emit disarmed();
242
    emit armingChanged(false);
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
}

/**
* Saves the settings of name, airframe, autopilot type and battery specifications
* by calling writeSettings.
*/
UAS::~UAS()
{
    writeSettings();
}

/**
* Saves the settings of name, airframe, autopilot type and battery specifications
* for the next instantiation of UAS.
*/
void UAS::writeSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    settings.setValue("NAME", this->name);
    settings.setValue("AIRFRAME", this->airframe);
    settings.setValue("AP_TYPE", this->autopilot);
    settings.setValue("BATTERY_SPECS", getBatterySpecs());
    settings.endGroup();
}

/**
* Reads in the settings: name, airframe, autopilot type, and battery specifications
* for the new UAS.
*/
void UAS::readSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    this->name = settings.value("NAME", this->name).toString();
    this->airframe = settings.value("AIRFRAME", this->airframe).toInt();
    this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt();
    if (settings.contains("BATTERY_SPECS"))
    {
        setBatterySpecs(settings.value("BATTERY_SPECS").toString());
    }
    settings.endGroup();
}

/**
*  Deletes the settings origianally read into the UAS by readSettings.
289
*  This is in case one does not want the old values but would rather
290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
*  start with the values assigned by the constructor.
*/
void UAS::deleteSettings()
{
    this->name = "";
    this->airframe = QGC_AIRFRAME_GENERIC;
    this->autopilot = -1;
    setBatterySpecs(QString("9V,9.5V,12.6V"));
}

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

308 309
void UAS::triggerAction(int action)
{
310
    if (action >= 0 && action < actions.size())
311 312 313 314 315 316
    {
        qDebug() << "Triggering action: '" << actions[action]->text() << "'";
        actions[action]->trigger();
    }
}

317 318 319 320 321 322 323 324 325 326
/**
* Update the heartbeat.
*/
void UAS::updateState()
{
    // Check if heartbeat timed out
    quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat;
    if (!connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
    {
        connectionLost = true;
327
        receivedMode = false;
328
        QString audiostring = QString("Link lost to system %1").arg(this->getUASID());
329
        GAudioOutput::instance()->say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_ALERT);
330 331 332 333 334 335 336 337 338 339 340 341
    }

    // Update connection loss time on each iteration
    if (connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
    {
        connectionLossTime = heartbeatInterval;
        emit heartbeatTimeout(true, heartbeatInterval/1000);
    }

    // Connection gained
    if (connectionLost && (heartbeatInterval < timeoutIntervalHeartbeat))
    {
342
        QString audiostring = QString("Link regained to system %1").arg(this->getUASID());
343
        GAudioOutput::instance()->say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_NOTICE);
344 345 346 347 348 349 350 351 352 353 354 355 356
        connectionLost = false;
        connectionLossTime = 0;
        emit heartbeatTimeout(false, 0);
    }

    // Position lock is set by the MAVLink message handler
    // if no position lock is available, indicate an error
    if (positionLock)
    {
        positionLock = false;
    }
    else
    {
357
        if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386
        {
            GAudioOutput::instance()->notifyNegative();
        }
    }
}

/**
* If the acitve UAS (the UAS that was selected) is not the one that is currently
* active, then change the active UAS to the one that was selected.
*/
void UAS::setSelected()
{
    if (UASManager::instance()->getActiveUAS() != this)
    {
        UASManager::instance()->setActiveUAS(this);
        emit systemSelected(true);
    }
}

/**
* @return if the active UAS is the current UAS
**/
bool UAS::getSelected() const
{
    return (UASManager::instance()->getActiveUAS() == this);
}

void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
387
    if (!links.contains(link))
388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479
    {
        addLink(link);
        //        qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
    }

    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);
        emit componentCreated(uasId, message.compid, componentName);
    }

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

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

        bool multiComponentSourceDetected = false;
        bool wrongComponent = false;

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

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

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


        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            lastHeartbeat = QGC::groundTimeUsecs();
            emit heartbeat(this);
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);
480 481 482

            // 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
483
            quint64 time = getUnixTime();
484 485 486 487 488
            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);

489 490 491 492
            // Set new type if it has changed
            if (this->type != state.type)
            {
                this->autopilot = state.autopilot;
493
                setSystemType(state.type);
494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 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
            }

            bool currentlyArmed = state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;

            if (systemIsArmed != currentlyArmed)
            {
                systemIsArmed = currentlyArmed;
                emit armingChanged(systemIsArmed);
                if (systemIsArmed)
                {
                    emit armed();
                }
                else
                {
                    emit disarmed();
                }
            }

            QString audiostring = QString("System %1").arg(uasId);
            QString stateAudio = "";
            QString modeAudio = "";
            QString navModeAudio = "";
            bool statechanged = false;
            bool modechanged = false;

            QString audiomodeText = getAudioModeTextFor(static_cast<int>(state.base_mode));

            if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT)
            {
                statechanged = true;
                this->status = state.system_status;
                getStatusForCode((int)state.system_status, uasState, stateDescription);
                emit statusChanged(this, uasState, stateDescription);
                emit statusChanged(this->status);

                shortStateText = uasState;

                // Adjust for better audio
                if (uasState == QString("STANDBY")) uasState = QString("standing by");
                if (uasState == QString("EMERGENCY")) uasState = QString("emergency condition");
                if (uasState == QString("CRITICAL")) uasState = QString("critical condition");
                if (uasState == QString("SHUTDOWN")) uasState = QString("shutting down");

                stateAudio = uasState;
            }

540
            if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode)
541 542
            {
                modechanged = true;
543 544
                this->base_mode = state.base_mode;
                this->custom_mode = state.custom_mode;
545
                shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode);
546 547 548 549 550 551

                emit modeChanged(this->getUASID(), shortModeText, "");

                modeAudio = " is now in " + audiomodeText;
            }

552 553 554
            // We got the mode
            receivedMode = true;

555 556 557 558 559 560 561 562 563
            // AUDIO
            if (modechanged && statechanged)
            {
                // Output both messages
                audiostring += modeAudio + " and " + stateAudio;
            }
            else if (modechanged || statechanged)
            {
                // Output the one message
564
                audiostring += modeAudio + stateAudio;
565 566 567 568
            }

            if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY))
            {
569
                GAudioOutput::instance()->say(QString("emergency for system %1").arg(this->getUASID()), GAudioOutput::AUDIO_SEVERITY_EMERGENCY);
570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588
                QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency()));
            }
            else if (modechanged || statechanged)
            {
                GAudioOutput::instance()->stopEmergency();
                GAudioOutput::instance()->say(audiostring.toLower());
            }
        }

            break;
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_sys_status_t state;
            mavlink_msg_sys_status_decode(&message, &state);

589
            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
590
            quint64 time = getUnixTime();
591 592 593 594 595 596 597
            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);
598 599
            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);

600
            // Process CPU load.
601
            emit loadChanged(this,state.load/10.0f);
602
            emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
603

604
            // Battery charge/time remaining/voltage calculations
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
            currentVoltage = state.voltage_battery/1000.0f;
            lpVoltage = filterVoltage(currentVoltage);
            tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage;

            // We don't want to tick above the threshold
            if (tickLowpassVoltage > tickVoltage)
            {
                lastTickVoltageValue = tickLowpassVoltage;
            }

            if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
                    /* warn if lower than treshold */
                    && (lpVoltage < tickVoltage)
                    /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
                    && (currentVoltage > 3.3f)
                    /* warn only if current voltage is really still lower by a reasonable amount */
                    && ((currentVoltage - 0.2f) < tickVoltage)
                    /* warn only every 12 seconds */
                    && (QGC::groundTimeUsecs() - lastVoltageWarning) > 12000000)
            {
                GAudioOutput::instance()->say(QString("voltage warning: %1 volts").arg(lpVoltage, 0, 'f', 1, QChar(' ')));
                lastVoltageWarning = QGC::groundTimeUsecs();
                lastTickVoltageValue = tickLowpassVoltage;
            }

            if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage;
            timeRemaining = calculateTimeRemaining();
            if (!batteryRemainingEstimateEnabled && chargeLevel != -1)
            {
                chargeLevel = state.battery_remaining;
            }
dongfang's avatar
dongfang committed
636 637

            emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), timeRemaining);
638
            emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time);
dongfang's avatar
dongfang committed
639
            // emit voltageChanged(message.sysid, currentVoltage);
640
            emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time);
641

642 643 644
            // And if the battery current draw is measured, log that also.
            if (state.current_battery != -1)
            {
dongfang's avatar
dongfang committed
645 646
                currentCurrent = ((double)state.current_battery)/100.0f;
                emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time);
647
            }
648 649 650 651

            // LOW BATTERY ALARM
            if (lpVoltage < warnVoltage && (currentVoltage - 0.2f) < warnVoltage && (currentVoltage > 3.3))
            {
dongfang's avatar
dongfang committed
652
                // An audio alarm. Does not generate any signals.
653 654 655 656 657 658 659 660 661 662 663 664 665 666
                startLowBattAlarm();
            }
            else
            {
                stopLowBattAlarm();
            }

            // control_sensors_enabled:
            // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
            emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11));
            emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12));
            emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13));
            emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14));

667 668 669 670 671 672 673 674 675 676 677
            // Trigger drop rate updates as needed. Here we convert the incoming
            // drop_rate_comm value from 1/100 of a percent in a uint16 to a true
            // percentage as a float. We also cap the incoming value at 100% as defined
            // by the MAVLink specifications.
            if (state.drop_rate_comm > 10000)
            {
                state.drop_rate_comm = 10000;
            }
            emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
            emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
        }
678 679 680 681 682 683 684 685 686 687 688 689
            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;
690 691 692
                setRoll(QGC::limitAngleToPMPIf(attitude.roll));
                setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
                setYaw(QGC::limitAngleToPMPIf(attitude.yaw));
693

694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 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
                attitudeKnown = true;
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
                emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, 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));
754 755

                attitudeKnown = true;
756
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
757
                emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789
            }
        }
            break;
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET:
        {
            mavlink_local_position_ned_system_global_offset_t offset;
            mavlink_msg_local_position_ned_system_global_offset_decode(&message, &offset);
            nedPosGlobalOffset.setX(offset.x);
            nedPosGlobalOffset.setY(offset.y);
            nedPosGlobalOffset.setZ(offset.z);
            nedAttGlobalOffset.setX(offset.roll);
            nedAttGlobalOffset.setY(offset.pitch);
            nedAttGlobalOffset.setZ(offset.yaw);
        }
            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();
            // Display updated values
            emit thrustChanged(this, hud.throttle/100.0);

            if (!attitudeKnown)
            {
790
                setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
791
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
792 793
            }

794 795 796 797 798
            setAltitudeAMSL(hud.alt);
            setGroundSpeed(hud.groundspeed);
            if (!isnan(hud.airspeed))
                setAirSpeed(hud.airspeed);
            speedZ = -hud.climb;
799
            emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);
800
            emit speedChanged(this, groundSpeed, airSpeed, time);
801 802 803 804 805 806 807 808 809 810 811 812 813 814 815
        }
            break;
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
        {
            mavlink_local_position_ned_t pos;
            mavlink_msg_local_position_ned_decode(&message, &pos);
            quint64 time = getUnixTime(pos.time_boot_ms);

            // Emit position always with component ID
            emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);

            if (!wrongComponent)
            {
816 817 818 819 820 821 822
                setLocalX(pos.x);
                setLocalY(pos.y);
                setLocalZ(pos.z);

                speedX = pos.vx;
                speedY = pos.vy;
                speedZ = pos.vz;
823 824

                // Emit
825 826
                emit localPositionChanged(this, localX, localY, localZ, time);
                emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852

                // Set internal state
                if (!positionLock) {
                    // If position was not locked before, notify positive
                    GAudioOutput::instance()->notifyPositive();
                }
                positionLock = true;
                isLocalPositionKnown = true;
            }
        }
            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 localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
            emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
        }
            break;
        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
        {
            mavlink_global_position_int_t pos;
            mavlink_msg_global_position_int_decode(&message, &pos);
853

854
            quint64 time = getUnixTime();
855

856 857
            setLatitude(pos.lat/(double)1E7);
            setLongitude(pos.lon/(double)1E7);
858
            setAltitudeWGS84(pos.alt/1000.0);
859
            setAltitudeRelative(pos.relative_alt/1000.0);
860

861
            globalEstimatorActive = true;
862

863 864 865
            speedX = pos.vx/100.0;
            speedY = pos.vy/100.0;
            speedZ = pos.vz/100.0;
866

867 868
            emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), getAltitudeWGS84(), time);
            emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);
869
            // We had some frame mess here, global and local axes were mixed.
870
            emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
871

872 873
            setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
            emit speedChanged(this, groundSpeed, airSpeed, time);
874 875 876 877 878 879 880 881 882 883

            // Set internal state
            if (!positionLock)
            {
                // If position was not locked before, notify positive
                GAudioOutput::instance()->notifyPositive();
            }
            positionLock = true;
            isGlobalPositionKnown = true;
            //TODO fix this hack for forwarding of global position for patch antenna tracking
884
            //forwardMessage(message);
885 886 887 888 889 890 891 892
        }
            break;
        case MAVLINK_MSG_ID_GPS_RAW_INT:
        {
            mavlink_gps_raw_int_t pos;
            mavlink_msg_gps_raw_int_decode(&message, &pos);

            quint64 time = getUnixTime(pos.time_usec);
893

894 895 896 897 898
            emit gpsLocalizationChanged(this, pos.fix_type);
            // TODO: track localization state not only for gps but also for other loc. sources
            int loc_type = pos.fix_type;
            if (loc_type == 1)
            {
899
                loc_type = 0;
900 901
            }
            emit localizationChanged(this, loc_type);
902
            setSatelliteCount(pos.satellites_visible);
903 904 905

            if (pos.fix_type > 2)
            {
906 907
                positionLock = true;
                isGlobalPositionKnown = true;
908

909 910 911 912
                latitude_gps = pos.lat/(double)1E7;
                longitude_gps = pos.lon/(double)1E7;
                altitude_gps = pos.alt/1000.0;

913
                // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
914
                if (!globalEstimatorActive) {
915 916
                    setLatitude(latitude_gps);
                    setLongitude(longitude_gps);
917 918 919
                    setAltitudeWGS84(altitude_gps);
                    emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), getAltitudeWGS84(), time);
                    emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);
920

921 922 923
                    float vel = pos.vel/100.0f;
                    // Smaller than threshold and not NaN
                    if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) {
924
                        setGroundSpeed(vel);
925 926
                        emit speedChanged(this, groundSpeed, airSpeed, time);
                    } else {
927
                        emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
928
                    }
929 930 931 932 933 934 935 936 937 938 939 940
                }
            }
        }
            break;
        case MAVLINK_MSG_ID_GPS_STATUS:
        {
            mavlink_gps_status_t pos;
            mavlink_msg_gps_status_decode(&message, &pos);
            for(int i = 0; i < (int)pos.satellites_visible; i++)
            {
                emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
            }
941
            setSatelliteCount(pos.satellites_visible);
942 943 944 945 946 947 948 949 950
        }
            break;
        case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
        {
            mavlink_gps_global_origin_t pos;
            mavlink_msg_gps_global_origin_decode(&message, &pos);
            emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0);
        }
            break;
Lorenz Meier's avatar
Lorenz Meier committed
951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997
        case MAVLINK_MSG_ID_RC_CHANNELS:
        {
            mavlink_rc_channels_t channels;
            mavlink_msg_rc_channels_decode(&message, &channels);

            // UINT8_MAX indicates this value is unknown
            if (channels.rssi != UINT8_MAX) {
                emit remoteControlRSSIChanged(channels.rssi/100.0f);
            }

            if (channels.chan1_raw != UINT16_MAX && channels.chancount > 0)
                emit remoteControlChannelRawChanged(0, channels.chan1_raw);
            if (channels.chan2_raw != UINT16_MAX && channels.chancount > 1)
                emit remoteControlChannelRawChanged(1, channels.chan2_raw);
            if (channels.chan3_raw != UINT16_MAX && channels.chancount > 2)
                emit remoteControlChannelRawChanged(2, channels.chan3_raw);
            if (channels.chan4_raw != UINT16_MAX && channels.chancount > 3)
                emit remoteControlChannelRawChanged(3, channels.chan4_raw);
            if (channels.chan5_raw != UINT16_MAX && channels.chancount > 4)
                emit remoteControlChannelRawChanged(4, channels.chan5_raw);
            if (channels.chan6_raw != UINT16_MAX && channels.chancount > 5)
                emit remoteControlChannelRawChanged(5, channels.chan6_raw);
            if (channels.chan7_raw != UINT16_MAX && channels.chancount > 6)
                emit remoteControlChannelRawChanged(6, channels.chan7_raw);
            if (channels.chan8_raw != UINT16_MAX && channels.chancount > 7)
                emit remoteControlChannelRawChanged(7, channels.chan8_raw);
            if (channels.chan9_raw != UINT16_MAX && channels.chancount > 8)
                emit remoteControlChannelRawChanged(8, channels.chan9_raw);
            if (channels.chan10_raw != UINT16_MAX && channels.chancount > 9)
                emit remoteControlChannelRawChanged(9, channels.chan10_raw);
            if (channels.chan11_raw != UINT16_MAX && channels.chancount > 10)
                emit remoteControlChannelRawChanged(10, channels.chan11_raw);
            if (channels.chan12_raw != UINT16_MAX && channels.chancount > 11)
                emit remoteControlChannelRawChanged(11, channels.chan12_raw);
            if (channels.chan13_raw != UINT16_MAX && channels.chancount > 12)
                emit remoteControlChannelRawChanged(12, channels.chan13_raw);
            if (channels.chan14_raw != UINT16_MAX && channels.chancount > 13)
                emit remoteControlChannelRawChanged(13, channels.chan14_raw);
            if (channels.chan15_raw != UINT16_MAX && channels.chancount > 14)
                emit remoteControlChannelRawChanged(14, channels.chan15_raw);
            if (channels.chan16_raw != UINT16_MAX && channels.chancount > 15)
                emit remoteControlChannelRawChanged(15, channels.chan16_raw);
            if (channels.chan17_raw != UINT16_MAX && channels.chancount > 16)
                emit remoteControlChannelRawChanged(16, channels.chan17_raw);
            if (channels.chan18_raw != UINT16_MAX && channels.chancount > 17)
                emit remoteControlChannelRawChanged(17, channels.chan18_raw);

998 999 1000 1001 1002 1003
        }
            break;
        case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
        {
            mavlink_rc_channels_scaled_t channels;
            mavlink_msg_rc_channels_scaled_decode(&message, &channels);
1004 1005 1006

            const unsigned int portWidth = 8; // XXX magic number

1007
            emit remoteControlRSSIChanged(channels.rssi/255.0f);
1008
            if (static_cast<uint16_t>(channels.chan1_scaled) != UINT16_MAX)
1009
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 0, channels.chan1_scaled/10000.0f);
1010
            if (static_cast<uint16_t>(channels.chan2_scaled) != UINT16_MAX)
1011
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 1, channels.chan2_scaled/10000.0f);
1012
            if (static_cast<uint16_t>(channels.chan3_scaled) != UINT16_MAX)
1013
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 2, channels.chan3_scaled/10000.0f);
1014
            if (static_cast<uint16_t>(channels.chan4_scaled) != UINT16_MAX)
1015
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 3, channels.chan4_scaled/10000.0f);
1016
            if (static_cast<uint16_t>(channels.chan5_scaled) != UINT16_MAX)
1017
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 4, channels.chan5_scaled/10000.0f);
1018
            if (static_cast<uint16_t>(channels.chan6_scaled) != UINT16_MAX)
1019
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 5, channels.chan6_scaled/10000.0f);
1020
            if (static_cast<uint16_t>(channels.chan7_scaled) != UINT16_MAX)
1021
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 6, channels.chan7_scaled/10000.0f);
1022
            if (static_cast<uint16_t>(channels.chan8_scaled) != UINT16_MAX)
1023
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 7, channels.chan8_scaled/10000.0f);
1024 1025 1026 1027
        }
            break;
        case MAVLINK_MSG_ID_PARAM_VALUE:
        {
1028 1029 1030
            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);
1031 1032 1033
            // 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);
1034 1035 1036
            mavlink_param_union_t paramVal;
            paramVal.param_float = rawValue.param_value;
            paramVal.type = rawValue.param_type;
1037

1038
            processParamValueMsg(message, parameterName,rawValue,paramVal);
1039
            processParamValueMsgHook(message, parameterName,rawValue,paramVal);
1040

1041
         }
1042 1043 1044 1045 1046 1047 1048 1049 1050
            break;
        case MAVLINK_MSG_ID_COMMAND_ACK:
        {
            mavlink_command_ack_t ack;
            mavlink_msg_command_ack_decode(&message, &ack);
            switch (ack.result)
            {
            case MAV_RESULT_ACCEPTED:
            {
1051
                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_INFO, tr("SUCCESS: Executed CMD: %1").arg(ack.command));
1052 1053 1054 1055
            }
                break;
            case MAV_RESULT_TEMPORARILY_REJECTED:
            {
1056
                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Temporarily rejected CMD: %1").arg(ack.command));
1057 1058 1059 1060
            }
                break;
            case MAV_RESULT_DENIED:
            {
1061
                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Denied CMD: %1").arg(ack.command));
1062 1063 1064 1065
            }
                break;
            case MAV_RESULT_UNSUPPORTED:
            {
1066
                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_WARNING, tr("FAILURE: Unsupported CMD: %1").arg(ack.command));
1067 1068 1069 1070
            }
                break;
            case MAV_RESULT_FAILED:
            {
1071
                emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_ERROR, tr("FAILURE: Failed CMD: %1").arg(ack.command));
1072 1073 1074 1075
            }
                break;
            }
        }
1076
        case MAVLINK_MSG_ID_ATTITUDE_TARGET:
1077
        {
1078 1079 1080 1081
            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);
1082
            quint64 time = getUnixTimeFromMs(out.time_boot_ms);
1083
            emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, out.thrust, time);
1084 1085

            // For plotting emit roll sp, pitch sp and yaw sp values
1086 1087 1088
            emit valueChanged(uasId, "roll sp", "rad", roll, time);
            emit valueChanged(uasId, "pitch sp", "rad", pitch, time);
            emit valueChanged(uasId, "yaw sp", "rad", yaw, time);
1089 1090 1091 1092
        }
            break;
        case MAVLINK_MSG_ID_MISSION_COUNT:
        {
1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104
            mavlink_mission_count_t mc;
            mavlink_msg_mission_count_decode(&message, &mc);

            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
            if (mc.target_system == 0) {
                mc.target_system = mavlink->getSystemId();
            }
            if (mc.target_component == 0) {
                mc.target_component = mavlink->getComponentId();
            }

            // Check that this message applies to the UAS.
1105
            if(mc.target_system == mavlink->getSystemId())
1106
            {
1107 1108 1109 1110 1111 1112

                if (mc.target_component != mavlink->getComponentId()) {
                    qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.";
                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mc.target_component;
                }

1113
                waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count);
1114 1115 1116
            }
            else
            {
1117
                qDebug() << QString("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system);
1118 1119 1120 1121 1122 1123
            }
        }
            break;

        case MAVLINK_MSG_ID_MISSION_ITEM:
        {
1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135
            mavlink_mission_item_t mi;
            mavlink_msg_mission_item_decode(&message, &mi);

            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
            if (mi.target_system == 0) {
                mi.target_system = mavlink->getSystemId();
            }
            if (mi.target_component == 0) {
                mi.target_component = mavlink->getComponentId();
            }

            // Check that the item pertains to this UAS.
1136
            if(mi.target_system == mavlink->getSystemId())
1137
            {
1138 1139 1140 1141 1142 1143

                if (mi.target_component != mavlink->getComponentId()) {
                    qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.";
                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mi.target_component;
                }

1144
                waypointManager.handleWaypoint(message.sysid, message.compid, &mi);
1145 1146 1147
            }
            else
            {
1148
                qDebug() << QString("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system);
1149 1150 1151 1152 1153 1154
            }
        }
            break;

        case MAVLINK_MSG_ID_MISSION_ACK:
        {
1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166
            mavlink_mission_ack_t ma;
            mavlink_msg_mission_ack_decode(&message, &ma);

            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
            if (ma.target_system == 0) {
                ma.target_system = mavlink->getSystemId();
            }
            if (ma.target_component == 0) {
                ma.target_component = mavlink->getComponentId();
            }

            // Check that the ack pertains to this UAS.
1167
            if(ma.target_system == mavlink->getSystemId())
1168
            {
1169 1170 1171 1172 1173 1174

                if (ma.target_component != mavlink->getComponentId()) {
                    qDebug() << tr("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.");
                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << ma.target_component;
                }

1175 1176 1177
                waypointManager.handleWaypointAck(message.sysid, message.compid, &ma);
            }
            else
1178
            {
1179
                qDebug() << QString("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system);
1180 1181 1182 1183 1184 1185
            }
        }
            break;

        case MAVLINK_MSG_ID_MISSION_REQUEST:
        {
1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197
            mavlink_mission_request_t mr;
            mavlink_msg_mission_request_decode(&message, &mr);

            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
            if (mr.target_system == 0) {
                mr.target_system = mavlink->getSystemId();
            }
            if (mr.target_component == 0) {
                mr.target_component = mavlink->getComponentId();
            }

            // Check that the request pertains to this UAS.
1198
            if(mr.target_system == mavlink->getSystemId())
1199
            {
1200 1201 1202 1203 1204 1205

                if (mr.target_component != mavlink->getComponentId()) {
                    qDebug() << QString("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error.");
                    qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mr.target_component;
                }

1206
                waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr);
1207 1208 1209
            }
            else
            {
1210
                qDebug() << QString("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system);
1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221
            }
        }
            break;

        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
        {
            mavlink_mission_item_reached_t wpr;
            mavlink_msg_mission_item_reached_decode(&message, &wpr);
            waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr);
            QString text = QString("System %1 reached waypoint %2").arg(getUASName()).arg(wpr.seq);
            GAudioOutput::instance()->say(text);
1222
            emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_INFO, text);
1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233
        }
            break;

        case MAVLINK_MSG_ID_MISSION_CURRENT:
        {
            mavlink_mission_current_t wpc;
            mavlink_msg_mission_current_decode(&message, &wpc);
            waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
        }
            break;

1234
        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
1235 1236 1237 1238 1239
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
1240 1241 1242 1243
            mavlink_position_target_local_ned_t p;
            mavlink_msg_position_target_local_ned_decode(&message, &p);
            quint64 time = getUnixTimeFromMs(p.time_boot_ms);
            emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time);
1244 1245
        }
            break;
1246
        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
1247
        {
1248 1249 1250
            mavlink_set_position_target_local_ned_t p;
            mavlink_msg_set_position_target_local_ned_decode(&message, &p);
            emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */);
1251 1252 1253 1254 1255
        }
            break;
        case MAVLINK_MSG_ID_STATUSTEXT:
        {
            QByteArray b;
1256
            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
1257
            mavlink_msg_statustext_get_text(&message, b.data());
1258 1259
            // Ensure NUL-termination
            b[b.length()-1] = '\0';
1260 1261 1262
            QString text = QString(b);
            int severity = mavlink_msg_statustext_get_severity(&message);

1263
            if (text.startsWith("#") || severity <= MAV_SEVERITY_WARNING)
1264 1265
            {
                text.remove("#audio:");
1266
                emit textMessageReceived(uasId, message.compid, severity, text);
1267
                GAudioOutput::instance()->say(text.toLower(), severity);
1268 1269 1270 1271 1272 1273 1274
            }
            else
            {
                emit textMessageReceived(uasId, message.compid, severity, text);
            }
        }
            break;
1275
#if 0
1276 1277 1278 1279 1280
        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
        {
            mavlink_servo_output_raw_t raw;
            mavlink_msg_servo_output_raw_decode(&message, &raw);

1281
            if (hilEnabled && raw.port == 0)
1282
            {
Lorenz Meier's avatar
Lorenz Meier committed
1283
                emit hilActuatorsChanged(static_cast<uint64_t>(getUnixTimeFromMs(raw.time_usec)), static_cast<float>(raw.servo1_raw),
1284 1285 1286 1287 1288 1289
                                     static_cast<float>(raw.servo2_raw), static_cast<float>(raw.servo3_raw),
                                     static_cast<float>(raw.servo4_raw), static_cast<float>(raw.servo5_raw), static_cast<float>(raw.servo6_raw),
                                     static_cast<float>(raw.servo7_raw), static_cast<float>(raw.servo8_raw));
            }
        }
        break;
1290
#endif
1291

1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303
        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();
1304 1305
            imagePacketsArrived = 0;

1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321
        }
            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
1322
                break;
1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335
            }

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

            ++imagePacketsArrived;

            // emit signal if all packets arrived
1336
            if (imagePacketsArrived >= imagePackets)
1337 1338
            {
                // Restart statemachine
Don Gagne's avatar
Don Gagne committed
1339 1340
                imagePackets = 0;
                imagePacketsArrived = 0;
1341 1342 1343 1344 1345 1346
                emit imageReady(this);
            }
        }
            break;

        case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
1347 1348 1349 1350
        {
            mavlink_nav_controller_output_t p;
            mavlink_msg_nav_controller_output_decode(&message,&p);
            setDistToWaypoint(p.wp_dist);
1351 1352
            setBearingToWaypoint(p.nav_bearing);
            emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error);
1353 1354
        }
            break;
Lorenz Meier's avatar
Lorenz Meier committed
1355 1356 1357
        // Messages to ignore
        case MAVLINK_MSG_ID_RAW_IMU:
        case MAVLINK_MSG_ID_SCALED_IMU:
1358 1359 1360 1361 1362 1363 1364 1365 1366
        case MAVLINK_MSG_ID_RAW_PRESSURE:
        case MAVLINK_MSG_ID_SCALED_PRESSURE:
        case MAVLINK_MSG_ID_OPTICAL_FLOW:
        case MAVLINK_MSG_ID_DEBUG_VECT:
        case MAVLINK_MSG_ID_DEBUG:
        case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
        case MAVLINK_MSG_ID_NAMED_VALUE_INT:
        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        case MAVLINK_MSG_ID_HIGHRES_IMU:
1367
        case MAVLINK_MSG_ID_DISTANCE_SENSOR:
1368 1369 1370 1371 1372 1373
            break;
        default:
        {
            if (!unknownPackets.contains(message.msgid))
            {
                unknownPackets.append(message.msgid);
1374

Lorenz Meier's avatar
Lorenz Meier committed
1375
                emit unknownPacketReceived(uasId, message.compid, message.msgid);
1376
                qDebug() << "Unknown message from system:" << uasId << "message:" << message.msgid;
1377 1378 1379 1380 1381 1382 1383 1384 1385 1386
            }
        }
            break;
        }
    }
}

/**
* Set the home position of the UAS.
* @param lat The latitude fo the home position
1387
* @param lon The longitude of the home position
1388 1389 1390 1391
* @param alt The altitude of the home position
*/
void UAS::setHomePosition(double lat, double lon, double alt)
{
1392 1393 1394
    if (blockHomePositionChanges)
        return;

1395 1396 1397 1398
    QString uasName = (getUASName() == "")?
                tr("UAS") + QString::number(getUASID())
              : getUASName();

Don Gagne's avatar
Don Gagne committed
1399 1400 1401 1402 1403
    QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set a new home position for vehicle %1").arg(uasName),
                                                                 tr("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"),
                                                                 QMessageBox::Yes | QMessageBox::Cancel,
                                                                 QMessageBox::Cancel);
    if (button == QMessageBox::Yes)
1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418
    {
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt);
        // Send message twice to increase chance that it reaches its goal
        sendMessage(msg);

        // Send new home position to UAS
        mavlink_set_gps_global_origin_t home;
        home.target_system = uasId;
        home.latitude = lat*1E7;
        home.longitude = lon*1E7;
        home.altitude = alt*1000;
        qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
        mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
        sendMessage(msg);
1419 1420
    } else {
        blockHomePositionChanges = true;
1421 1422 1423 1424 1425 1426 1427 1428
    }
}

/**
* Set the origin to the current GPS location.
**/
void UAS::setLocalOriginAtCurrentGPSPosition()
{
Don Gagne's avatar
Don Gagne committed
1429 1430 1431 1432 1433
    QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set the home position at the current GPS position?"),
                                                                 tr("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"),
                                                                 QMessageBox::Yes | QMessageBox::Cancel,
                                                                 QMessageBox::Cancel);
    if (button == QMessageBox::Yes)
1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446
    {
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 1, 0, 0, 0, 0, 0, 0);
        // Send message twice to increase chance that it reaches its goal
        sendMessage(msg);
    }
}

/**
* Set a local position setpoint.
* @param x postion
* @param y position
* @param z position
1447
*/
1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460
void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
{
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
}

/**
* Set a offset of the local position.
* @param x position
* @param y position
* @param z position
1461
* @param yaw
1462 1463 1464 1465 1466 1467 1468 1469 1470
*/
void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
{
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
}

1471
void UAS::startRadioControlCalibration(int param)
1472 1473 1474
{
    mavlink_message_t msg;
    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
1475
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0);
1476 1477 1478 1479 1480 1481 1482 1483
    sendMessage(msg);
}

void UAS::endRadioControlCalibration()
{
    mavlink_message_t msg;
    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0, 0);
1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521 1522 1523 1524
    sendMessage(msg);
}

void UAS::startDataRecording()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 2, 0, 0, 0);
    sendMessage(msg);
}

void UAS::stopDataRecording()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_DO_CONTROL_VIDEO, 1, -1, -1, -1, 0, 0, 0, 0);
    sendMessage(msg);
}

void UAS::startMagnetometerCalibration()
{
    mavlink_message_t msg;
    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

void UAS::startGyroscopeCalibration()
{
    mavlink_message_t msg;
    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

void UAS::startPressureCalibration()
{
    mavlink_message_t msg;
    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0, 0, 0, 0);
    sendMessage(msg);
}

1525 1526
/**
* Check if time is smaller than 40 years, assuming no system without Unix
1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577
* 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
1578
* the precise time stamp of this measurement augmented to UNIX time, but will
1579
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
1580
* reason why one would want this, except for system setups where the onboard
1581
* clock is not present or broken and datasets should be collected that are still
1582
* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE
1583 1584 1585 1586 1587 1588 1589 1590 1591
* 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
1592 1593 1594 1595
* 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
1596 1597 1598 1599 1600 1601 1602 1603 1604 1605 1606 1607 1608 1609 1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673
* 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;
}

/**
* @param component that will be searched for in the map of parameters.
*/
QList<QString> UAS::getParameterNames(int component)
{
    if (parameters.contains(component))
    {
        return parameters.value(component)->keys();
    }
    else
    {
        return QList<QString>();
    }
}

QList<int> UAS::getComponentIds()
{
    return parameters.keys();
}

/**
1674 1675
* @param newBaseMode that UAS is to be set to.
* @param newCustomMode that UAS is to be set to.
1676
*/
1677
void UAS::setMode(uint8_t newBaseMode, uint32_t newCustomMode)
1678
{
1679 1680 1681
    if (receivedMode)
    {
        //this->mode = mode; //no call assignament, update receive message from UAS
Lorenz Meier's avatar
Lorenz Meier committed
1682

1683 1684 1685 1686
        // Strip armed / disarmed call for safety reasons, this is not relevant for setting the mode
        newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
        // Now set current state (request no change)
        newBaseMode |= this->base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
Lorenz Meier's avatar
Lorenz Meier committed
1687

1688 1689 1690 1691
//        // Strip HIL part, replace it with current system state
//        newBaseMode &= (~MAV_MODE_FLAG_HIL_ENABLED);
//        // Now set current state (request no change)
//        newBaseMode |= this->base_mode & MAV_MODE_FLAG_HIL_ENABLED;
Lorenz Meier's avatar
Lorenz Meier committed
1692

1693 1694 1695 1696
        setModeArm(newBaseMode, newCustomMode);
    }
    else
    {
Thomas Gubler's avatar
Thomas Gubler committed
1697
        qDebug() << "WARNING: setMode called before base_mode bitmask was received from UAS, new mode was not sent to system";
1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716
    }
}

/**
* @param newBaseMode that UAS is to be set to.
* @param newCustomMode that UAS is to be set to.
*/
void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode)
{
    if (receivedMode)
    {
        mavlink_message_t msg;
        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newBaseMode, newCustomMode);
        qDebug() << "mavlink_msg_set_mode_pack 1";
        sendMessage(msg);
        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", MODE " << newBaseMode << " " << newCustomMode;
    }
    else
    {
Thomas Gubler's avatar
Thomas Gubler committed
1717
        qDebug() << "WARNING: setModeArm called before base_mode bitmask was received from UAS, new mode was not sent to system";
1718
    }
1719 1720 1721 1722 1723 1724 1725 1726
}

/**
* Send a message to every link that is connected.
* @param message that is to be sent
*/
void UAS::sendMessage(mavlink_message_t message)
{
1727 1728
    if (!LinkManager::instance())
    {
1729
        qDebug() << "LINKMANAGER NOT AVAILABLE!";
1730 1731 1732
        return;
    }

1733
    if (links.count() < 1) {
1734 1735 1736
        qDebug() << "NO LINK AVAILABLE TO SEND!";
    }

1737
    // Emit message on all links that are currently connected
1738 1739 1740
    foreach (LinkInterface* link, links) {
        if (link->isConnected()) {
            sendMessage(link, message);
1741 1742 1743 1744 1745 1746 1747 1748 1749 1750 1751
        }
    }
}

/**
* Forward a message to all links that are currently connected.
* @param message that is to be forwarded
*/
void UAS::forwardMessage(mavlink_message_t message)
{
    // Emit message on all links that are currently connected
1752
    QList<LinkInterface*>link_list = LinkManager::instance()->getLinks();
1753 1754 1755 1756 1757 1758 1759 1760

    foreach(LinkInterface* link, link_list)
    {
        if (link)
        {
            SerialLink* serial = dynamic_cast<SerialLink*>(link);
            if(serial != 0)
            {
1761
                for(int i=0; i<links.size(); i++)
1762
                {
1763
                    if(serial != links.at(i))
1764
                    {
1765 1766 1767 1768
                        if (link->isConnected()) {
                            qDebug()<<"Antenna tracking: Forwarding Over link: "<<serial->getName()<<" "<<serial;
                            sendMessage(serial, message);
                        }
1769 1770 1771 1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784 1785 1786 1787 1788 1789
                    }
                }
            }
        }
    }
}

/**
* Send a message to the link that is connected.
* @param link that the message will be sent to
* @message that is to be sent
*/
void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    if(!link) return;
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
    mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]);
1790

1791 1792 1793 1794 1795 1796
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
1797 1798 1799 1800
    else
    {
        qDebug() << "LINK NOT CONNECTED, NOT SENDING!";
    }
1801 1802 1803 1804 1805 1806 1807
}

/**
 * @param value battery voltage
 */
float UAS::filterVoltage(float value) const
{
Lorenz Meier's avatar
Lorenz Meier committed
1808
    return lpVoltage * 0.6f + value * 0.4f;
1809 1810
}

1811
/**
1812 1813 1814 1815 1816 1817 1818 1819 1820 1821 1822 1823 1824 1825 1826 1827 1828 1829 1830 1831 1832 1833 1834 1835 1836 1837 1838 1839 1840 1841 1842 1843 1844 1845 1846 1847 1848 1849 1850 1851 1852 1853 1854 1855 1856 1857 1858 1859 1860 1861 1862 1863 1864 1865 1866 1867 1868 1869 1870 1871 1872
* 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)
    {
1873
        int imgColors = 255;
1874 1875 1876 1877 1878

        // 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
1879
        QByteArray tmpImage(header.toStdString().c_str(), header.length());
1880 1881 1882 1883 1884 1885 1886 1887 1888 1889 1890 1891
        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"))
        {
1892
            qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
1893 1894 1895 1896 1897 1898 1899 1900 1901 1902 1903 1904
            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))
        {
1905
            qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!";
1906
            return QImage();
1907 1908
        }
    }
1909

1910 1911
    // Restart statemachine
    imagePacketsArrived = 0;
1912 1913
    imagePackets = 0;
    imageRecBuffer.clear();
1914 1915 1916 1917 1918 1919 1920 1921 1922 1923 1924
    return image;
}

void UAS::requestImage()
{
    qDebug() << "trying to get an image from the uas...";

    // check if there is already an image transmission going on
    if (imagePacketsArrived == 0)
    {
        mavlink_message_t msg;
1925
        mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50);
1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936 1937 1938 1939 1940 1941 1942 1943 1944 1945 1946 1947 1948 1949 1950 1951 1952 1953 1954 1955 1956 1957 1958 1959 1960 1961 1962 1963 1964 1965 1966 1967 1968 1969
        sendMessage(msg);
    }
}


/* MANAGEMENT */

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

int UAS::getCommunicationStatus() const
{
    return commStatus;
}

void UAS::writeParametersToStorage()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
    qDebug() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE;
    sendMessage(msg);
}

void UAS::readParametersFromStorage()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1, 0, 0, 0);
    sendMessage(msg);
}

1970 1971
bool UAS::isRotaryWing()
{
1972
    switch (type) {
1973 1974 1975 1976 1977 1978 1979 1980 1981 1982 1983 1984 1985 1986 1987
        case MAV_TYPE_QUADROTOR:
        /* fallthrough */
        case MAV_TYPE_COAXIAL:
        case MAV_TYPE_HELICOPTER:
        case MAV_TYPE_HEXAROTOR:
        case MAV_TYPE_OCTOROTOR:
        case MAV_TYPE_TRICOPTER:
            return true;
        default:
            return false;
    }
}

bool UAS::isFixedWing()
{
1988
    switch (type) {
1989 1990 1991 1992 1993 1994 1995
        case MAV_TYPE_FIXED_WING:
            return true;
        default:
            return false;
    }
}

1996
/**
1997 1998 1999 2000 2001 2002 2003 2004 2005 2006 2007 2008 2009 2010 2011 2012 2013 2014 2015 2016 2017 2018 2019 2020 2021 2022 2023
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableAllDataTransmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    // 0 is a magic ID and will enable/disable the standard message set except for heartbeat
    stream.req_stream_id = MAV_DATA_STREAM_ALL;
    // Select the update rate in Hz the message should be send
    // All messages will be send with their default rate
    // TODO: use 0 to turn off and get rid of enable/disable? will require
    //  a different magic flag for turning on defaults, possibly something really high like 1111 ?
    stream.req_message_rate = 0;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
}

2024
/**
2025 2026 2027 2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038 2039 2040 2041 2042 2043 2044 2045 2046 2047
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableRawSensorDataTransmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
}

2048
/**
2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062 2063 2064 2065 2066 2067 2068 2069 2070 2071
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableExtendedSystemStatusTransmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
}

2072
/**
2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084 2085 2086 2087 2088 2089 2090 2091 2092 2093 2094 2095 2096 2097 2098 2099 2100
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableRCChannelDataTransmission(int rate)
{
#if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES)
    mavlink_message_t msg;
    mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled);
    sendMessage(msg);
#else
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
#endif
}

2101
/**
2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114 2115 2116 2117 2118 2119 2120 2121 2122 2123 2124 2125 2126 2127 2128 2129 2130 2131 2132 2133 2134 2135 2136 2137 2138 2139 2140 2141 2142 2143 2144 2145 2146
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableRawControllerDataTransmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
}

//void UAS::enableRawSensorFusionTransmission(int rate)
//{
//    // Buffers to write data to
//    mavlink_message_t msg;
//    mavlink_request_data_stream_t stream;
//    // Select the message to request from now on
//    stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION;
//    // Select the update rate in Hz the message should be send
//    stream.req_message_rate = rate;
//    // Start / stop the message
//    stream.start_stop = (rate) ? 1 : 0;
//    // The system which should take this command
//    stream.target_system = uasId;
//    // The component / subsystem which should take this command
//    stream.target_component = 0;
//    // Encode and send the message
//    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
//    sendMessage(msg);
//}

2147
/**
2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160 2161 2162 2163 2164 2165 2166 2167 2168 2169 2170
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enablePositionTransmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_POSITION;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
}

2171
/**
2172 2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183 2184 2185 2186 2187 2188 2189 2190 2191 2192 2193 2194 2195
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableExtra1Transmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA1;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
}

2196
/**
2197 2198 2199 2200 2201 2202 2203 2204 2205 2206 2207 2208 2209 2210 2211 2212 2213 2214 2215 2216 2217 2218 2219 2220
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableExtra2Transmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA2;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
}

2221
/**
2222 2223 2224 2225 2226 2227 2228 2229 2230 2231 2232 2233 2234 2235 2236 2237 2238 2239 2240 2241 2242 2243 2244 2245 2246 2247 2248 2249
* @param rate The update rate in Hz the message should be sent
*/
void UAS::enableExtra3Transmission(int rate)
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA3;
    // Select the update rate in Hz the message should be send
    stream.req_message_rate = rate;
    // Start / stop the message
    stream.start_stop = (rate) ? 1 : 0;
    // The system which should take this command
    stream.target_system = uasId;
    // The component / subsystem which should take this command
    stream.target_component = 0;
    // Encode and send the message
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
}

/**
 * Set a parameter value onboard
 *
 * @param component The component to set the parameter
2250
 * @param id Name of the parameter
2251
 */
2252
void UAS::setParameter(const int compId, const QString& paramId, const QVariant& value)
2253
{
2254
    if (!paramId.isNull())
2255 2256 2257 2258 2259 2260
    {
        mavlink_message_t msg;
        mavlink_param_set_t p;
        mavlink_param_union_t union_value;

        // Assign correct value based on QVariant
2261
        // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
2262
        if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA)
2263
        {
2264
            switch ((int)value.type())
2265 2266
            {
            case QVariant::Char:
2267
                union_value.param_float = (unsigned char)value.toChar().toLatin1();
2268 2269 2270 2271 2272 2273 2274 2275 2276 2277 2278 2279 2280 2281 2282 2283 2284 2285 2286 2287 2288
                p.param_type = MAV_PARAM_TYPE_INT8;
                break;
            case QVariant::Int:
                union_value.param_float = value.toInt();
                p.param_type = MAV_PARAM_TYPE_INT32;
                break;
            case QVariant::UInt:
                union_value.param_float = value.toUInt();
                p.param_type = MAV_PARAM_TYPE_UINT32;
                break;
            case QMetaType::Float:
                union_value.param_float = value.toFloat();
                p.param_type = MAV_PARAM_TYPE_REAL32;
                break;
            default:
                qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE";
                return;
            }
        }
        else
        {
2289
            switch ((int)value.type())
2290 2291
            {
            case QVariant::Char:
2292
                union_value.param_int8 = (unsigned char)value.toChar().toLatin1();
2293 2294 2295 2296 2297 2298 2299 2300 2301 2302 2303 2304 2305 2306 2307 2308 2309 2310
                p.param_type = MAV_PARAM_TYPE_INT8;
                break;
            case QVariant::Int:
                union_value.param_int32 = value.toInt();
                p.param_type = MAV_PARAM_TYPE_INT32;
                break;
            case QVariant::UInt:
                union_value.param_uint32 = value.toUInt();
                p.param_type = MAV_PARAM_TYPE_UINT32;
                break;
            case QMetaType::Float:
                union_value.param_float = value.toFloat();
                p.param_type = MAV_PARAM_TYPE_REAL32;
                break;
            default:
                qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE";
                return;
            }
2311 2312 2313 2314
        }

        p.param_value = union_value.param_float;
        p.target_system = (uint8_t)uasId;
2315
        p.target_component = (uint8_t)compId;
2316 2317 2318 2319 2320 2321 2322

        //qDebug() << "SENT PARAM:" << value;

        // Copy string into buffer, ensuring not to exceed the buffer size
        for (unsigned int i = 0; i < sizeof(p.param_id); i++)
        {
            // String characters
2323
            if ((int)i < paramId.length())
2324
            {
2325
                p.param_id[i] = paramId.toLatin1()[i];
2326 2327 2328 2329 2330 2331 2332
            }
            else
            {
                // Fill rest with zeros
                p.param_id[i] = 0;
            }
        }
2333
        mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p);
2334 2335 2336 2337
        sendMessage(msg);
    }
}

2338 2339 2340 2341



//TODO update this to use the parameter manager / param data model instead
2342
void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue,  mavlink_param_union_t& paramUnion)
2343 2344 2345 2346 2347 2348 2349 2350
{
    int compId = msg.compid;

    // Insert component if necessary
    if (!parameters.contains(compId)) {
        parameters.insert(compId, new QMap<QString, QVariant>());
    }

2351 2352 2353 2354
    // Insert parameter into registry
    if (parameters.value(compId)->contains(paramName)) {
        parameters.value(compId)->remove(paramName);
    }
2355

2356
    QVariant paramValue;
2357 2358 2359

    // Insert with correct type
    // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
2360

2361 2362 2363 2364 2365 2366 2367 2368
    switch (rawValue.param_type) {
        case MAV_PARAM_TYPE_REAL32:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant(paramUnion.param_float);
            } else {
                paramValue = QVariant(paramUnion.param_float);
            }
            break;
2369

2370 2371 2372 2373 2374 2375 2376
        case MAV_PARAM_TYPE_UINT8:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant(QChar((unsigned char)paramUnion.param_float));
            } else {
                paramValue = QVariant(QChar((unsigned char)paramUnion.param_uint8));
            }
            break;
2377

2378 2379 2380 2381 2382 2383 2384
        case MAV_PARAM_TYPE_INT8:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant(QChar((char)paramUnion.param_float));
            } else  {
                paramValue = QVariant(QChar((char)paramUnion.param_int8));
            }
            break;
2385

2386 2387 2388 2389 2390 2391 2392
        case MAV_PARAM_TYPE_INT16:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant((short)paramUnion.param_float);
            } else {
                paramValue = QVariant(paramUnion.param_int16);
            }
            break;
2393

2394 2395 2396 2397 2398 2399 2400 2401 2402 2403 2404 2405 2406 2407
        case MAV_PARAM_TYPE_UINT32:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant((unsigned int)paramUnion.param_float);
            } else {
                paramValue = QVariant(paramUnion.param_uint32);
            }
            break;
        case MAV_PARAM_TYPE_INT32:
            if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                paramValue = QVariant((int)paramUnion.param_float);
            } else {
                paramValue = QVariant(paramUnion.param_int32);
            }
            break;
2408

2409 2410
        default:
            qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
2411
    }
2412

2413
    qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
2414

2415 2416 2417
    parameters.value(compId)->insert(paramName, paramValue);
    emit parameterChanged(uasId, compId, paramName, paramValue);
    emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, paramValue);
2418 2419
}

2420
/**
2421 2422 2423 2424 2425 2426 2427 2428 2429 2430 2431 2432 2433 2434 2435 2436 2437 2438 2439 2440 2441 2442 2443 2444 2445 2446 2447 2448
* Request parameter, use parameter name to request it.
*/
void UAS::requestParameter(int component, int id)
{
    // Request parameter, use parameter name to request it
    mavlink_message_t msg;
    mavlink_param_request_read_t read;
    read.param_index = id;
    read.param_id[0] = '\0'; // Enforce null termination
    read.target_system = uasId;
    read.target_component = component;
    mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read);
    sendMessage(msg);
    //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << id;
}

/**
* Request a parameter, use parameter name to request it.
*/
void UAS::requestParameter(int component, const QString& parameter)
{
    // Request parameter, use parameter name to request it
    mavlink_message_t msg;
    mavlink_param_request_read_t read;
    read.param_index = -1;
    // Copy full param name or maximum max field size
    if (parameter.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)
    {
2449
        emit textMessageReceived(uasId, 0, MAV_SEVERITY_WARNING, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1));
2450
    }
Don Gagne's avatar
Don Gagne committed
2451 2452
    strncpy(read.param_id, parameter.toStdString().c_str(), sizeof(read.param_id));
    read.param_id[sizeof(read.param_id) - 1] = '\0'; // Enforce null termination
2453 2454 2455 2456
    read.target_system = uasId;
    read.target_component = component;
    mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read);
    sendMessage(msg);
2457
    //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM NAME" << parameter;
2458 2459 2460 2461 2462 2463 2464 2465 2466 2467
}

/**
* @param systemType Type of MAV.
*/
void UAS::setSystemType(int systemType)
{
    if((systemType >= MAV_TYPE_GENERIC) && (systemType < MAV_TYPE_ENUM_END))
    {
      type = systemType;
2468

2469 2470 2471
      // If the airframe is still generic, change it to a close default type
      if (airframe == 0)
      {
2472
          switch (type)
2473 2474
          {
          case MAV_TYPE_FIXED_WING:
2475
              setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR);
2476 2477
              break;
          case MAV_TYPE_QUADROTOR:
2478 2479 2480 2481 2482 2483 2484
              setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
              break;
          case MAV_TYPE_HEXAROTOR:
              setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER);
              break;
          default:
              // Do nothing
2485 2486 2487 2488
              break;
          }
      }
      emit systemSpecsChanged(uasId);
2489 2490
      emit systemTypeSet(this, type);
      qDebug() << "TYPE CHANGED TO:" << type;
2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2519 2520 2521 2522
   }
}

void UAS::setUASName(const QString& name)
{
    if (name != "")
    {
        this->name = name;
        writeSettings();
        emit nameChanged(name);
        emit systemSpecsChanged(uasId);
    }
}

void UAS::executeCommand(MAV_CMD command)
{
    mavlink_message_t msg;
    mavlink_command_long_t cmd;
    cmd.command = (uint16_t)command;
    cmd.confirmation = 0;
    cmd.param1 = 0.0f;
    cmd.param2 = 0.0f;
    cmd.param3 = 0.0f;
    cmd.param4 = 0.0f;
    cmd.param5 = 0.0f;
    cmd.param6 = 0.0f;
    cmd.param7 = 0.0f;
    cmd.target_system = uasId;
    cmd.target_component = 0;
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
    sendMessage(msg);
}
2523 2524 2525 2526 2527 2528 2529 2530 2531
void UAS::executeCommandAck(int num, bool success)
{
    mavlink_message_t msg;
    mavlink_command_ack_t ack;
    ack.command = num;
    ack.result = (success ? 1 : 0);
    mavlink_msg_command_ack_encode(mavlink->getSystemId(),mavlink->getComponentId(),&msg,&ack);
    sendMessage(msg);
}
2532 2533 2534 2535 2536 2537 2538 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568

void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{
    mavlink_message_t msg;
    mavlink_command_long_t cmd;
    cmd.command = (uint16_t)command;
    cmd.confirmation = confirmation;
    cmd.param1 = param1;
    cmd.param2 = param2;
    cmd.param3 = param3;
    cmd.param4 = param4;
    cmd.param5 = param5;
    cmd.param6 = param6;
    cmd.param7 = param7;
    cmd.target_system = uasId;
    cmd.target_component = component;
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
    sendMessage(msg);
}

/**
 * Launches the system
 *
 */
void UAS::launch()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

/**
 * @warning Depending on the UAS, this might make the rotors of a helicopter spinning
 *
 */
void UAS::armSystem()
{
2569
    setModeArm(base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
2570 2571 2572 2573 2574 2575 2576 2577
}

/**
 * @warning Depending on the UAS, this might completely stop all motors.
 *
 */
void UAS::disarmSystem()
{
2578
    setModeArm(base_mode & ~(MAV_MODE_FLAG_SAFETY_ARMED), custom_mode);
2579 2580
}

2581 2582
void UAS::toggleArmedState()
{
2583
    setModeArm(base_mode ^ (MAV_MODE_FLAG_SAFETY_ARMED), custom_mode);
2584 2585
}

2586 2587
void UAS::goAutonomous()
{
2588
    setMode((base_mode & ~(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)) | (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), 0);
2589
    qDebug() << __FILE__ << __LINE__ << "Going autonomous";
2590 2591 2592 2593
}

void UAS::goManual()
{
2594
    setMode((base_mode & ~(MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED))  | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
2595
    qDebug() << __FILE__ << __LINE__ << "Going manual";
2596 2597 2598 2599
}

void UAS::toggleAutonomy()
{
2600
    setMode(base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ^ MAV_MODE_FLAG_GUIDED_ENABLED ^ MAV_MODE_FLAG_STABILIZE_ENABLED, 0);
2601
    qDebug() << __FILE__ << __LINE__ << "Toggling autonomy";
2602 2603
}

2604 2605
/**
* Set the manual control commands.
2606 2607
* This can only be done if the system has manual inputs enabled and is armed.
*/
2608
void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons)
2609
{
2610 2611 2612
    Q_UNUSED(xHat);
    Q_UNUSED(yHat);

2613
    // Store the previous manual commands
2614 2615 2616 2617 2618 2619
    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
2620

2621 2622
    // We only transmit manual command messages if the system has manual inputs enabled and is armed
    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
2623 2624
    {

2625 2626 2627 2628 2629 2630
        // Transmit the manual commands 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 50Hz, but when no inputs have changed it drops down to 5Hz.
        bool sendCommand = false;
        if (countSinceLastTransmission++ >= 10)
2631
        {
2632 2633
            sendCommand = true;
            countSinceLastTransmission = 0;
2634
        }
2635 2636 2637 2638 2639
        else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
                   (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
                   buttons != manualButtons)
        {
            sendCommand = true;
2640

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

2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671
        // 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;

            // Store scaling values for all 3 axes
            const float axesScaling = 1.0 * 1000.0;

            // Calculate the new commands for roll, pitch, yaw, and thrust
            const float newRollCommand = roll * axesScaling;
            const float newPitchCommand = pitch * axesScaling;
            const float newYawCommand = yaw * axesScaling;
            const float newThrustCommand = thrust * axesScaling;

            // Send the MANUAL_COMMAND message
            mavlink_message_t message;
            mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
            sendMessage(message);

            // Emit an update in control values to other UI elements, like the HSI display
            emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
        }
2672 2673 2674 2675 2676
    }
}

void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{
2677
    // If system has manual inputs enabled and is armed
2678
    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
2679 2680
    {
        mavlink_message_t message;
2681 2682
        float q[4];
        mavlink_euler_to_quaternion(roll, pitch, yaw, q);
2683

Lorenz Meier's avatar
Lorenz Meier committed
2684 2685
        float yawrate = 0.0f;

2686
        // Do not control rates and throttle
2687
        quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates
2688 2689 2690 2691
        mask |= (1 << 6); // ignore throttle
        mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(),
                                             &message, QGC::groundTimeMilliseconds(), this->uasId, 0,
                                             mask, q, 0, 0, 0, 0);
2692
        sendMessage(message);
Lorenz Meier's avatar
Lorenz Meier committed
2693
        quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) |
2694
            (1 << 6) | (1 << 7) | (1 << 8);
2695 2696
        mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(),
                                                       &message, QGC::groundTimeMilliseconds(), this->uasId, 0,
Lorenz Meier's avatar
Lorenz Meier committed
2697
                                                       MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate);
2698
        sendMessage(message);
2699
        qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
2700 2701 2702 2703 2704 2705 2706

        //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
    }
    else
    {
        qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
    }
2707 2708 2709 2710 2711 2712 2713 2714 2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736
}

/**
* @return the type of the system
*/
int UAS::getSystemType()
{
    return this->type;
}

/**
* Halt the uas.
*/
void UAS::halt()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_HOLD, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

/**
* Make the UAS move.
*/
void UAS::go()
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

2737 2738
/**
* Order the robot to return home
2739 2740 2741 2742 2743 2744 2745 2746 2747 2748 2749 2750 2751 2752 2753
*/
void UAS::home()
{
    mavlink_message_t msg;

    double latitude = UASManager::instance()->getHomeLatitude();
    double longitude = UASManager::instance()->getHomeLongitude();
    double altitude = UASManager::instance()->getHomeAltitude();
    int frame = UASManager::instance()->getHomeFrame();

    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude);
    sendMessage(msg);
}

/**
2754
* Order the robot to land on the runway
2755 2756 2757 2758 2759 2760 2761 2762 2763
*/
void UAS::land()
{
    mavlink_message_t msg;

    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

2764
/**
Jean Cyr's avatar
Jean Cyr committed
2765
* Order the robot to start receiver pairing
2766 2767 2768 2769 2770 2771 2772 2773 2774
*/
void UAS::pairRX(int rxType, int rxSubType)
{
    mavlink_message_t msg;

    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0);
    sendMessage(msg);
}

2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806 2807 2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819 2820 2821 2822 2823 2824 2825
/**
 * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
 * and might differ between systems.
 */
void UAS::emergencySTOP()
{
    // FIXME MAVLINKV10PORTINGNEEDED
    halt();
}

/**
 * Shut down this mav - All onboard systems are immediately shut down (e.g. the
 *  main power line is cut).
 * @warning This might lead to a crash.
 *
 * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards
 */
bool UAS::emergencyKILL()
{
    halt();
    // FIXME MAVLINKV10PORTINGNEEDED
    //    bool result = false;
    //    QMessageBox msgBox;
    //    msgBox.setIcon(QMessageBox::Critical);
    //    msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
    //    msgBox.setInformativeText("Do you want to cut power on all systems?");
    //    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
    //    msgBox.setDefaultButton(QMessageBox::Cancel);
    //    int ret = msgBox.exec();

    //    // Close the message box shortly after the click to prevent accidental clicks
    //    QTimer::singleShot(5000, &msgBox, SLOT(reject()));


    //    if (ret == QMessageBox::Yes)
    //    {
    //        mavlink_message_t msg;
    //        // TODO Replace MG System ID with static function call and allow to change ID in GUI
    //        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
    //        // Send message twice to increase chance of reception
    //        sendMessage(msg);
    //        sendMessage(msg);
    //        result = true;
    //    }
    //    return result;
    return false;
}

/**
* If enabled, connect the flight gear link.
*/
2826
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration)
2827
{
2828
    Q_UNUSED(configuration);
2829

2830 2831 2832 2833 2834 2835 2836 2837 2838 2839 2840 2841
    QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
    if (!link || !simulation) {
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
        simulation = new QGCFlightGearLink(this, options);
    }
    // Connect Flight Gear Link
    link = dynamic_cast<QGCFlightGearLink*>(simulation);
    link->setStartupArguments(options);
Thomas Gubler's avatar
Thomas Gubler committed
2842
    link->sensorHilEnabled(sensorHil);
2843 2844
    // 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)));
2845 2846 2847 2848 2849 2850 2851 2852 2853 2854 2855 2856 2857 2858 2859 2860 2861 2862 2863 2864 2865 2866 2867 2868 2869 2870 2871
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}

/**
* If enabled, connect the JSBSim link.
*/
void UAS::enableHilJSBSim(bool enable, QString options)
{
    QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
    if (!link || !simulation) {
        // Delete wrong sim
        if (simulation) {
            stopHil();
            delete simulation;
        }
        simulation = new QGCJSBSimLink(this, options);
    }
    // Connect Flight Gear Link
    link = dynamic_cast<QGCJSBSimLink*>(simulation);
    link->setStartupArguments(options);
2872 2873 2874 2875 2876 2877 2878 2879 2880 2881 2882 2883 2884 2885 2886 2887 2888 2889 2890 2891 2892 2893 2894 2895 2896 2897 2898 2899 2900 2901 2902 2903 2904 2905 2906
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}

/**
* If enabled, connect the X-plane gear link.
*/
void UAS::enableHilXPlane(bool enable)
{
    QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
    if (!link || !simulation) {
        if (simulation) {
            stopHil();
            delete simulation;
        }
        qDebug() << "CREATED NEW XPLANE LINK";
        simulation = new QGCXPlaneLink(this);
    }
    // Connect X-Plane Link
    if (enable)
    {
        startHil();
    }
    else
    {
        stopHil();
    }
}

2907 2908 2909 2910 2911 2912 2913 2914 2915 2916 2917 2918 2919 2920 2921 2922 2923 2924 2925 2926 2927 2928
/**
* @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)
*/
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)
{
2929 2930 2931 2932
    Q_UNUSED(time_us);
    Q_UNUSED(xacc);
    Q_UNUSED(yacc);
    Q_UNUSED(zacc);
2933

2934 2935 2936 2937 2938 2939 2940 2941 2942 2943 2944 2945 2946 2947 2948 2949 2950 2951 2952 2953 2954
        // 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());
}

2955 2956 2957 2958 2959 2960 2961 2962 2963 2964 2965 2966 2967 2968 2969 2970 2971 2972
/**
* @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)
*/
2973
void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
Lorenz Meier's avatar
Lorenz Meier committed
2974
                       float pitchspeed, float yawspeed, double lat, double lon, double alt,
2975
                       float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
2976
{
2977
    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
2978
    {
2979 2980 2981 2982 2983 2984 2985 2986 2987 2988 2989 2990 2991 2992 2993 2994 2995 2996
        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;
2997
        mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
2998 2999 3000
                                   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);
        sendMessage(msg);
3001 3002 3003 3004
    }
    else
    {
        // Attempt to set HIL mode
3005
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
3006 3007 3008 3009
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}

3010 3011 3012 3013
/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure  (hPa)
*/
Lorenz Meier's avatar
Lorenz Meier committed
3014
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
3015
                                    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
3016
{
3017
    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
Lorenz Meier's avatar
Lorenz Meier committed
3018 3019
    {
        mavlink_message_t msg;
3020
        mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
Lorenz Meier's avatar
Lorenz Meier committed
3021 3022 3023 3024
                                   time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
                                     xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature,
                                     fields_changed);
        sendMessage(msg);
3025
        lastSendTimeSensors = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
3026 3027 3028 3029
    }
    else
    {
        // Attempt to set HIL mode
3030
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
Lorenz Meier's avatar
Lorenz Meier committed
3031 3032 3033 3034
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}

3035 3036 3037
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)
{
Don Gagne's avatar
Don Gagne committed
3038
    // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api
3039

Don Gagne's avatar
Don Gagne committed
3040 3041 3042 3043 3044 3045 3046
    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);
3047

3048 3049
    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
    {
Don Gagne's avatar
Don Gagne committed
3050
#if 0
3051 3052
        mavlink_message_t msg;
        mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
Don Gagne's avatar
Don Gagne committed
3053
                                   time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance);
3054

3055 3056
        sendMessage(msg);
        lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
Don Gagne's avatar
Don Gagne committed
3057
#endif
3058 3059 3060 3061 3062 3063 3064 3065 3066 3067
    }
    else
    {
        // Attempt to set HIL mode
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }

}

3068
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
3069
{
3070 3071 3072 3073
    // Only send at 10 Hz max rate
    if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
        return;

3074
    if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
Lorenz Meier's avatar
Lorenz Meier committed
3075
    {
Lorenz Meier's avatar
Lorenz Meier committed
3076 3077 3078
        float course = cog;
        // map to 0..2pi
        if (course < 0)
3079
            course += 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
3080 3081 3082
        // scale from radians to degrees
        course = (course / M_PI) * 180.0f;

Lorenz Meier's avatar
Lorenz Meier committed
3083
        mavlink_message_t msg;
3084 3085
        mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &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);
3086
        lastSendTimeGPS = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
3087 3088 3089 3090 3091
        sendMessage(msg);
    }
    else
    {
        // Attempt to set HIL mode
3092
        setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
Lorenz Meier's avatar
Lorenz Meier committed
3093 3094 3095 3096 3097
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
    }
}


3098 3099 3100 3101 3102 3103 3104
/**
* Connect flight gear link.
**/
void UAS::startHil()
{
    if (hilEnabled) return;
    hilEnabled = true;
3105
    sensorHil = false;
3106
    setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
3107
    qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
3108 3109
    // Connect HIL simulation link
    simulation->connectSimulation();
3110 3111 3112 3113 3114 3115 3116
}

/**
* disable flight gear link.
*/
void UAS::stopHil()
{
3117 3118 3119 3120 3121
    if (simulation && simulation->isConnected()) {
        simulation->disconnectSimulation();
        setMode(base_mode & ~MAV_MODE_FLAG_HIL_ENABLED, custom_mode);
        qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
    }
3122
    hilEnabled = false;
3123
    sensorHil = false;
3124 3125 3126 3127
}

void UAS::shutdown()
{
Don Gagne's avatar
Don Gagne committed
3128 3129 3130 3131 3132
    QMessageBox::StandardButton button = QGCMessageBox::question(tr("Shutting down the UAS"),
                                                                 tr("Do you want to shut down the onboard computer?"),
                                                                 QMessageBox::Yes | QMessageBox::Cancel,
                                                                 QMessageBox::Cancel);
    if (button == QMessageBox::Yes)
3133 3134 3135 3136 3137 3138 3139 3140 3141 3142 3143 3144 3145 3146 3147 3148 3149 3150 3151 3152 3153 3154 3155 3156 3157 3158 3159 3160 3161 3162 3163 3164 3165 3166 3167 3168 3169 3170 3171 3172 3173 3174 3175 3176 3177 3178
    {
        // If the active UAS is set, execute command
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 0, 2, 0, 0, 0, 0, 0);
        sendMessage(msg);
    }
}

/**
* @param x position
* @param y position
* @param z position
* @param yaw
*/
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
    mavlink_message_t msg;
    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 1, 0, yaw, x, y, z);
    sendMessage(msg);
}

/**
 * @return The name of this system as string in human-readable form
 */
QString UAS::getUASName(void) const
{
    QString result;
    if (name == "")
    {
        result = tr("MAV ") + result.sprintf("%03d", getUASID());
    }
    else
    {
        result = name;
    }
    return result;
}

/**
* @return the state of the uas as a short text.
*/
const QString& UAS::getShortState() const
{
    return shortStateText;
}

3179 3180
/**
* The mode can be autonomous, guided, manual or armed. It will also return if
3181 3182 3183 3184 3185 3186 3187 3188 3189 3190 3191 3192 3193 3194 3195 3196 3197
* hardware in the loop is being used.
* @return the audio mode text for the id given.
*/
QString UAS::getAudioModeTextFor(int id)
{
    QString mode;
    uint8_t modeid = id;

    // BASE MODE DECODING
    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_AUTO)
    {
        mode += "autonomous";
    }
    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
    {
        mode += "guided";
    }
Lorenz Meier's avatar
Lorenz Meier committed
3198 3199 3200 3201
    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
    {
        mode += "stabilized";
    }
3202 3203 3204 3205 3206 3207 3208 3209 3210 3211 3212 3213 3214 3215 3216 3217 3218 3219 3220 3221 3222 3223 3224 3225 3226 3227 3228 3229 3230 3231
    else if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
    {
        mode += "manual";
    }
    else
    {
        // Nothing else applies, we're in preflight
        mode += "preflight";
    }

    if (modeid != 0)
    {
        mode += " mode";
    }

    // ARMED STATE DECODING
    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
    {
        mode.append(" and armed");
    }

    // HARDWARE IN THE LOOP DECODING
    if (modeid & (uint8_t)MAV_MODE_FLAG_DECODE_POSITION_HIL)
    {
        mode.append(" using hardware in the loop simulation");
    }

    return mode;
}

3232 3233 3234 3235
/**
* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
* @return the short text of the mode for the id given.
*/
3236
/**
3237
* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
3238 3239
* @return the short text of the mode for the id given.
*/
3240
QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const
3241
{
3242
    QString mode = AutoPilotPluginManager::instance()->getShortModeText(base_mode, custom_mode, autopilot);
3243 3244

    if (mode.length() == 0)
3245 3246
    {
        mode = "|UNKNOWN";
3247
        qDebug() << __FILE__ << __LINE__ << " Unknown mode: base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot;
3248 3249 3250
    }

    // ARMED STATE DECODING
3251
    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
3252 3253 3254 3255 3256 3257 3258 3259 3260
    {
        mode.prepend("A");
    }
    else
    {
        mode.prepend("D");
    }

    // HARDWARE IN THE LOOP DECODING
3261
    if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
3262 3263 3264 3265
    {
        mode.prepend("HIL:");
    }

3266
    //qDebug() << "base_mode=" << base_mode << " custom_mode=" << custom_mode << " autopilot=" << autopilot << ": " << mode;
3267

3268 3269 3270 3271 3272 3273 3274 3275 3276 3277 3278 3279 3280
    return mode;
}

const QString& UAS::getShortMode() const
{
    return shortModeText;
}

/**
* Add the link and connect a signal to it which will be set off when it is destroyed.
*/
void UAS::addLink(LinkInterface* link)
{
3281
    if (!links.contains(link))
3282
    {
3283 3284
        links.append(link);
        qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16);
3285 3286 3287 3288 3289 3290
        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
    }
}

void UAS::removeLink(QObject* object)
{
3291 3292
    qCDebug(UASLog) << "removeLink:" << QString("%1").arg((ulong)object, 0, 16);
    qCDebug(UASLog) << "link count:" << links.count();
3293

3294 3295
    // Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already
    // been destroyed.
3296

Don Gagne's avatar
Don Gagne committed
3297
    LinkInterface* link = (LinkInterface*)object;
3298

3299
    int index = links.indexOf(link);
Don Gagne's avatar
Don Gagne committed
3300
    Q_ASSERT(index != -1);
3301
    links.removeAt(index);
3302 3303 3304 3305 3306
}

/**
* @return the list of links
*/
3307
QList<LinkInterface*> UAS::getLinks()
3308 3309 3310 3311 3312 3313 3314 3315 3316 3317 3318 3319 3320 3321 3322 3323 3324 3325 3326 3327 3328 3329 3330 3331 3332 3333 3334 3335 3336 3337
{
    return links;
}

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

/**
* Set the battery type and the  number of cells.
* @param type of the battery
* @param cells Number of cells.
*/
void UAS::setBattery(BatteryType type, int cells)
{
    this->batteryType = type;
    this->cells = cells;
    switch (batteryType)
    {
    case NICD:
        break;
    case NIMH:
        break;
    case LIION:
        break;
    case LIPOLY:
3338 3339
        fullVoltage = this->cells * lipoFull;
        emptyVoltage = this->cells * lipoEmpty;
3340 3341 3342 3343 3344 3345 3346 3347 3348 3349 3350 3351 3352 3353 3354 3355 3356 3357 3358 3359 3360 3361 3362 3363 3364 3365 3366
        break;
    case LIFE:
        break;
    case AGZN:
        break;
    }
}

/**
* Set the battery specificaitons: empty voltage, warning voltage, and full voltage.
* @param specifications of the battery
*/
void UAS::setBatterySpecs(const QString& specs)
{
    if (specs.length() == 0 || specs.contains("%"))
    {
        batteryRemainingEstimateEnabled = false;
        bool ok;
        QString percent = specs;
        percent = percent.remove("%");
        float temp = percent.toFloat(&ok);
        if (ok)
        {
            warnLevelPercent = temp;
        }
        else
        {
3367
            emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong");
3368 3369 3370 3371 3372 3373 3374 3375 3376 3377 3378 3379 3380 3381 3382 3383 3384 3385 3386 3387 3388 3389 3390 3391 3392
        }
    }
    else
    {
        batteryRemainingEstimateEnabled = true;
        QString stringList = specs;
        stringList = stringList.remove("V");
        stringList = stringList.remove("v");
        QStringList parts = stringList.split(",");
        if (parts.length() == 3)
        {
            float temp;
            bool ok;
            // Get the empty voltage
            temp = parts.at(0).toFloat(&ok);
            if (ok) emptyVoltage = temp;
            // Get the warning voltage
            temp = parts.at(1).toFloat(&ok);
            if (ok) warnVoltage = temp;
            // Get the full voltage
            temp = parts.at(2).toFloat(&ok);
            if (ok) fullVoltage = temp;
        }
        else
        {
3393
            emit textMessageReceived(0, 0, MAV_SEVERITY_WARNING, "Could not set battery options, format is wrong");
3394 3395 3396 3397 3398 3399 3400 3401 3402 3403 3404 3405 3406 3407 3408 3409 3410 3411 3412 3413 3414 3415 3416 3417 3418 3419 3420 3421 3422 3423 3424 3425 3426 3427 3428 3429 3430 3431 3432 3433 3434 3435 3436 3437 3438 3439 3440 3441 3442 3443 3444 3445 3446 3447 3448 3449 3450 3451 3452 3453 3454 3455 3456 3457 3458 3459 3460 3461 3462 3463 3464 3465 3466 3467 3468 3469
        }
    }
}

/**
* @return the battery specifications(empty voltage, warning voltage, full voltage)
*/
QString UAS::getBatterySpecs()
{
    if (batteryRemainingEstimateEnabled)
    {
        return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage);
    }
    else
    {
        return QString("%1%").arg(warnLevelPercent);
    }
}

/**
* @return the time remaining.
*/
int UAS::calculateTimeRemaining()
{
    quint64 dt = QGC::groundTimeMilliseconds() - startTime;
    double seconds = dt / 1000.0f;
    double voltDifference = startVoltage - currentVoltage;
    if (voltDifference <= 0) voltDifference = 0.00000000001f;
    double dischargePerSecond = voltDifference / seconds;
    int remaining = static_cast<int>((currentVoltage - emptyVoltage) / dischargePerSecond);
    // Can never be below 0
    if (remaining < 0) remaining = 0;
    return remaining;
}

/**
 * @return charge level in percent - 0 - 100
 */
float UAS::getChargeLevel()
{
    if (batteryRemainingEstimateEnabled)
    {
        if (lpVoltage < emptyVoltage)
        {
            chargeLevel = 0.0f;
        }
        else if (lpVoltage > fullVoltage)
        {
            chargeLevel = 100.0f;
        }
        else
        {
            chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
        }
    }
    return chargeLevel;
}

void UAS::startLowBattAlarm()
{
    if (!lowBattAlarm)
    {
        GAudioOutput::instance()->alert(tr("system %1 has low battery").arg(getUASName()));
        QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency()));
        lowBattAlarm = true;
    }
}

void UAS::stopLowBattAlarm()
{
    if (lowBattAlarm)
    {
        GAudioOutput::instance()->stopEmergency();
        lowBattAlarm = false;
    }
}
3470

3471
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
3472 3473 3474
{
    mavlink_message_t message;

3475 3476 3477 3478 3479 3480 3481 3482 3483 3484
    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];
        }
    }

3485 3486 3487 3488 3489
    mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
                                  mavlink->getComponentId(),
                                  &message,
                                  this->uasId,
                                  0,
3490
                                  param_id_cstr,
3491 3492
                                  -1,
                                  param_rc_channel_index,
3493 3494 3495 3496
                                  value0,
                                  scale,
                                  valueMin,
                                  valueMax);
3497 3498 3499
    sendMessage(message);
    qDebug() << "Mavlink message sent";
}
3500

3501 3502
void UAS::unsetRCToParameterMap()
{
3503 3504
    char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};

3505 3506 3507 3508 3509 3510 3511
    for (int i = 0; i < 3; i++) {
        mavlink_message_t message;
        mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
                                      mavlink->getComponentId(),
                                      &message,
                                      this->uasId,
                                      0,
3512
                                      param_id_cstr,
3513 3514 3515
                                      -2,
                                      i,
                                      0.0f,
3516 3517
                                      0.0f,
                                      0.0f,
3518 3519 3520 3521
                                      0.0f);
        sendMessage(message);
    }
}