UAS.cc 81.8 KB
Newer Older
1
/*===================================================================
pixhawk's avatar
pixhawk committed
2 3 4 5 6 7 8 9 10 11 12 13 14
======================================================================*/

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

#include <QList>
#include <QMessageBox>
#include <QTimer>
15
#include <QSettings>
pixhawk's avatar
pixhawk committed
16 17 18
#include <iostream>
#include <QDebug>
#include <cmath>
19
#include <qmath.h>
pixhawk's avatar
pixhawk committed
20 21 22
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
23
#include "QGC.h"
pixhawk's avatar
pixhawk committed
24
#include "GAudioOutput.h"
25
#include "MAVLinkProtocol.h"
pixhawk's avatar
pixhawk committed
26
#include "QGCMAVLink.h"
27 28
#include "LinkManager.h"
#include "SerialLink.h"
pixhawk's avatar
pixhawk committed
29

30
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77
    uasId(id),
    startTime(QGC::groundTimeMilliseconds()),
    commStatus(COMM_DISCONNECTED),
    name(""),
    autopilot(-1),
    links(new QList<LinkInterface*>()),
    unknownPackets(),
    mavlink(protocol),
    waypointManager(*this),
    thrustSum(0),
    thrustMax(10),
    startVoltage(0),
    warnVoltage(9.5f),
    warnLevelPercent(20.0f),
    currentVoltage(12.0f),
    lpVoltage(12.0f),
    batteryRemainingEstimateEnabled(false),
    mode(-1),
    status(-1),
    navMode(-1),
    onboardTimeOffset(0),
    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),
    receiveDropRate(0),
    sendDropRate(0),
    lowBattAlarm(false),
    positionLock(false),
    localX(0.0),
    localY(0.0),
    localZ(0.0),
    latitude(0.0),
    longitude(0.0),
    altitude(0.0),
    roll(0.0),
    pitch(0.0),
    yaw(0.0),
    statusTimeout(new QTimer(this)),
    paramsOnceRequested(false),
    airframe(0),
    attitudeKnown(false),
    paramManager(NULL)
pixhawk's avatar
pixhawk committed
78
{
79
    color = UASInterface::getNextColor();
pixhawk's avatar
pixhawk committed
80
    setBattery(LIPOLY, 3);
81
    connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
82
    connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings()));
83
    statusTimeout->start(500);
84
    readSettings();
pixhawk's avatar
pixhawk committed
85 86 87 88
}

UAS::~UAS()
{
89
    writeSettings();
pixhawk's avatar
pixhawk committed
90
    delete links;
91
    links=NULL;
pixhawk's avatar
pixhawk committed
92 93
}

94 95 96 97 98
void UAS::writeSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    settings.setValue("NAME", this->name);
99 100
    settings.setValue("AIRFRAME", this->airframe);
    settings.setValue("AP_TYPE", this->autopilot);
101
    settings.setValue("BATTERY_SPECS", getBatterySpecs());
102 103 104 105 106 107 108 109 110
    settings.endGroup();
    settings.sync();
}

void UAS::readSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    this->name = settings.value("NAME", this->name).toString();
111 112
    this->airframe = settings.value("AIRFRAME", this->airframe).toInt();
    this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt();
113
    if (settings.contains("BATTERY_SPECS")) {
114 115
        setBatterySpecs(settings.value("BATTERY_SPECS").toString());
    }
116 117 118
    settings.endGroup();
}

119
int UAS::getUASID() const
pixhawk's avatar
pixhawk committed
120 121 122 123
{
    return uasId;
}

124 125
void UAS::updateState()
{
126 127
    // Check if heartbeat timed out
    quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat;
128
    if (heartbeatInterval > timeoutIntervalHeartbeat) {
129 130 131 132
        emit heartbeatTimeout(heartbeatInterval);
        emit heartbeatTimeout();
    }

133 134
    // Position lock is set by the MAVLink message handler
    // if no position lock is available, indicate an error
135
    if (positionLock) {
136
        positionLock = false;
137 138
    } else {
        if (mode > (uint8_t)MAV_MODE_LOCKED && positionLock) {
139 140 141 142 143
            GAudioOutput::instance()->notifyNegative();
        }
    }
}

pixhawk's avatar
pixhawk committed
144 145
void UAS::setSelected()
{
146
    if (UASManager::instance()->getActiveUAS() != this) {
147 148 149 150 151 152 153 154
        UASManager::instance()->setActiveUAS(this);
        emit systemSelected(true);
    }
}

bool UAS::getSelected() const
{
    return (UASManager::instance()->getActiveUAS() == this);
pixhawk's avatar
pixhawk committed
155 156
}

157 158
void UAS::receiveMessageNamedValue(const mavlink_message_t& message)
{
159
    if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) {
pixhawk's avatar
pixhawk committed
160 161
        mavlink_named_value_float_t val;
        mavlink_msg_named_value_float_decode(&message, &val);
lm's avatar
lm committed
162 163
        QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN);
        emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime());
164
    } else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) {
pixhawk's avatar
pixhawk committed
165 166
        mavlink_named_value_int_t val;
        mavlink_msg_named_value_int_decode(&message, &val);
lm's avatar
lm committed
167 168
        QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN);
        emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime());
169 170 171
    }
}

pixhawk's avatar
pixhawk committed
172 173
void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
174
    if (!link) return;
175
    if (!links->contains(link)) {
pixhawk's avatar
pixhawk committed
176
        addLink(link);
177
        //        qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
pixhawk's avatar
pixhawk committed
178
    }
179 180 181 182
    //    else
    //    {
    //        qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST";
    //    }
pixhawk's avatar
pixhawk committed
183

184
    //    qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;
pixhawk's avatar
pixhawk committed
185

186
    if (message.sysid == uasId) {
pixhawk's avatar
pixhawk committed
187 188
        QString uasState;
        QString stateDescription;
pixhawk's avatar
pixhawk committed
189

190
        switch (message.msgid) {
pixhawk's avatar
pixhawk committed
191
        case MAVLINK_MSG_ID_HEARTBEAT:
192
            lastHeartbeat = QGC::groundTimeUsecs();
pixhawk's avatar
pixhawk committed
193 194
            emit heartbeat(this);
            // Set new type if it has changed
195
            if (this->type != mavlink_msg_heartbeat_get_type(&message)) {
pixhawk's avatar
pixhawk committed
196
                this->type = mavlink_msg_heartbeat_get_type(&message);
197 198
                if (airframe == 0) {
                    switch (type) {
199 200 201 202 203 204 205 206 207 208 209
                    case MAV_FIXED_WING:
                        setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR);
                        break;
                    case MAV_QUADROTOR:
                        setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
                        break;
                    default:
                        // Do nothing
                        break;
                    }
                }
210
                this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message);
pixhawk's avatar
pixhawk committed
211 212
                emit systemTypeSet(this, type);
            }
213

214 215 216 217 218
            break;
        case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
        case MAVLINK_MSG_ID_NAMED_VALUE_INT:
            // Receive named value message
            receiveMessageNamedValue(message);
pixhawk's avatar
pixhawk committed
219 220 221 222 223 224
            break;
        case MAVLINK_MSG_ID_BOOT:
            getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription);
            emit statusChanged(this, uasState, stateDescription);
            onboardTimeOffset = 0; // Reset offset measurement
            break;
225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246
        case MAVLINK_MSG_ID_SYS_STATUS: {
            mavlink_sys_status_t state;
            mavlink_msg_sys_status_decode(&message, &state);

            // FIXME
            //qDebug() << "1 SYSTEM STATUS:" << state.status;

            QString audiostring = "System " + getUASName();
            QString stateAudio = "";
            QString modeAudio = "";
            bool statechanged = false;
            bool modechanged = false;

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

                stateAudio = " changed status to " + uasState;
            }
247

248 249 250 251
            if (navMode != state.nav_mode) {
                emit navModeChanged(uasId, state.nav_mode, getNavModeText(state.nav_mode));
                navMode = state.nav_mode;
            }
lm's avatar
lm committed
252

253 254
            emit loadChanged(this,state.load/10.0f);
            emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime());
pixhawk's avatar
pixhawk committed
255

256 257 258 259
            if (this->mode != static_cast<int>(state.mode)) {
                modechanged = true;
                this->mode = static_cast<int>(state.mode);
                QString mode;
260

261 262 263 264 265 266 267
                switch (state.mode) {
                case (uint8_t)MAV_MODE_LOCKED:
                    mode = "LOCKED MODE";
                    break;
                case (uint8_t)MAV_MODE_MANUAL:
                    mode = "MANUAL MODE";
                    break;
268

269 270 271 272 273 274 275
#ifdef MAVLINK_ENABLED_SLUGS
                case (uint8_t)MAV_MODE_AUTO:
                    mode = "WAYPOINT MODE";
                    break;
                case (uint8_t)MAV_MODE_GUIDED:
                    mode = "MID-L CMDS MODE";
                    break;
276

277 278 279 280 281 282 283 284 285 286 287 288 289
                case (uint8_t)MAV_MODE_TEST1:
                    mode = "PASST MODE";
                    break;
                case (uint8_t)MAV_MODE_TEST2:
                    mode = "SEL PT MODE";
                    break;
#else
                case (uint8_t)MAV_MODE_AUTO:
                    mode = "AUTO MODE";
                    break;
                case (uint8_t)MAV_MODE_GUIDED:
                    mode = "GUIDED MODE";
                    break;
290

291 292 293 294 295 296 297 298 299
                case (uint8_t)MAV_MODE_TEST1:
                    mode = "TEST1 MODE";
                    break;
                case (uint8_t)MAV_MODE_TEST2:
                    mode = "TEST2 MODE";
                    break;
#endif
                case (uint8_t)MAV_MODE_READY:
                    mode = "READY MODE";
300 301
                    break;

302 303 304
                case (uint8_t)MAV_MODE_TEST3:
                    mode = "TEST3 MODE";
                    break;
pixhawk's avatar
pixhawk committed
305

306 307 308 309 310 311 312
                case (uint8_t)MAV_MODE_RC_TRAINING:
                    mode = "RC TRAINING MODE";
                    break;
                default:
                    mode = "UNINIT MODE";
                    break;
                }
313

314
                emit modeChanged(this->getUASID(), mode, "");
315

316
                //qDebug() << "2 SYSTEM MODE:" << mode;
lm's avatar
lm committed
317

318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336
                modeAudio = " is now in " + mode;
            }
            currentVoltage = state.vbat/1000.0f;
            lpVoltage = filterVoltage(currentVoltage);
            if (startVoltage == 0) startVoltage = currentVoltage;
            timeRemaining = calculateTimeRemaining();
            if (!batteryRemainingEstimateEnabled) {
                chargeLevel = state.battery_remaining/10.0f;
            }
            //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
            emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
            emit voltageChanged(message.sysid, state.vbat/1000.0f);

            // LOW BATTERY ALARM
            if (lpVoltage < warnVoltage) {
                startLowBattAlarm();
            } else {
                stopLowBattAlarm();
            }
337

338 339
            // COMMUNICATIONS DROP RATE
            emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f);
340 341


342 343
            //add for development
            //emit remoteControlRSSIChanged(state.packet_drop/1000.0f);
344

345 346 347
            //float en = state.packet_drop/1000.0f;
            //emit remoteControlChannelRawChanged(0, en);//MAVLINK_MSG_ID_RC_CHANNELS_RAW
            //emit remoteControlChannelScaledChanged(0, en/100.0f);//MAVLINK_MSG_ID_RC_CHANNELS_SCALED
348

lm's avatar
lm committed
349

350
            //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop;
351

352 353 354 355 356 357 358 359 360 361 362 363 364
            // AUDIO
            if (modechanged && statechanged) {
                // Output both messages
                audiostring += modeAudio + " and " + stateAudio;
            } else {
                // Output the one message
                audiostring += modeAudio + stateAudio;
            }
            if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY) {
                GAudioOutput::instance()->startEmergency();
            } else if (modechanged || statechanged) {
                GAudioOutput::instance()->stopEmergency();
                GAudioOutput::instance()->say(audiostring);
pixhawk's avatar
pixhawk committed
365
            }
James Goppert's avatar
James Goppert committed
366

367 368 369
            if (state.status == MAV_STATE_POWEROFF) {
                emit systemRemoved(this);
                emit systemRemoved();
370
            }
371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389
        }
        break;

#ifdef MAVLINK_ENABLED_PIXHAWK
        case MAVLINK_MSG_ID_CONTROL_STATUS: {
            mavlink_control_status_t status;
            mavlink_msg_control_status_decode(&message, &status);
            // Emit control status vector
            emit attitudeControlEnabled(static_cast<bool>(status.control_att));
            emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
            emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
            emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));

            // Emit localization status vector
            emit localizationChanged(this, status.position_fix);
            emit visionLocalizationChanged(this, status.vision_fix);
            emit gpsLocalizationChanged(this, status.gps_fix);
        }
        break;
lm's avatar
lm committed
390
#endif // PIXHAWK
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
        case MAVLINK_MSG_ID_RAW_IMU: {
            mavlink_raw_imu_t raw;
            mavlink_msg_raw_imu_decode(&message, &raw);
            quint64 time = getUnixTime(raw.usec);

            emit valueChanged(uasId, "accel x", "raw", static_cast<double>(raw.xacc), time);
            emit valueChanged(uasId, "accel y", "raw", static_cast<double>(raw.yacc), time);
            emit valueChanged(uasId, "accel z", "raw", static_cast<double>(raw.zacc), time);
            emit valueChanged(uasId, "gyro roll", "raw", static_cast<double>(raw.xgyro), time);
            emit valueChanged(uasId, "gyro pitch", "raw", static_cast<double>(raw.ygyro), time);
            emit valueChanged(uasId, "gyro yaw", "raw", static_cast<double>(raw.zgyro), time);
            emit valueChanged(uasId, "mag x", "raw", static_cast<double>(raw.xmag), time);
            emit valueChanged(uasId, "mag y", "raw", static_cast<double>(raw.ymag), time);
            emit valueChanged(uasId, "mag z", "raw", static_cast<double>(raw.zmag), time);
        }
        break;
        case MAVLINK_MSG_ID_SCALED_IMU: {
            mavlink_scaled_imu_t scaled;
            mavlink_msg_scaled_imu_decode(&message, &scaled);
            quint64 time = getUnixTime(scaled.usec);

            emit valueChanged(uasId, "accel x", "g", scaled.xacc/1000.0f, time);
            emit valueChanged(uasId, "accel y", "g", scaled.yacc/1000.0f, time);
            emit valueChanged(uasId, "accel z", "g", scaled.zacc/1000.0f, time);
            emit valueChanged(uasId, "gyro roll", "rad/s", scaled.xgyro/1000.0f, time);
            emit valueChanged(uasId, "gyro pitch", "rad/s", scaled.ygyro/1000.0f, time);
            emit valueChanged(uasId, "gyro yaw", "rad/s", scaled.zgyro/1000.0f, time);
            emit valueChanged(uasId, "mag x", "tesla", scaled.xmag/1000.0f, time);
            emit valueChanged(uasId, "mag y", "tesla", scaled.ymag/1000.0f, time);
            emit valueChanged(uasId, "mag z", "tesla", scaled.zmag/1000.0f, time);
        }
        break;
pixhawk's avatar
pixhawk committed
423 424
        case MAVLINK_MSG_ID_ATTITUDE:
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
425
            //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;
426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447
        {
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&message, &attitude);
            quint64 time = getUnixTime(attitude.usec);
            roll = QGC::limitAngleToPMPIf(attitude.roll);
            pitch = QGC::limitAngleToPMPIf(attitude.pitch);
            yaw = QGC::limitAngleToPMPIf(attitude.yaw);
            emit valueChanged(uasId, "roll", "rad", roll, time);
            emit valueChanged(uasId, "pitch", "rad", pitch, time);
            emit valueChanged(uasId, "yaw", "rad", yaw, time);
            emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time);
            emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time);
            emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time);

            // Emit in angles

            // Convert yaw angle to compass value
            // in 0 - 360 deg range
            float compass = (yaw/M_PI)*180.0+360.0f;
            while (compass > 360.0f) {
                compass -= 360.0f;
            }
lm's avatar
lm committed
448

449
            attitudeKnown = true;
lm's avatar
lm committed
450

451 452 453 454 455 456
            emit valueChanged(uasId, "roll deg", "deg", (roll/M_PI)*180.0, time);
            emit valueChanged(uasId, "pitch deg", "deg", (pitch/M_PI)*180.0, time);
            emit valueChanged(uasId, "heading deg", "deg", compass, time);
            emit valueChanged(uasId, "rollspeed d/s", "deg/s", (attitude.rollspeed/M_PI)*180.0, time);
            emit valueChanged(uasId, "pitchspeed d/s", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time);
            emit valueChanged(uasId, "yawspeed d/s", "deg/s", (attitude.yawspeed/M_PI)*180.0, time);
457

458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476
            emit attitudeChanged(this, roll, pitch, yaw, time);
            emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
        }
        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 valueChanged(uasId, "airspeed", "m/s", hud.airspeed, time);
            emit valueChanged(uasId, "groundspeed", "m/s", hud.groundspeed, time);
            emit valueChanged(uasId, "altitude", "m", hud.alt, time);
            emit valueChanged(uasId, "heading", "deg", hud.heading, time);
            emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time);
            emit valueChanged(uasId, "throttle", "%", hud.throttle, time);
            emit thrustChanged(this, hud.throttle/100.0);

            if (!attitudeKnown) {
                yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI);
477
                emit attitudeChanged(this, roll, pitch, yaw, time);
pixhawk's avatar
pixhawk committed
478
            }
lm's avatar
lm committed
479

480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500
            emit altitudeChanged(uasId, hud.alt);
            //yaw = (hud.heading-180.0f/360.0f)*M_PI;
            //emit attitudeChanged(this, roll, pitch, yaw, getUnixTime());
            emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime());
        }
        break;
        case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
            mavlink_nav_controller_output_t nav;
            mavlink_msg_nav_controller_output_decode(&message, &nav);
            quint64 time = getUnixTime();
            // Update UI
            emit valueChanged(uasId, "nav roll", "deg", nav.nav_roll, time);
            emit valueChanged(uasId, "nav pitch", "deg", nav.nav_pitch, time);
            emit valueChanged(uasId, "nav bearing", "deg", nav.nav_bearing, time);
            emit valueChanged(uasId, "target bearing", "deg", nav.target_bearing, time);
            emit valueChanged(uasId, "wp dist", "m", nav.wp_dist, time);
            emit valueChanged(uasId, "alt err", "m", nav.alt_error, time);
            emit valueChanged(uasId, "airspeed err", "m/s", nav.alt_error, time);
            emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time);
        }
        break;
501
        case MAVLINK_MSG_ID_LOCAL_POSITION:
lm's avatar
lm committed
502
            //std::cerr << std::endl;
pixhawk's avatar
pixhawk committed
503
            //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;
504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527
        {
            mavlink_local_position_t pos;
            mavlink_msg_local_position_decode(&message, &pos);
            quint64 time = getUnixTime(pos.usec);
            localX = pos.x;
            localY = pos.y;
            localZ = pos.z;
            emit valueChanged(uasId, "x", "m", pos.x, time);
            emit valueChanged(uasId, "y", "m", pos.y, time);
            emit valueChanged(uasId, "z", "m", pos.z, time);
            emit valueChanged(uasId, "x speed", "m/s", pos.vx, time);
            emit valueChanged(uasId, "y speed", "m/s", pos.vy, time);
            emit valueChanged(uasId, "z speed", "m/s", pos.vz, time);
            emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
            emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);

            //                qDebug()<<"Local Position = "<<pos.x<<" - "<<pos.y<<" - "<<pos.z;
            //                qDebug()<<"Speed Local Position = "<<pos.vx<<" - "<<pos.vy<<" - "<<pos.vz;

            //emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time);
            // Set internal state
            if (!positionLock) {
                // If position was not locked before, notify positive
                GAudioOutput::instance()->notifyPositive();
lm's avatar
lm committed
528
            }
529 530 531
            positionLock = true;
        }
        break;
532
        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
533 534
            //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;
535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555
        {
            mavlink_global_position_int_t pos;
            mavlink_msg_global_position_int_decode(&message, &pos);
            quint64 time = QGC::groundTimeUsecs()/1000;
            latitude = pos.lat/(double)1E7;
            longitude = pos.lon/(double)1E7;
            altitude = pos.alt/1000.0;
            speedX = pos.vx/100.0;
            speedY = pos.vy/100.0;
            speedZ = pos.vz/100.0;
            emit valueChanged(uasId, "latitude", "deg", latitude, time);
            emit valueChanged(uasId, "longitude", "deg", longitude, time);
            emit valueChanged(uasId, "altitude", "m", altitude, time);
            double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
            emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
            emit globalPositionChanged(this, latitude, longitude, altitude, time);
            emit speedChanged(this, speedX, speedY, speedZ, time);
            // Set internal state
            if (!positionLock) {
                // If position was not locked before, notify positive
                GAudioOutput::instance()->notifyPositive();
556
            }
557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582
            positionLock = true;
            //TODO fix this hack for forwarding of global position for patch antenna tracking
            forwardMessage(message);
        }
        break;
        case MAVLINK_MSG_ID_GLOBAL_POSITION: {
            mavlink_global_position_t pos;
            mavlink_msg_global_position_decode(&message, &pos);
            quint64 time = QGC::groundTimeUsecs()/1000;
            latitude = pos.lat;
            longitude = pos.lon;
            altitude = pos.alt;
            speedX = pos.vx;
            speedY = pos.vy;
            speedZ = pos.vz;
            emit valueChanged(uasId, "latitude", "deg", latitude, time);
            emit valueChanged(uasId, "longitude", "deg", longitude, time);
            emit valueChanged(uasId, "altitude", "m", altitude, time);
            double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
            emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
            emit globalPositionChanged(this, latitude, longitude, altitude, time);
            emit speedChanged(this, speedX, speedY, speedZ, time);
            // Set internal state
            if (!positionLock) {
                // If position was not locked before, notify positive
                GAudioOutput::instance()->notifyPositive();
583
            }
584 585 586 587 588
            positionLock = true;
            //TODO fix this hack for forwarding of global position for patch antenna tracking
            forwardMessage(message);
        }
        break;
589 590 591
        case MAVLINK_MSG_ID_GPS_RAW:
            //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;
592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616
        {
            mavlink_gps_raw_t pos;
            mavlink_msg_gps_raw_decode(&message, &pos);

            // SANITY CHECK
            // only accept values in a realistic range
            // quint64 time = getUnixTime(pos.usec);
            quint64 time = getUnixTime();

            emit valueChanged(uasId, "latitude", "deg", pos.lat, time);
            emit valueChanged(uasId, "longitude", "deg", pos.lon, time);

            if (pos.fix_type > 0) {
                emit globalPositionChanged(this, pos.lat, pos.lon, pos.alt, time);
                emit valueChanged(uasId, "gps speed", "m/s", pos.v, time);
                latitude = pos.lat;
                longitude = pos.lon;
                altitude = pos.alt;
                positionLock = true;

                // Check for NaN
                int alt = pos.alt;
                if (alt != alt) {
                    alt = 0;
                    emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
617
                }
618 619 620 621 622 623 624 625
                emit valueChanged(uasId, "altitude", "m", pos.alt, time);
                // Smaller than threshold and not NaN
                if (pos.v < 1000000 && pos.v == pos.v) {
                    emit valueChanged(uasId, "speed", "m/s", pos.v, time);
                    //qDebug() << "GOT GPS RAW";
                    // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
                } else {
                    emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
626 627
                }
            }
628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663
        }
        break;
        case MAVLINK_MSG_ID_GPS_RAW_INT: {
            mavlink_gps_raw_int_t pos;
            mavlink_msg_gps_raw_int_decode(&message, &pos);

            // SANITY CHECK
            // only accept values in a realistic range
            // quint64 time = getUnixTime(pos.usec);
            quint64 time = getUnixTime();

            emit valueChanged(uasId, "latitude", "deg", pos.lat/(double)1E7, time);
            emit valueChanged(uasId, "longitude", "deg", pos.lon/(double)1E7, time);

            if (pos.fix_type > 0) {
                emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
                emit valueChanged(uasId, "gps speed", "m/s", pos.v, time);
                latitude = pos.lat/(double)1E7;
                longitude = pos.lon/(double)1E7;
                altitude = pos.alt/1000.0;
                positionLock = true;

                // Check for NaN
                int alt = pos.alt;
                if (alt != alt) {
                    alt = 0;
                    emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
                }
                emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                // Smaller than threshold and not NaN
                if (pos.v < 1000000 && pos.v == pos.v) {
                    emit valueChanged(uasId, "speed", "m/s", pos.v, time);
                    //qDebug() << "GOT GPS RAW";
                    // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
                } else {
                    emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
lm's avatar
lm committed
664 665
                }
            }
666 667 668 669 670 671 672
        }
        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]));
673
            }
674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691
        }
        break;
        case MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET: {
            mavlink_gps_local_origin_set_t pos;
            mavlink_msg_gps_local_origin_set_decode(&message, &pos);
            // FIXME Emit to other components
        }
        break;
        case MAVLINK_MSG_ID_RAW_PRESSURE: {
            mavlink_raw_pressure_t pressure;
            mavlink_msg_raw_pressure_decode(&message, &pressure);
            quint64 time = this->getUnixTime(pressure.usec);
            emit valueChanged(uasId, "abs pressure", "raw", pressure.press_abs, time);
            emit valueChanged(uasId, "diff pressure 1", "raw", pressure.press_diff1, time);
            emit valueChanged(uasId, "diff pressure 2", "raw", pressure.press_diff2, time);
            emit valueChanged(uasId, "temperature", "raw", pressure.temperature, time);
        }
        break;
692

James Goppert's avatar
James Goppert committed
693 694 695 696 697 698 699 700 701
        case MAVLINK_MSG_ID_SCALED_PRESSURE: {
            mavlink_scaled_pressure_t pressure;
            mavlink_msg_scaled_pressure_decode(&message, &pressure);
            quint64 time = this->getUnixTime(pressure.usec);
            emit valueChanged(uasId, "abs pressure", "hPa", pressure.press_abs, time);
            emit valueChanged(uasId, "diff pressure", "hPa", pressure.press_diff, time);
            emit valueChanged(uasId, "temperature", "C", pressure.temperature/100.0, time);
        }
        break;
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
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
            mavlink_rc_channels_raw_t channels;
            mavlink_msg_rc_channels_raw_decode(&message, &channels);
            emit remoteControlRSSIChanged(channels.rssi/255.0f);
            emit remoteControlChannelRawChanged(0, channels.chan1_raw);
            emit remoteControlChannelRawChanged(1, channels.chan2_raw);
            emit remoteControlChannelRawChanged(2, channels.chan3_raw);
            emit remoteControlChannelRawChanged(3, channels.chan4_raw);
            emit remoteControlChannelRawChanged(4, channels.chan5_raw);
            emit remoteControlChannelRawChanged(5, channels.chan6_raw);
            emit remoteControlChannelRawChanged(6, channels.chan7_raw);
            emit remoteControlChannelRawChanged(7, channels.chan8_raw);
        }
        break;
        case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
            mavlink_rc_channels_scaled_t channels;
            mavlink_msg_rc_channels_scaled_decode(&message, &channels);
            emit remoteControlRSSIChanged(channels.rssi/255.0f);
            emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f);
            emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f);
        }
        break;
        case MAVLINK_MSG_ID_PARAM_VALUE: {
            mavlink_param_value_t value;
            mavlink_msg_param_value_decode(&message, &value);
            QByteArray bytes((char*)value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
            QString parameterName = QString(bytes);
            int component = message.compid;
            float val = value.param_value;

            // Insert component if necessary
            if (!parameters.contains(component)) {
                parameters.insert(component, new QMap<QString, float>());
lm's avatar
lm committed
742
            }
743

744 745 746
            // Insert parameter into registry
            if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName);
            parameters.value(component)->insert(parameterName, val);
747

748 749 750 751 752
            // Emit change
            emit parameterChanged(uasId, message.compid, parameterName, val);
            emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val);
        }
        break;
lm's avatar
lm committed
753 754 755
        case MAVLINK_MSG_ID_ACTION_ACK:
            mavlink_action_ack_t ack;
            mavlink_msg_action_ack_decode(&message, &ack);
756
            if (ack.result == 1) {
lm's avatar
lm committed
757
                emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action));
758
            } else {
lm's avatar
lm committed
759
                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action));
760 761
            }
            break;
pixhawk's avatar
pixhawk committed
762
        case MAVLINK_MSG_ID_DEBUG:
763
            emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow());
pixhawk's avatar
pixhawk committed
764
            break;
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
        case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: {
            mavlink_attitude_controller_output_t out;
            mavlink_msg_attitude_controller_output_decode(&message, &out);
            quint64 time = MG::TIME::getGroundTimeNowUsecs();
            emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time);
            emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f);
            emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f);
            emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f);
        }
        break;
        case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: {
            mavlink_position_controller_output_t out;
            mavlink_msg_position_controller_output_decode(&message, &out);
            quint64 time = MG::TIME::getGroundTimeNow();
            //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
            emit valueChanged(uasId, "pos control x", "raw", out.x, time);
            emit valueChanged(uasId, "pos control y", "raw", out.y, time);
            emit valueChanged(uasId, "pos control z", "raw", out.z, time);
        }
        break;
        case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
            mavlink_waypoint_count_t wpc;
            mavlink_msg_waypoint_count_decode(&message, &wpc);
            if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId()) {
                waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
pixhawk's avatar
pixhawk committed
790
            }
791 792
        }
        break;
pixhawk's avatar
pixhawk committed
793

794 795 796 797 798 799
        case MAVLINK_MSG_ID_WAYPOINT: {
            mavlink_waypoint_t wp;
            mavlink_msg_waypoint_decode(&message, &wp);
            //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
            if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId()) {
                waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
800
            }
801 802
        }
        break;
pixhawk's avatar
pixhawk committed
803

804 805 806 807 808
        case MAVLINK_MSG_ID_WAYPOINT_ACK: {
            mavlink_waypoint_ack_t wpa;
            mavlink_msg_waypoint_ack_decode(&message, &wpa);
            if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) {
                waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
809
            }
810 811
        }
        break;
812

813 814 815 816 817
        case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
            mavlink_waypoint_request_t wpr;
            mavlink_msg_waypoint_request_decode(&message, &wpr);
            if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId()) {
                waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
pixhawk's avatar
pixhawk committed
818
            }
819 820
        }
        break;
pixhawk's avatar
pixhawk committed
821

822 823 824 825 826 827 828 829 830
        case MAVLINK_MSG_ID_WAYPOINT_REACHED: {
            mavlink_waypoint_reached_t wpr;
            mavlink_msg_waypoint_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);
            emit textMessageReceived(message.sysid, message.compid, 0, text);
        }
        break;
pixhawk's avatar
pixhawk committed
831

832 833 834 835 836 837
        case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
            mavlink_waypoint_current_t wpc;
            mavlink_msg_waypoint_current_decode(&message, &wpc);
            waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
        }
        break;
pixhawk's avatar
pixhawk committed
838

839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870
        case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: {
            mavlink_local_position_setpoint_t p;
            mavlink_msg_local_position_setpoint_decode(&message, &p);
            emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
        }
        break;
        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
            mavlink_servo_output_raw_t servos;
            mavlink_msg_servo_output_raw_decode(&message, &servos);
            quint64 time = getUnixTime(0);
            emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time);
            emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time);
            emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time);
            emit valueChanged(uasId, "servo #4", "us", servos.servo4_raw, time);
            emit valueChanged(uasId, "servo #5", "us", servos.servo5_raw, time);
            emit valueChanged(uasId, "servo #6", "us", servos.servo6_raw, time);
            emit valueChanged(uasId, "servo #7", "us", servos.servo7_raw, time);
            emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time);
        }
        break;
        case MAVLINK_MSG_ID_STATUSTEXT: {
            QByteArray b;
            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
            mavlink_msg_statustext_get_text(&message, (int8_t*)b.data());
            //b.append('\0');
            QString text = QString(b);
            int severity = mavlink_msg_statustext_get_severity(&message);
            //qDebug() << "RECEIVED STATUS:" << text;false
            //emit statusTextReceived(severity, text);
            emit textMessageReceived(uasId, message.compid, severity, text);
        }
        break;
871
#ifdef MAVLINK_ENABLED_PIXHAWK
872 873 874 875 876 877 878 879 880 881 882
        case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: {
            qDebug() << "RECIEVED ACK TO GET IMAGE";
            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;
            imageStart = QGC::groundTimeMilliseconds();
        }
        break;
883

884 885 886 887 888 889 890 891 892
        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;

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

897
            ++imagePacketsArrived;
898

899 900 901 902 903 904
            // emit signal if all packets arrived
            if ((imagePacketsArrived == imagePackets)) {
                image.loadFromData(imageRecBuffer);
                emit imageReady(this);
                // Restart statemachine
                imagePacketsArrived = 0;
905

906 907
                //this->requestImage();
                //qDebug() << "SENDING REQUEST TO GET NEW IMAGE FROM SYSTEM" << uasId;
908
            }
909 910
        }
        break;
911
#endif
912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937
        case MAVLINK_MSG_ID_DEBUG_VECT: {
            mavlink_debug_vect_t vect;
            mavlink_msg_debug_vect_decode(&message, &vect);
            QString str((const char*)vect.name);
            quint64 time = getUnixTime(vect.usec);
            emit valueChanged(uasId, str+".x", "raw", vect.x, time);
            emit valueChanged(uasId, str+".y", "raw", vect.y, time);
            emit valueChanged(uasId, str+".z", "raw", vect.z, time);
        }
        break;
        //#ifdef MAVLINK_ENABLED_PIXHAWK
        //            case MAVLINK_MSG_ID_POINT_OF_INTEREST:
        //            {
        //                mavlink_point_of_interest_t poi;
        //                mavlink_msg_point_of_interest_decode(&message, &poi);
        //                emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z);
        //            }
        //            break;
        //            case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION:
        //            {
        //                mavlink_point_of_interest_connection_t poi;
        //                mavlink_msg_point_of_interest_connection_decode(&message, &poi);
        //                emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2);
        //            }
        //            break;
        //#endif
lm's avatar
lm committed
938
#ifdef MAVLINK_ENABLED_UALBERTA
939 940 941 942 943 944 945 946 947 948 949 950 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
        case MAVLINK_MSG_ID_NAV_FILTER_BIAS: {
            mavlink_nav_filter_bias_t bias;
            mavlink_msg_nav_filter_bias_decode(&message, &bias);
            quint64 time = MG::TIME::getGroundTimeNow();
            emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time);
            emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time);
            emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time);
            emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time);
            emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time);
            emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time);
        }
        break;
        case MAVLINK_MSG_ID_RADIO_CALIBRATION: {
            mavlink_radio_calibration_t radioMsg;
            mavlink_msg_radio_calibration_decode(&message, &radioMsg);
            QVector<float> aileron;
            QVector<float> elevator;
            QVector<float> rudder;
            QVector<float> gyro;
            QVector<float> pitch;
            QVector<float> throttle;

            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN; ++i)
                aileron << radioMsg.aileron[i];
            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN; ++i)
                elevator << radioMsg.elevator[i];
            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN; ++i)
                rudder << radioMsg.rudder[i];
            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN; ++i)
                gyro << radioMsg.gyro[i];
            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN; ++i)
                pitch << radioMsg.pitch[i];
            for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN; ++i)
                throttle << radioMsg.throttle[i];

            QPointer<RadioCalibrationData> radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle);
            emit radioCalibrationReceived(radioData);
            delete radioData;
        }
        break;
979

980
#endif
981 982
        // Messages to ignore
        case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
983
            break;
984 985 986 987 988 989 990 991
        default: {
            if (!unknownPackets.contains(message.msgid)) {
                unknownPackets.append(message.msgid);
                QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid);
                GAudioOutput::instance()->say(errString+tr(", please check the communication console for details."));
                emit textMessageReceived(uasId, message.compid, 255, errString);
                std::cout << "Unable to decode message from system " << std::dec << static_cast<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
lm's avatar
lm committed
992
            }
993 994
        }
        break;
pixhawk's avatar
pixhawk committed
995 996 997 998
        }
    }
}

999 1000 1001 1002 1003 1004 1005 1006 1007
void UAS::setHomePosition(double lat, double lon, double alt)
{
    // Send new home position to UAS
    mavlink_gps_set_global_origin_t home;
    home.target_system = uasId;
    home.target_component = 0; // ALL components
    home.latitude = lat*1E7;
    home.longitude = lon*1E7;
    home.altitude = alt*1000;
1008
    qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
1009 1010 1011 1012 1013
    mavlink_message_t msg;
    mavlink_msg_gps_set_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
    sendMessage(msg);
}

1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028
void UAS::setLocalOriginAtCurrentGPSPosition()
{
    bool result = false;
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Warning);
    msgBox.setText("Setting new World Coordinate Frame Origin");
    msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
    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()));


1029
    if (ret == QMessageBox::Yes) {
1030
        mavlink_message_t msg;
1031
        mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN);
1032 1033 1034 1035 1036 1037 1038 1039 1040 1041
        // Send message twice to increase chance that it reaches its goal
        sendMessage(msg);
        // Wait 5 ms
        MG::SLEEP::usleep(5000);
        // Send again
        sendMessage(msg);
        result = true;
    }
}

pixhawk's avatar
pixhawk committed
1042 1043
void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
{
1044
#ifdef MAVLINK_ENABLED_PIXHAWK
pixhawk's avatar
pixhawk committed
1045
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1046 1047
    mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw);
    sendMessage(msg);
1048
#else
lm's avatar
lm committed
1049 1050 1051 1052
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
1053
#endif
pixhawk's avatar
pixhawk committed
1054 1055
}

pixhawk's avatar
pixhawk committed
1056 1057
void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
{
lm's avatar
lm committed
1058
#ifdef MAVLINK_ENABLED_PIXHAWK
1059 1060 1061
    mavlink_message_t msg;
    mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw);
    sendMessage(msg);
lm's avatar
lm committed
1062
#else
1063 1064 1065 1066
    Q_UNUSED(x);
    Q_UNUSED(y);
    Q_UNUSED(z);
    Q_UNUSED(yaw);
pixhawk's avatar
pixhawk committed
1067 1068 1069 1070 1071
#endif
}

void UAS::startRadioControlCalibration()
{
1072 1073 1074
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1075 1076
}

lm's avatar
lm committed
1077 1078
void UAS::startDataRecording()
{
1079 1080 1081
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START);
    sendMessage(msg);
lm's avatar
lm committed
1082 1083 1084 1085
}

void UAS::pauseDataRecording()
{
1086 1087 1088
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE);
    sendMessage(msg);
lm's avatar
lm committed
1089 1090 1091 1092
}

void UAS::stopDataRecording()
{
1093 1094 1095
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP);
    sendMessage(msg);
lm's avatar
lm committed
1096 1097
}

pixhawk's avatar
pixhawk committed
1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118
void UAS::startMagnetometerCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG);
    sendMessage(msg);
}

void UAS::startGyroscopeCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO);
    sendMessage(msg);
}

void UAS::startPressureCalibration()
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE);
    sendMessage(msg);
}

1119 1120
quint64 UAS::getUnixTime(quint64 time)
{
1121
    if (time == 0) {
1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139
        return MG::TIME::getGroundTimeNow();
    }
    // 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
1140
#ifndef _MSC_VER
1141
    else if (time < 1261440000000000LLU)
1142
#else
1143
    else if (time < 1261440000000000)
1144
#endif
1145 1146
    {
        if (onboardTimeOffset == 0) {
1147
            onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000;
1148
        }
1149
        return time/1000 + onboardTimeOffset;
1150
    } else {
1151 1152
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
1153
        return time/1000;
1154 1155 1156
    }
}

1157 1158
QList<QString> UAS::getParameterNames(int component)
{
1159
    if (parameters.contains(component)) {
1160
        return parameters.value(component)->keys();
1161
    } else {
1162 1163 1164 1165 1166 1167 1168 1169 1170
        return QList<QString>();
    }
}

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

pixhawk's avatar
pixhawk committed
1171
void UAS::setMode(int mode)
pixhawk's avatar
pixhawk committed
1172
{
1173
    if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) {
1174
        //this->mode = mode; //no call assignament, update receive message from UAS
pixhawk's avatar
pixhawk committed
1175
        mavlink_message_t msg;
lm's avatar
lm committed
1176
        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode);
pixhawk's avatar
pixhawk committed
1177
        sendMessage(msg);
lm's avatar
lm committed
1178
        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
1179
    } else {
1180 1181
        qDebug() << "uas Mode not assign: " << mode;
    }
pixhawk's avatar
pixhawk committed
1182 1183 1184 1185 1186
}

void UAS::sendMessage(mavlink_message_t message)
{
    // Emit message on all links that are currently connected
1187 1188
    foreach (LinkInterface* link, *links) {
        if (link) {
1189
            sendMessage(link, message);
1190
        } else {
1191 1192 1193
            // Remove from list
            links->removeAt(links->indexOf(link));
        }
pixhawk's avatar
pixhawk committed
1194 1195 1196
    }
}

1197 1198 1199 1200
void UAS::forwardMessage(mavlink_message_t message)
{
    // Emit message on all links that are currently connected
    QList<LinkInterface*>link_list = LinkManager::instance()->getLinksForProtocol(mavlink);
1201

1202 1203
    foreach(LinkInterface* link, link_list) {
        if (link) {
1204
            SerialLink* serial = dynamic_cast<SerialLink*>(link);
1205
            if(serial != 0) {
1206

1207 1208
                for(int i=0; i<links->size(); i++) {
                    if(serial != links->at(i)) {
1209
                        qDebug()<<"Antenna tracking: Forwarding Over link: "<<serial->getName()<<" "<<serial;
1210 1211
                        sendMessage(serial, message);
                    }
1212 1213 1214 1215 1216 1217
                }
            }
        }
    }
}

pixhawk's avatar
pixhawk committed
1218 1219
void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
{
1220
    if(!link) return;
pixhawk's avatar
pixhawk committed
1221 1222 1223
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
1224
    int len = mavlink_msg_to_send_buffer(buffer, &message);
1225
    mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getId(), message.len);
pixhawk's avatar
pixhawk committed
1226
    // If link is connected
1227
    if (link->isConnected()) {
pixhawk's avatar
pixhawk committed
1228 1229 1230 1231 1232 1233 1234 1235
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * @param value battery voltage
 */
1236
float UAS::filterVoltage(float value) const
pixhawk's avatar
pixhawk committed
1237
{
1238
    return lpVoltage * 0.7f + value * 0.3f;
pixhawk's avatar
pixhawk committed
1239 1240
}

1241 1242
QString UAS::getNavModeText(int mode)
{
1243
    switch (mode) {
1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275
    case MAV_NAV_GROUNDED:
        return QString("GROUNDED");
        break;
    case MAV_NAV_HOLD:
        return QString("HOLD");
        break;
    case MAV_NAV_LANDING:
        return QString("LANDING");
        break;
    case MAV_NAV_LIFTOFF:
        return QString("LIFTOFF");
        break;
    case MAV_NAV_LOITER:
        return QString("LOITER");
        break;
    case MAV_NAV_LOST:
        return QString("LOST");
        break;
    case MAV_NAV_RETURNING:
        return QString("RETURNING");
        break;
    case MAV_NAV_VECTOR:
        return QString("VECTOR");
        break;
    case MAV_NAV_WAYPOINT:
        return QString("WAYPOINT");
        break;
    default:
        return QString("UNKNOWN");
    }
}

pixhawk's avatar
pixhawk committed
1276 1277
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
1278
    switch (statusCode) {
lm's avatar
lm committed
1279
    case MAV_STATE_UNINIT:
pixhawk's avatar
pixhawk committed
1280
        uasState = tr("UNINIT");
1281
        stateDescription = tr("Unitialized, booting up.");
pixhawk's avatar
pixhawk committed
1282
        break;
lm's avatar
lm committed
1283
    case MAV_STATE_BOOT:
pixhawk's avatar
pixhawk committed
1284
        uasState = tr("BOOT");
1285
        stateDescription = tr("Booting system, please wait.");
pixhawk's avatar
pixhawk committed
1286
        break;
lm's avatar
lm committed
1287
    case MAV_STATE_CALIBRATING:
pixhawk's avatar
pixhawk committed
1288
        uasState = tr("CALIBRATING");
1289
        stateDescription = tr("Calibrating sensors, please wait.");
pixhawk's avatar
pixhawk committed
1290
        break;
lm's avatar
lm committed
1291
    case MAV_STATE_ACTIVE:
pixhawk's avatar
pixhawk committed
1292
        uasState = tr("ACTIVE");
1293
        stateDescription = tr("Active, normal operation.");
pixhawk's avatar
pixhawk committed
1294
        break;
lm's avatar
lm committed
1295 1296
    case MAV_STATE_STANDBY:
        uasState = tr("STANDBY");
1297
        stateDescription = tr("Standby mode, ready for liftoff.");
lm's avatar
lm committed
1298 1299
        break;
    case MAV_STATE_CRITICAL:
pixhawk's avatar
pixhawk committed
1300
        uasState = tr("CRITICAL");
1301
        stateDescription = tr("FAILURE: Continuing operation.");
pixhawk's avatar
pixhawk committed
1302
        break;
lm's avatar
lm committed
1303
    case MAV_STATE_EMERGENCY:
pixhawk's avatar
pixhawk committed
1304
        uasState = tr("EMERGENCY");
1305
        stateDescription = tr("EMERGENCY: Land Immediately!");
pixhawk's avatar
pixhawk committed
1306
        break;
James Goppert's avatar
James Goppert committed
1307
        //case MAV_STATE_HILSIM:
James Goppert's avatar
James Goppert committed
1308 1309 1310
        //uasState = tr("HIL SIM");
        //stateDescription = tr("HIL Simulation, Sensors read from SIM");
        //break;
1311

lm's avatar
lm committed
1312
    case MAV_STATE_POWEROFF:
pixhawk's avatar
pixhawk committed
1313
        uasState = tr("SHUTDOWN");
1314
        stateDescription = tr("Powering off system.");
pixhawk's avatar
pixhawk committed
1315
        break;
1316

lm's avatar
lm committed
1317
    default:
pixhawk's avatar
pixhawk committed
1318
        uasState = tr("UNKNOWN");
1319
        stateDescription = tr("Unknown system state");
pixhawk's avatar
pixhawk committed
1320 1321 1322 1323
        break;
    }
}

1324 1325 1326 1327 1328 1329 1330
QImage UAS::getImage()
{
    return image;
}

void UAS::requestImage()
{
1331
#ifdef MAVLINK_ENABLED_PIXHAWK
1332 1333
    qDebug() << "trying to get an image from the uas...";

1334
    if (imagePacketsArrived == 0) {
1335 1336 1337
        mavlink_message_t msg;
        mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 50);
        sendMessage(msg);
1338
    } else if (QGC::groundTimeMilliseconds() - imageStart >= 1000) {
1339 1340 1341 1342 1343 1344 1345
        // handshake happened more than 1 second ago, packets should have arrived by now
        // maybe we missed some packets (dropped along the way)
        image.loadFromData(imageRecBuffer);
        emit imageReady(this);
        // Restart statemachine
        imagePacketsArrived = 0;
    }
1346
#endif
1347 1348
    // default else, wait?
}
pixhawk's avatar
pixhawk committed
1349 1350 1351 1352 1353 1354 1355 1356 1357


/* MANAGEMENT */

/*
 *
 * @return The uptime in milliseconds
 *
 **/
1358
quint64 UAS::getUptime() const
pixhawk's avatar
pixhawk committed
1359 1360 1361 1362 1363 1364 1365 1366
{
    if(startTime == 0) {
        return 0;
    } else {
        return MG::TIME::getGroundTimeNow() - startTime;
    }
}

1367
int UAS::getCommunicationStatus() const
pixhawk's avatar
pixhawk committed
1368 1369 1370 1371
{
    return commStatus;
}

1372 1373 1374
void UAS::requestParameters()
{
    mavlink_message_t msg;
1375
    mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25);
1376 1377
    // Send message twice to increase chance of reception
    sendMessage(msg);
1378 1379
}

1380
void UAS::writeParametersToStorage()
1381
{
1382 1383 1384
    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, (uint8_t)MAV_ACTION_STORAGE_WRITE);
unknown's avatar
unknown committed
1385
    //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE);
1386 1387 1388 1389 1390 1391 1392 1393 1394
    sendMessage(msg);
}

void UAS::readParametersFromStorage()
{
    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,(uint8_t)MAV_ACTION_STORAGE_READ);
    sendMessage(msg);
lm's avatar
lm committed
1395 1396
}

1397
void UAS::enableAllDataTransmission(int rate)
lm's avatar
lm committed
1398 1399
{
    // Buffers to write data to
1400
    mavlink_message_t msg;
1401
    mavlink_request_data_stream_t stream;
lm's avatar
lm committed
1402 1403
    // Select the message to request from now on
    // 0 is a magic ID and will enable/disable the standard message set except for heartbeat
1404
    stream.req_stream_id = MAV_DATA_STREAM_ALL;
lm's avatar
lm committed
1405 1406
    // Select the update rate in Hz the message should be send
    // All messages will be send with their default rate
1407 1408
    // 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 ?
lm's avatar
lm committed
1409 1410
    stream.req_message_rate = 0;
    // Start / stop the message
1411
    stream.start_stop = (rate) ? 1 : 0;
lm's avatar
lm committed
1412 1413 1414 1415 1416
    // 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
1417
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
1418 1419
    // Send message twice to increase chance of reception
    sendMessage(msg);
lm's avatar
lm committed
1420 1421 1422
    sendMessage(msg);
}

1423
void UAS::enableRawSensorDataTransmission(int rate)
lm's avatar
lm committed
1424 1425 1426
{
    // Buffers to write data to
    mavlink_message_t msg;
1427
    mavlink_request_data_stream_t stream;
lm's avatar
lm committed
1428
    // Select the message to request from now on
1429
    stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS;
lm's avatar
lm committed
1430
    // Select the update rate in Hz the message should be send
1431
    stream.req_message_rate = rate;
lm's avatar
lm committed
1432
    // Start / stop the message
1433
    stream.start_stop = (rate) ? 1 : 0;
lm's avatar
lm committed
1434 1435 1436 1437 1438
    // 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
1439
    mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream);
1440 1441
    // Send message twice to increase chance of reception
    sendMessage(msg);
lm's avatar
lm committed
1442 1443 1444
    sendMessage(msg);
}

1445
void UAS::enableExtendedSystemStatusTransmission(int rate)
lm's avatar
lm committed
1446
{
1447 1448 1449 1450
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1451
    stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS;
1452
    // Select the update rate in Hz the message should be send
1453
    stream.req_message_rate = rate;
1454
    // Start / stop the message
1455
    stream.start_stop = (rate) ? 1 : 0;
1456 1457 1458 1459 1460 1461
    // 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);
1462 1463
    // Send message twice to increase chance of reception
    sendMessage(msg);
1464
    sendMessage(msg);
lm's avatar
lm committed
1465
}
1466

1467
void UAS::enableRCChannelDataTransmission(int rate)
lm's avatar
lm committed
1468
{
1469 1470 1471 1472 1473
#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
1474 1475 1476
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1477
    stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS;
1478
    // Select the update rate in Hz the message should be send
1479
    stream.req_message_rate = rate;
1480
    // Start / stop the message
1481
    stream.start_stop = (rate) ? 1 : 0;
1482 1483 1484 1485 1486 1487
    // 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);
1488 1489
    // Send message twice to increase chance of reception
    sendMessage(msg);
1490
    sendMessage(msg);
1491
#endif
lm's avatar
lm committed
1492 1493
}

1494
void UAS::enableRawControllerDataTransmission(int rate)
lm's avatar
lm committed
1495
{
1496 1497 1498 1499
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1500
    stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER;
1501
    // Select the update rate in Hz the message should be send
1502
    stream.req_message_rate = rate;
1503
    // Start / stop the message
1504
    stream.start_stop = (rate) ? 1 : 0;
1505 1506 1507 1508 1509 1510
    // 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);
1511 1512
    // Send message twice to increase chance of reception
    sendMessage(msg);
1513
    sendMessage(msg);
lm's avatar
lm committed
1514 1515
}

lm's avatar
lm committed
1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536
//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);
//}
1537

1538
void UAS::enablePositionTransmission(int rate)
pixhawk's avatar
pixhawk committed
1539 1540 1541 1542 1543
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1544
    stream.req_stream_id = MAV_DATA_STREAM_POSITION;
pixhawk's avatar
pixhawk committed
1545
    // Select the update rate in Hz the message should be send
1546
    stream.req_message_rate = rate;
pixhawk's avatar
pixhawk committed
1547
    // Start / stop the message
1548
    stream.start_stop = (rate) ? 1 : 0;
pixhawk's avatar
pixhawk committed
1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559
    // 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);
}

1560
void UAS::enableExtra1Transmission(int rate)
pixhawk's avatar
pixhawk committed
1561 1562 1563 1564 1565
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1566
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA1;
pixhawk's avatar
pixhawk committed
1567
    // Select the update rate in Hz the message should be send
1568
    stream.req_message_rate = rate;
pixhawk's avatar
pixhawk committed
1569
    // Start / stop the message
1570
    stream.start_stop = (rate) ? 1 : 0;
pixhawk's avatar
pixhawk committed
1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581
    // 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);
}

1582
void UAS::enableExtra2Transmission(int rate)
pixhawk's avatar
pixhawk committed
1583 1584 1585 1586 1587
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1588
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA2;
pixhawk's avatar
pixhawk committed
1589
    // Select the update rate in Hz the message should be send
1590
    stream.req_message_rate = rate;
pixhawk's avatar
pixhawk committed
1591
    // Start / stop the message
1592
    stream.start_stop = (rate) ? 1 : 0;
pixhawk's avatar
pixhawk committed
1593 1594 1595 1596 1597 1598 1599 1600 1601 1602 1603
    // 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);
}

1604
void UAS::enableExtra3Transmission(int rate)
pixhawk's avatar
pixhawk committed
1605 1606 1607 1608 1609
{
    // Buffers to write data to
    mavlink_message_t msg;
    mavlink_request_data_stream_t stream;
    // Select the message to request from now on
1610
    stream.req_stream_id = MAV_DATA_STREAM_EXTRA3;
pixhawk's avatar
pixhawk committed
1611
    // Select the update rate in Hz the message should be send
1612
    stream.req_message_rate = rate;
1613
    // Start / stop the message
1614
    stream.start_stop = (rate) ? 1 : 0;
1615 1616 1617 1618 1619 1620
    // 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);
1621 1622
    // Send message twice to increase chance of reception
    sendMessage(msg);
1623
    sendMessage(msg);
1624 1625
}

lm's avatar
lm committed
1626 1627 1628 1629 1630 1631 1632
/**
 * Set a parameter value onboard
 *
 * @param component The component to set the parameter
 * @param id Name of the parameter
 * @param value Parameter value
 */
lm's avatar
lm committed
1633 1634
void UAS::setParameter(const int component, const QString& id, const float value)
{
1635 1636 1637 1638 1639 1640 1641 1642 1643 1644 1645 1646 1647
    if (!id.isNull()) {
        mavlink_message_t msg;
        mavlink_param_set_t p;
        p.param_value = value;
        p.target_system = (uint8_t)uasId;
        p.target_component = (uint8_t)component;

        // Copy string into buffer, ensuring not to exceed the buffer size
        for (unsigned int i = 0; i < sizeof(p.param_id); i++) {
            // String characters
            if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) {
                p.param_id[i] = id.toAscii()[i];
            }
1648 1649 1650 1651 1652
//        // Null termination at end of string or end of buffer
//        else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
//        {
//            p.param_id[i] = '\0';
//        }
1653 1654 1655 1656
            // Zero fill
            else {
                p.param_id[i] = 0;
            }
lm's avatar
lm committed
1657
        }
1658 1659
        mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p);
        sendMessage(msg);
lm's avatar
lm committed
1660
    }
1661 1662
}

1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674
void UAS::requestParameter(int component, int parameter)
{
    mavlink_message_t msg;
    mavlink_param_request_read_t read;
    read.param_index = parameter;
    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" << parameter;
}

1675 1676 1677 1678
void UAS::setSystemType(int systemType)
{
    type = systemType;
    // If the airframe is still generic, change it to a close default type
1679 1680
    if (airframe == 0) {
        switch (systemType) {
1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691
        case MAV_FIXED_WING:
            airframe = QGC_AIRFRAME_EASYSTAR;
            break;
        case MAV_QUADROTOR:
            airframe = QGC_AIRFRAME_MIKROKOPTER;
            break;
        }
    }
    emit systemSpecsChanged(uasId);
}

1692 1693 1694
void UAS::setUASName(const QString& name)
{
    this->name = name;
1695
    writeSettings();
1696
    emit nameChanged(name);
1697
    emit systemSpecsChanged(uasId);
1698 1699
}

lm's avatar
lm committed
1700 1701 1702 1703
void UAS::executeCommand(MAV_CMD command)
{
    mavlink_message_t msg;
    mavlink_command_t cmd;
1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728
    cmd.command = (uint8_t)command;
    cmd.confirmation = 0;
    cmd.param1 = 0.0f;
    cmd.param2 = 0.0f;
    cmd.param3 = 0.0f;
    cmd.param4 = 0.0f;
    cmd.target_system = uasId;
    cmd.target_component = 0;
    mavlink_msg_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
    sendMessage(msg);
}

void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
{
    mavlink_message_t msg;
    mavlink_command_t cmd;
    cmd.command = (uint8_t)command;
    cmd.confirmation = confirmation;
    cmd.param1 = param1;
    cmd.param2 = param2;
    cmd.param3 = param3;
    cmd.param4 = param4;
    cmd.target_system = uasId;
    cmd.target_component = component;
    mavlink_msg_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
lm's avatar
lm committed
1729 1730 1731
    sendMessage(msg);
}

1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744
/**
 * Sets an action
 *
 **/
void UAS::setAction(MAV_ACTION action)
{
    mavlink_message_t msg;
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action);
    // Send message twice to increase chance that it reaches its goal
    sendMessage(msg);
    sendMessage(msg);
}

pixhawk's avatar
pixhawk committed
1745
/**
lm's avatar
lm committed
1746
 * Launches the system
pixhawk's avatar
pixhawk committed
1747 1748 1749 1750
 *
 **/
void UAS::launch()
{
1751
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1752
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1753
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
1754 1755 1756
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1757 1758 1759 1760 1761 1762 1763 1764
}

/**
 * Depending on the UAS, this might make the rotors of a helicopter spinning
 *
 **/
void UAS::enable_motors()
{
1765
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1766
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1767
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START);
1768 1769 1770
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1771 1772 1773 1774 1775 1776 1777 1778
}

/**
 * @warning Depending on the UAS, this might completely stop all motors.
 *
 **/
void UAS::disable_motors()
{
1779
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1780
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1781
    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP);
1782 1783 1784
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1785 1786 1787 1788 1789 1790 1791 1792 1793 1794 1795 1796 1797 1798
}

void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust)
{
    // Scale values
    double rollPitchScaling = 0.2f;
    double yawScaling = 0.5f;
    double thrustScaling = 1.0f;

    manualRollAngle = roll * rollPitchScaling;
    manualPitchAngle = pitch * rollPitchScaling;
    manualYawAngle = yaw * yawScaling;
    manualThrust = thrust * thrustScaling;

1799
    if(mode == (int)MAV_MODE_MANUAL) {
1800 1801 1802 1803
        mavlink_message_t message;
        mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
        sendMessage(message);
        qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
pixhawk's avatar
pixhawk committed
1804

1805
        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
1806
    } else {
1807 1808
        qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
    }
pixhawk's avatar
pixhawk committed
1809 1810
}

1811 1812 1813 1814 1815
int UAS::getSystemType()
{
    return this->type;
}

pixhawk's avatar
pixhawk committed
1816 1817
void UAS::receiveButton(int buttonIndex)
{
1818
    switch (buttonIndex) {
pixhawk's avatar
pixhawk committed
1819
    case 0:
pixhawk's avatar
pixhawk committed
1820

pixhawk's avatar
pixhawk committed
1821 1822
        break;
    case 1:
pixhawk's avatar
pixhawk committed
1823

pixhawk's avatar
pixhawk committed
1824 1825 1826 1827 1828
        break;
    default:

        break;
    }
1829
    //    qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";
pixhawk's avatar
pixhawk committed
1830 1831 1832

}

1833

1834
/*void UAS::requestWaypoints()
1835 1836 1837 1838 1839
{
//    mavlink_message_t msg;
//    mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1840 1841
    waypointManager.requestWaypoints();
    qDebug() << "UAS Request WPs";
1842 1843
}

pixhawk's avatar
pixhawk committed
1844 1845
void UAS::setWaypoint(Waypoint* wp)
{
1846 1847 1848 1849 1850 1851 1852 1853 1854 1855 1856 1857 1858 1859 1860 1861 1862 1863
//    mavlink_message_t msg;
//    mavlink_waypoint_set_t set;
//    set.id = wp->id;
//    //QString name = wp->name;
//    // FIXME Check if this works properly
//    //name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN);
//    //strcpy((char*)set.name, name.toStdString().c_str());
//    set.autocontinue = wp->autocontinue;
//    set.target_component = 25; // FIXME
//    set.target_system = uasId;
//    set.active = wp->current;
//    set.x = wp->x;
//    set.y = wp->y;
//    set.z = wp->z;
//    set.yaw = wp->yaw;
//    mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1864 1865 1866 1867
}

void UAS::setWaypointActive(int id)
{
1868 1869 1870 1871 1872 1873 1874 1875 1876 1877 1878 1879 1880 1881 1882 1883 1884 1885 1886 1887 1888 1889 1890
//    mavlink_message_t msg;
//    mavlink_waypoint_set_active_t active;
//    active.id = id;
//    active.target_system = uasId;
//    active.target_component = 25; // FIXME
//    mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active);
//    // Send message twice to increase chance of reception
//    sendMessage(msg);
//    sendMessage(msg);
//    // TODO This should be not directly emitted, but rather being fed back from the UAS
//    emit waypointSelected(getUASID(), id);
}

void UAS::clearWaypointList()
{
//    mavlink_message_t msg;
//    // FIXME
//    mavlink_waypoint_clear_list_t clist;
//    clist.target_system = uasId;
//    clist.target_component = 25;  // FIXME
//    mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist);
//    sendMessage(msg);
//    qDebug() << "UAS clears Waypoints!";
1891
}*/
pixhawk's avatar
pixhawk committed
1892 1893 1894 1895


void UAS::halt()
{
1896

1897
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1898
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1899
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT);
1900 1901 1902
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
1903

pixhawk's avatar
pixhawk committed
1904 1905 1906 1907
}

void UAS::go()
{
1908
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1909
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1910
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_CONTINUE);
1911 1912 1913
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1914 1915 1916 1917 1918
}

/** Order the robot to return home / to land on the runway **/
void UAS::home()
{
1919
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1920
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1921
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,  (int)MAV_ACTION_RETURN);
1922 1923 1924
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1925 1926 1927 1928 1929 1930 1931 1932
}

/**
 * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
 * and might differ between systems.
 */
void UAS::emergencySTOP()
{
1933

1934
    mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1935
    // TODO Replace MG System ID with static function call and allow to change ID in GUI
1936
    mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND);
1937 1938 1939
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
pixhawk's avatar
pixhawk committed
1940 1941 1942 1943 1944 1945 1946 1947 1948 1949 1950 1951 1952 1953 1954 1955 1956 1957 1958 1959 1960 1961 1962
}

/**
 * All 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()
{
    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()));


1963
    if (ret == QMessageBox::Yes) {
1964
        mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
1965
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
1966
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL);
1967 1968 1969
        // Send message twice to increase chance of reception
        sendMessage(msg);
        sendMessage(msg);
pixhawk's avatar
pixhawk committed
1970 1971 1972 1973 1974
        result = true;
    }
    return result;
}

1975 1976
void UAS::startHil()
{
1977

James Goppert's avatar
James Goppert committed
1978 1979 1980 1981 1982 1983
    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_START_HILSIM);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
1984 1985 1986

}

1987 1988
void UAS::stopHil()
{
1989

James Goppert's avatar
James Goppert committed
1990 1991 1992 1993 1994 1995
    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_STOP_HILSIM);
    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
1996 1997 1998 1999

}


pixhawk's avatar
pixhawk committed
2000 2001 2002 2003 2004 2005 2006
void UAS::shutdown()
{
    bool result = false;
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Critical);
    msgBox.setText("Shutting down the UAS");
    msgBox.setInformativeText("Do you want to shut down the onboard computer?");
2007

pixhawk's avatar
pixhawk committed
2008 2009 2010 2011 2012 2013 2014 2015
    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()));


2016
    if (ret == QMessageBox::Yes) {
pixhawk's avatar
pixhawk committed
2017
        // If the active UAS is set, execute command
2018
        mavlink_message_t msg;
pixhawk's avatar
pixhawk committed
2019
        // TODO Replace MG System ID with static function call and allow to change ID in GUI
2020
        mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN);
2021 2022 2023
        // Send message twice to increase chance of reception
        sendMessage(msg);
        sendMessage(msg);
2024

pixhawk's avatar
pixhawk committed
2025 2026 2027 2028
        result = true;
    }
}

2029 2030
void UAS::setTargetPosition(float x, float y, float z, float yaw)
{
2031 2032 2033 2034 2035 2036
    mavlink_message_t msg;
    mavlink_msg_position_target_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, x, y, z, yaw);

    // Send message twice to increase chance of reception
    sendMessage(msg);
    sendMessage(msg);
2037 2038
}

pixhawk's avatar
pixhawk committed
2039 2040 2041
/**
 * @return The name of this system as string in human-readable form
 */
2042
QString UAS::getUASName(void) const
pixhawk's avatar
pixhawk committed
2043 2044
{
    QString result;
2045
    if (name == "") {
pixhawk's avatar
pixhawk committed
2046
        result = tr("MAV ") + result.sprintf("%03d", getUASID());
2047
    } else {
pixhawk's avatar
pixhawk committed
2048 2049 2050 2051 2052 2053 2054
        result = name;
    }
    return result;
}

void UAS::addLink(LinkInterface* link)
{
2055
    if (!links->contains(link)) {
pixhawk's avatar
pixhawk committed
2056
        links->append(link);
2057
        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
pixhawk's avatar
pixhawk committed
2058 2059 2060
    }
}

2061 2062 2063
void UAS::removeLink(QObject* object)
{
    LinkInterface* link = dynamic_cast<LinkInterface*>(object);
2064
    if (link) {
2065 2066 2067 2068
        links->removeAt(links->indexOf(link));
    }
}

pixhawk's avatar
pixhawk committed
2069 2070 2071 2072 2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083
/**
 * @brief Get the links associated with this robot
 *
 **/
QList<LinkInterface*>* UAS::getLinks()
{
    return links;
}



void UAS::setBattery(BatteryType type, int cells)
{
    this->batteryType = type;
    this->cells = cells;
2084
    switch (batteryType) {
lm's avatar
lm committed
2085
    case NICD:
pixhawk's avatar
pixhawk committed
2086
        break;
lm's avatar
lm committed
2087
    case NIMH:
pixhawk's avatar
pixhawk committed
2088
        break;
lm's avatar
lm committed
2089
    case LIION:
pixhawk's avatar
pixhawk committed
2090
        break;
lm's avatar
lm committed
2091
    case LIPOLY:
pixhawk's avatar
pixhawk committed
2092 2093 2094
        fullVoltage = this->cells * UAS::lipoFull;
        emptyVoltage = this->cells * UAS::lipoEmpty;
        break;
lm's avatar
lm committed
2095
    case LIFE:
pixhawk's avatar
pixhawk committed
2096
        break;
lm's avatar
lm committed
2097
    case AGZN:
pixhawk's avatar
pixhawk committed
2098 2099 2100 2101
        break;
    }
}

2102 2103
void UAS::setBatterySpecs(const QString& specs)
{
2104
    if (specs.length() == 0 || specs.contains("%")) {
2105
        batteryRemainingEstimateEnabled = false;
2106
        bool ok;
2107 2108 2109
        QString percent = specs;
        percent = percent.remove("%");
        float temp = percent.toFloat(&ok);
2110
        if (ok) {
2111
            warnLevelPercent = temp;
2112
        } else {
2113 2114
            emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
        }
2115
    } else {
2116 2117 2118 2119 2120
        batteryRemainingEstimateEnabled = true;
        QString stringList = specs;
        stringList = stringList.remove("V");
        stringList = stringList.remove("v");
        QStringList parts = stringList.split(",");
2121
        if (parts.length() == 3) {
2122 2123 2124 2125 2126 2127 2128 2129 2130 2131 2132
            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;
2133
        } else {
2134 2135
            emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong");
        }
2136 2137 2138 2139 2140
    }
}

QString UAS::getBatterySpecs()
{
2141
    if (batteryRemainingEstimateEnabled) {
2142
        return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage);
2143
    } else {
2144 2145
        return QString("%1%").arg(warnLevelPercent);
    }
2146 2147
}

pixhawk's avatar
pixhawk committed
2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160
int UAS::calculateTimeRemaining()
{
    quint64 dt = MG::TIME::getGroundTimeNow() - 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;
}

lm's avatar
lm committed
2161 2162 2163
/**
 * @return charge level in percent - 0 - 100
 */
2164
float UAS::getChargeLevel()
pixhawk's avatar
pixhawk committed
2165
{
2166 2167
    if (batteryRemainingEstimateEnabled) {
        if (lpVoltage < emptyVoltage) {
2168
            chargeLevel = 0.0f;
2169
        } else if (lpVoltage > fullVoltage) {
2170
            chargeLevel = 100.0f;
2171
        } else {
2172 2173
            chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
        }
2174 2175
    }
    return chargeLevel;
pixhawk's avatar
pixhawk committed
2176 2177
}

lm's avatar
lm committed
2178 2179
void UAS::startLowBattAlarm()
{
2180
    if (!lowBattAlarm) {
2181
        GAudioOutput::instance()->alert(tr("SYSTEM %1 HAS LOW BATTERY").arg(getUASName()));
2182
        QTimer::singleShot(2000, GAudioOutput::instance(), SLOT(startEmergency()));
lm's avatar
lm committed
2183 2184 2185 2186 2187 2188
        lowBattAlarm = true;
    }
}

void UAS::stopLowBattAlarm()
{
2189
    if (lowBattAlarm) {
lm's avatar
lm committed
2190 2191 2192 2193
        GAudioOutput::instance()->stopEmergency();
        lowBattAlarm = false;
    }
}