TerrainProtocolHandler.cc 6.33 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 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 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
/****************************************************************************
 *
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "TerrainProtocolHandler.h"
#include "TerrainQuery.h"
#include "QGCApplication.h"

QGC_LOGGING_CATEGORY(TerrainProtocolHandlerLog, "TerrainProtocolHandlerLog")

TerrainProtocolHandler::TerrainProtocolHandler(Vehicle* vehicle, TerrainFactGroup* terrainFactGroup, QObject *parent)
    : QObject           (parent)
    , _vehicle          (vehicle)
    , _terrainFactGroup (terrainFactGroup)
{
    _terrainDataSendTimer.setSingleShot(false);
    _terrainDataSendTimer.setInterval(1000.0/12.0);
    connect(&_terrainDataSendTimer, &QTimer::timeout, this, &TerrainProtocolHandler::_sendNextTerrainData);
}

bool TerrainProtocolHandler::mavlinkMessageReceived(const mavlink_message_t message)
{
    switch (message.msgid) {
    case MAVLINK_MSG_ID_TERRAIN_REQUEST:
        _handleTerrainRequest(message);
        return false;
    case MAVLINK_MSG_ID_TERRAIN_REPORT:
        _handleTerrainReport(message);
        return false;
    default:
        return true;
    }
}

void TerrainProtocolHandler::_handleTerrainRequest(const mavlink_message_t& message)
{
    _terrainRequestActive = true;
    mavlink_msg_terrain_request_decode(&message, &_currentTerrainRequest);
    _sendNextTerrainData();
}

void TerrainProtocolHandler::_handleTerrainReport(const mavlink_message_t& message)
{
    mavlink_terrain_report_t terrainReport;
    mavlink_msg_terrain_report_decode(&message, &terrainReport);

    _terrainFactGroup->blocksPending()->setRawValue(terrainReport.pending);
    _terrainFactGroup->blocksLoaded()->setRawValue(terrainReport.loaded);

    if (TerrainProtocolHandlerLog().isDebugEnabled()) {

        bool error;
        QGeoCoordinate coord(static_cast<double>(terrainReport.lat) / 1e7, static_cast<double>(terrainReport.lon) / 1e7);
        QList<double>   altitudes;
        QList<QGeoCoordinate> coordinates;
        coordinates.append(coord);
        bool altAvailable = (TerrainAtCoordinateQuery::getAltitudesForCoordinates(coordinates, altitudes, error));
        QString vehicleAlt = terrainReport.spacing ? QStringLiteral("%1").arg(terrainReport.terrain_height) : QStringLiteral("n/a");
        QString qgcAlt = error ? QStringLiteral("error") :
                                 (altAvailable ? QStringLiteral("%1").arg(altitudes[0]) : QStringLiteral("n/a"));
        qDebug() << "TERRAIN_REPORT" << coord << QStringLiteral("Vehicle(%1) QGC(%2)").arg(vehicleAlt).arg(qgcAlt);
    }
}

void TerrainProtocolHandler::_sendNextTerrainData(void)
{
    if (!_terrainRequestActive) {
        return;
    }

    QGeoCoordinate terrainRequestCoordSWCorner(static_cast<double>(_currentTerrainRequest.lat) / 1e7, static_cast<double>(_currentTerrainRequest.lon) / 1e7);
    int spacingBetweenGrids = _currentTerrainRequest.grid_spacing * 4;

    // Each TERRAIN_DATA sent to vehicle contains a 4x4 grid of heights
    // TERRAIN_REQUEST.mask has a bit for each entry in an 8x7 grid
    // gridBit = 0 refers to the the sw corner of the 8x7 grid

    bool bitFound = false;
    for (int rowIndex=0; rowIndex<7; rowIndex++) {
        for (int colIndex=0; colIndex<8; colIndex++) {
            uint8_t gridBit = (rowIndex * 8) + colIndex;
            uint64_t checkBit = 1ull << gridBit;
            if (_currentTerrainRequest.mask & checkBit) {
                // Move east and then north to generate the coordinate for sw corner of the specific gridBit
                QGeoCoordinate swCorner = terrainRequestCoordSWCorner.atDistanceAndAzimuth(spacingBetweenGrids * colIndex, 90);
                swCorner = swCorner.atDistanceAndAzimuth(spacingBetweenGrids * rowIndex, 0);
                _sendTerrainData(swCorner, gridBit);
                bitFound = true;
                break;
            }
        }
        if (bitFound) {
            break;
        }
    }

    if (bitFound) {
        // Kick timer to send next possible TERRAIN_DATA to vehicle
        _terrainDataSendTimer.start();
    } else {
        _terrainRequestActive = false;
    }
}

void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, uint8_t gridBit)
{
    QList<QGeoCoordinate> coordinates;
    for (int rowIndex=0; rowIndex<4; rowIndex++) {
        for (int colIndex=0; colIndex<4; colIndex++) {
            // Move east and then north to generate the coordinate for grid point
            QGeoCoordinate coord = swCorner.atDistanceAndAzimuth(_currentTerrainRequest.grid_spacing * colIndex, 90);
            coord = coord.atDistanceAndAzimuth(_currentTerrainRequest.grid_spacing * rowIndex, 0);
            coordinates.append(coord);
        }
    }

    // Query terrain system for altitudes. If it has them available it will return them. If not they will be queued for download.
    bool            error = false;
    QList<double>   altitudes;
    if (TerrainAtCoordinateQuery::getAltitudesForCoordinates(coordinates, altitudes, error)) {
        if (error) {
            qCWarning(TerrainProtocolHandlerLog) << "_sendTerrainData TerrainAtCoordinateQuery::getAltitudesForCoordinates failed";
        } else {
            // Only clear the bit if the query succeeds. Otherwise just let it try again on the next timer tick
            uint64_t removeBit = ~(1ull << gridBit);
            _currentTerrainRequest.mask &= removeBit;
            int altIndex = 0;
            int16_t terrainData[16];
            for (const double& altitude : altitudes) {
                terrainData[altIndex++] = static_cast<int16_t>(altitude);
            }

            mavlink_message_t msg;
            mavlink_msg_terrain_data_pack_chan(
                        qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                        qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                        _vehicle->priorityLink()->mavlinkChannel(),
                        &msg,
                        _currentTerrainRequest.lat,
                        _currentTerrainRequest.lon,
                        _currentTerrainRequest.grid_spacing,
                        gridBit,
                        terrainData);
            _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
        }
    }
}