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

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

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

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

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

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

41
QGC_LOGGING_CATEGORY(UASLog, "UASLog")
42

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

    status(-1),

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

58 59 60 61
    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
Don Gagne's avatar
Don Gagne committed
62

63 64 65
    attitudeKnown(false),
    attitudeStamped(false),
    lastAttitude(0),
66

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

Don Gagne's avatar
Don Gagne committed
69 70
    blockHomePositionChanges(false),

71 72
    // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
    // TODO: calibrate stand-still pixhawk variances
73
    xacc_var(0.6457f),
dogmaphobic's avatar
dogmaphobic committed
74
    yacc_var(0.7048f),
75
    zacc_var(0.97885f),
dogmaphobic's avatar
dogmaphobic committed
76 77 78
    rollspeed_var(0.8126f),
    pitchspeed_var(0.6145f),
    yawspeed_var(0.5852f),
79 80 81 82 83 84 85
    xmag_var(0.2393f),
    ymag_var(0.2283f),
    zmag_var(0.1665f),
    abs_pressure_var(0.5802f),
    diff_pressure_var(0.5802f),
    pressure_alt_var(0.5802f),
    temperature_var(0.7145f),
86
    /*
87 88 89 90 91 92 93 94 95 96 97 98 99
    xacc_var(0.0f),
    yacc_var(0.0f),
    zacc_var(0.0f),
    rollspeed_var(0.0f),
    pitchspeed_var(0.0f),
    yawspeed_var(0.0f),
    xmag_var(0.0f),
    ymag_var(0.0f),
    zmag_var(0.0f),
    abs_pressure_var(0.0f),
    diff_pressure_var(0.0f),
    pressure_alt_var(0.0f),
    temperature_var(0.0f),
100
    */
101

102
    // The protected members.
103 104 105 106
    connectionLost(false),
    lastVoltageWarning(0),
    lastNonNullTime(0),
    onboardTimeOffsetInvalidCount(0),
107 108
    _vehicle(vehicle),
    _firmwarePluginManager(firmwarePluginManager)
109
{
dogmaphobic's avatar
dogmaphobic committed
110

111 112 113 114 115 116 117 118 119 120
}

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

121 122
// Ignore warnings from mavlink headers for both GCC/Clang and MSVC
#ifdef __GNUC__
123

124 125 126 127 128 129 130 131 132 133
#if __GNUC__ > 8
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#elif defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Waddress-of-packed-member"
#else
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wall"
#endif
134

135 136 137
#else
#pragma warning(push, 0)
#endif
138

139 140
void UAS::receiveMessage(mavlink_message_t message)
{
141 142 143
    // Only accept messages from this system (condition 1)
    // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled
    // and we already got one attitude packet
144
    if (message.sysid == uasId && (!attitudeStamped || lastAttitude != 0 || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
145 146 147
    {
        switch (message.msgid)
        {
dogmaphobic's avatar
dogmaphobic committed
148

149 150 151 152 153 154 155 156 157 158 159 160
        case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
        {
            mavlink_data_transmission_handshake_t p;
            mavlink_msg_data_transmission_handshake_decode(&message, &p);
            imageSize = p.size;
            imagePackets = p.packets;
            imagePayload = p.payload;
            imageQuality = p.jpg_quality;
            imageType = p.type;
            imageWidth = p.width;
            imageHeight = p.height;
            imageStart = QGC::groundTimeMilliseconds();
161 162
            imagePacketsArrived = 0;

163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178
        }
            break;

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

            // Check if we have a valid transaction
            if (imagePackets == 0)
            {
                // NO VALID TRANSACTION - ABORT
                // Restart statemachine
                imagePacketsArrived = 0;
Don Gagne's avatar
Don Gagne committed
179
                break;
180 181 182 183 184 185 186 187 188 189 190 191 192
            }

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

            ++imagePacketsArrived;

            // emit signal if all packets arrived
193
            if (imagePacketsArrived >= imagePackets)
194 195
            {
                // Restart statemachine
Don Gagne's avatar
Don Gagne committed
196 197
                imagePackets = 0;
                imagePacketsArrived = 0;
198 199 200 201 202
                emit imageReady(this);
            }
        }
            break;

dogmaphobic's avatar
dogmaphobic committed
203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218
        case MAVLINK_MSG_ID_LOG_ENTRY:
        {
            mavlink_log_entry_t log;
            mavlink_msg_log_entry_decode(&message, &log);
            emit logEntry(this, log.time_utc, log.size, log.id, log.num_logs, log.last_log_num);
        }
            break;

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

219 220 221 222 223 224
        default:
            break;
        }
    }
}

225 226 227 228 229 230 231 232 233 234
// Pop warnings ignoring for mavlink headers for both GCC/Clang and MSVC
#ifdef __GNUC__
    #if defined(__clang__)
        #pragma clang diagnostic pop
    #else
        #pragma GCC diagnostic pop
    #endif
#else
#pragma warning(pop, 0)
#endif
235

236 237
/**
* Check if time is smaller than 40 years, assuming no system without Unix
238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
* timestamp runs longer than 40 years continuously without reboot. In worst case
* this will add/subtract the communication delay between GCS and MAV, it will
* never alter the timestamp in a safety critical way.
*/
quint64 UAS::getUnixReferenceTime(quint64 time)
{
    // Same as getUnixTime, but does not react to attitudeStamped mode
    if (time == 0)
    {
        //        qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
        return QGC::groundTimeMilliseconds();
    }
    // Check if time is smaller than 40 years,
    // assuming no system without Unix timestamp
    // runs longer than 40 years continuously without
    // reboot. In worst case this will add/subtract the
    // communication delay between GCS and MAV,
    // it will never alter the timestamp in a safety
    // critical way.
    //
    // Calculation:
    // 40 years
    // 365 days
    // 24 hours
    // 60 minutes
    // 60 seconds
    // 1000 milliseconds
    // 1000 microseconds
#ifndef _MSC_VER
    else if (time < 1261440000000000LLU)
#else
    else if (time < 1261440000000000)
#endif
    {
        //        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
        if (onboardTimeOffset == 0)
        {
            onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
        }
        return time/1000 + onboardTimeOffset;
    }
    else
    {
        // Time is not zero and larger than 40 years -> has to be
        // a Unix epoch timestamp. Do nothing.
        return time/1000;
    }
}

/**
* @warning If attitudeStamped is enabled, this function will not actually return
289
* the precise time stamp of this measurement augmented to UNIX time, but will
290
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
291
* reason why one would want this, except for system setups where the onboard
292
* clock is not present or broken and datasets should be collected that are still
293
* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE
294 295 296 297 298 299 300 301 302
* SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
*/
quint64 UAS::getUnixTimeFromMs(quint64 time)
{
    return getUnixTime(time*1000);
}

/**
* @warning If attitudeStamped is enabled, this function will not actually return
303 304 305 306
* the precise time stam of this measurement augmented to UNIX time, but will
* MOVE the timestamp IN TIME to match the last measured attitude. There is no
* reason why one would want this, except for system setups where the onboard
* clock is not present or broken and datasets should be collected that are
307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
* still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED
* RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL!
*/
quint64 UAS::getUnixTime(quint64 time)
{
    quint64 ret = 0;
    if (attitudeStamped)
    {
        ret = lastAttitude;
    }

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

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

    return ret;
}

364
/**
365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425
* Get the status of the code and a description of the status.
* Status can be unitialized, booting up, calibrating sensors, active
* standby, cirtical, emergency, shutdown or unknown.
*/
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
    switch (statusCode)
    {
    case MAV_STATE_UNINIT:
        uasState = tr("UNINIT");
        stateDescription = tr("Unitialized, booting up.");
        break;
    case MAV_STATE_BOOT:
        uasState = tr("BOOT");
        stateDescription = tr("Booting system, please wait.");
        break;
    case MAV_STATE_CALIBRATING:
        uasState = tr("CALIBRATING");
        stateDescription = tr("Calibrating sensors, please wait.");
        break;
    case MAV_STATE_ACTIVE:
        uasState = tr("ACTIVE");
        stateDescription = tr("Active, normal operation.");
        break;
    case MAV_STATE_STANDBY:
        uasState = tr("STANDBY");
        stateDescription = tr("Standby mode, ready for launch.");
        break;
    case MAV_STATE_CRITICAL:
        uasState = tr("CRITICAL");
        stateDescription = tr("FAILURE: Continuing operation.");
        break;
    case MAV_STATE_EMERGENCY:
        uasState = tr("EMERGENCY");
        stateDescription = tr("EMERGENCY: Land Immediately!");
        break;
        //case MAV_STATE_HILSIM:
        //uasState = tr("HIL SIM");
        //stateDescription = tr("HIL Simulation, Sensors read from SIM");
        //break;

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

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

QImage UAS::getImage()
{

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

    // RAW greyscale
    if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U)
    {
426
        int imgColors = 255;
427 428 429 430 431

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

Don Gagne's avatar
Don Gagne committed
432
        QByteArray tmpImage(header.toStdString().c_str(), header.length());
433 434 435 436 437 438 439 440 441 442 443 444
        tmpImage.append(imageRecBuffer);

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

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

        if (!image.loadFromData(tmpImage, "PGM"))
        {
445
            qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
446 447 448 449 450 451 452 453 454 455 456 457
            return QImage();
        }

    }
    // BMP with header
    else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
             imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
             imageType == MAVLINK_DATA_STREAM_IMG_PGM ||
             imageType == MAVLINK_DATA_STREAM_IMG_PNG)
    {
        if (!image.loadFromData(imageRecBuffer))
        {
458
            qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!";
459
            return QImage();
460 461
        }
    }
462

463 464
    // Restart statemachine
    imagePacketsArrived = 0;
465 466
    imagePackets = 0;
    imageRecBuffer.clear();
467 468 469 470 471
    return image;
}

void UAS::requestImage()
{
472 473 474
    if (!_vehicle) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
475

476
   qDebug() << "trying to get an image from the uas...";
477 478 479 480 481

    // check if there is already an image transmission going on
    if (imagePacketsArrived == 0)
    {
        mavlink_message_t msg;
482 483
        mavlink_msg_data_transmission_handshake_pack_chan(mavlink->getSystemId(),
                                                          mavlink->getComponentId(),
484
                                                          _vehicle->vehicleLinkManager()->primaryLink()->mavlinkChannel(),
485 486 487
                                                          &msg,
                                                          MAVLINK_DATA_STREAM_IMG_JPEG,
                                                          0, 0, 0, 0, 0, 50);
488
        _vehicle->sendMessageOnLinkThreadSafe(_vehicle->vehicleLinkManager()->primaryLink(), msg);
489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511
    }
}


/* MANAGEMENT */

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

512
/**
Jean Cyr's avatar
Jean Cyr committed
513
* Order the robot to start receiver pairing
514 515 516
*/
void UAS::pairRX(int rxType, int rxSubType)
{
517 518 519 520 521 522
    if (_vehicle) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(),    // target component
                                 MAV_CMD_START_RX_PAIR,             // command id
                                 true,                              // showError
                                 rxType,
                                 rxSubType);
523
    }
524
}
dogmaphobic's avatar
dogmaphobic committed
525

Don Gagne's avatar
Don Gagne committed
526 527
void UAS::shutdownVehicle(void)
{
528
    _vehicle = nullptr;
Don Gagne's avatar
Don Gagne committed
529
}