MockLink.cc

/****************************************************************************
 *
 * (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 "MockLink.h"
#include "QGCLoggingCategory.h"
#include "QGCApplication.h"
#include "LinkManager.h"

#ifdef UNITTEST_BUILD
#include "UnitTest.h"
#endif

#include <QTimer>
#include <QDebug>
#include <QFile>

#include <string.h>

// FIXME: Hack to work around clean headers
#include "FirmwarePlugin/PX4/px4_custom_mode.h"

QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")
QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")

// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
// testing of a gazebo vehicle and a mocklink vehicle
#if 1
double      MockLink::_defaultVehicleLatitude =     47.397;
double      MockLink::_defaultVehicleLongitude =    8.5455;
double      MockLink::_defaultVehicleAltitude =     488.056;
#else
double      MockLink::_defaultVehicleLatitude =     47.6333022928789;
double      MockLink::_defaultVehicleLongitude =    -122.08833157994995;
double      MockLink::_defaultVehicleAltitude =     19.0;
#endif
int         MockLink::_nextVehicleSystemId =        128;
const char* MockLink::_failParam =                  "COM_FLTMODE6";

const char* MockConfiguration::_firmwareTypeKey         = "FirmwareType";
const char* MockConfiguration::_vehicleTypeKey          = "VehicleType";
const char* MockConfiguration::_sendStatusTextKey       = "SendStatusText";
const char* MockConfiguration::_incrementVehicleIdKey   = "IncrementVehicleId";
const char* MockConfiguration::_failureModeKey          = "FailureMode";

MockLink::MockLink(SharedLinkConfigurationPtr& config)
    : LinkInterface                         (config)
    , _missionItemHandler                   (this, qgcApp()->toolbox()->mavlinkProtocol())
    , _name                                 ("MockLink")
    , _connected                            (false)
    , _mavlinkChannel                       (0)
    , _vehicleComponentId                   (MAV_COMP_ID_AUTOPILOT1)
    , _inNSH                                (false)
    , _mavlinkStarted                       (true)
    , _mavBaseMode                          (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
    , _mavState                             (MAV_STATE_STANDBY)
    , _firmwareType                         (MAV_AUTOPILOT_PX4)
    , _vehicleType                          (MAV_TYPE_QUADROTOR)
    , _vehicleAltitude                      (_defaultVehicleAltitude)
    , _sendStatusText                       (false)
    , _apmSendHomePositionOnEmptyList       (false)
    , _failureMode                          (MockConfiguration::FailNone)
    , _sendHomePositionDelayCount           (10)    // No home position for 4 seconds
    , _sendGPSPositionDelayCount            (100)   // No gps lock for 5 seconds
    , _currentParamRequestListComponentIndex(-1)
    , _currentParamRequestListParamIndex    (-1)
    , _logDownloadCurrentOffset             (0)
    , _logDownloadBytesRemaining            (0)
    , _adsbAngle                            (0)
{
    qDebug() << "MockLink" << this;

    MockConfiguration* mockConfig = qobject_cast<MockConfiguration*>(_config.get());
    _firmwareType       = mockConfig->firmwareType();
    _vehicleType        = mockConfig->vehicleType();
    _sendStatusText     = mockConfig->sendStatusText();
    _failureMode        = mockConfig->failureMode();
    _vehicleSystemId    = mockConfig->incrementVehicleId() ?  _nextVehicleSystemId++ : _nextVehicleSystemId;
    _vehicleLatitude    = _defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001);
    _vehicleLongitude   = _defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001);

    QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);

    union px4_custom_mode   px4_cm;
    px4_cm.data = 0;
    px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
    _mavCustomMode = px4_cm.data;

    _mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this);

    moveToThread(this);

    _loadParams();

    _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(1000, _adsbAngle);
    _adsbVehicleCoordinate.setAltitude(100);
    _runningTime.start();
}

MockLink::~MockLink(void)
{
    disconnect();
    if (!_logDownloadFilename.isEmpty()) {
        QFile::remove(_logDownloadFilename);
    }
    qDebug() << "~MockLink" << this;
}

bool MockLink::_connect(void)
{
    if (!_connected) {
        _connected = true;
        // MockLinks use Mavlink 2.0
        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(_mavlinkChannel);
        mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
        start();
        emit connected();
    }

    return true;
}

void MockLink::disconnect(void)
{
    if (_connected) {
        _connected = false;
        quit();
        wait();
        emit disconnected();
    }
}

void MockLink::run(void)
{
    QTimer  timer1HzTasks;
    QTimer  timer10HzTasks;
    QTimer  timer500HzTasks;

    QObject::connect(&timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::connect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks);

    timer1HzTasks.start(1000);
    timer10HzTasks.start(100);
    timer500HzTasks.start(2);

    // Send first set right away
    _run1HzTasks();
    _run10HzTasks();
    _run500HzTasks();

    exec();

    QObject::disconnect(&timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::disconnect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::disconnect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks);

    _missionItemHandler.shutdown();
}

void MockLink::_run1HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
        if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) {
            _sendHighLatency2();
        } else {
            _sendVibration();
            _sendBatteryStatus();
            _sendSysStatus();
            _sendADSBVehicles();
            if (!qgcApp()->runningUnitTests()) {
                // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
                _sendRCChannels();
            }
            if (_sendHomePositionDelayCount > 0) {
                // We delay home position for better testing
                _sendHomePositionDelayCount--;
            } else {
                _sendHomePosition();
            }
            if (_sendStatusText) {
                _sendStatusText = false;
                _sendStatusTextMessages();
            }
        }
    }
}

void MockLink::_run10HzTasks(void)
{
    if (linkConfiguration()->isHighLatency()) {
        return;
    }

    if (_mavlinkStarted && _connected) {