MockLink.cc

/****************************************************************************
 *
 *   (c) 2009-2016 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"

#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")

/// @file
///     @brief Mock implementation of a Link.
///
///     @author Don Gagne <don@thegagnes.com>

// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
// testing of a gazebo vehicle and a mocklink vehicle
double      MockLink::_defaultVehicleLatitude =     47.397f;
double      MockLink::_defaultVehicleLongitude =    8.5455f;
double      MockLink::_defaultVehicleAltitude =     488.056f;
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::_failureModeKey =    "FailureMode";

MockLink::MockLink(SharedLinkConfigurationPointer& config)
    : LinkInterface                         (config)
    , _missionItemHandler                   (this, qgcApp()->toolbox()->mavlinkProtocol())
    , _name                                 ("MockLink")
    , _connected                            (false)
    , _mavlinkChannel                       (0)
    , _vehicleSystemId                      (_nextVehicleSystemId++)
    , _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)
    , _vehicleLatitude                      (_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))     // Slight offset for each vehicle
    , _vehicleLongitude                     (_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
    , _vehicleAltitude                      (_defaultVehicleAltitude)
    , _fileServer                           (NULL)
    , _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)
{
    MockConfiguration* mockConfig = qobject_cast<MockConfiguration*>(_config.data());
    _firmwareType = mockConfig->firmwareType();
    _vehicleType = mockConfig->vehicleType();
    _sendStatusText = mockConfig->sendStatusText();
    _failureMode = mockConfig->failureMode();

    union px4_custom_mode   px4_cm;

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

    _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
    Q_CHECK_PTR(_fileServer);

    moveToThread(this);

    _loadParams();
}

MockLink::~MockLink(void)
{
    _disconnect();
    if (!_logDownloadFilename.isEmpty()) {
        QFile::remove(_logDownloadFilename);
    }
}

bool MockLink::_connect(void)
{
    if (!_connected) {
        _connected = true;
        _mavlinkChannel = qgcApp()->toolbox()->linkManager()->_reserveMavlinkChannel();
        if (_mavlinkChannel == 0) {
            qWarning() << "No mavlink channels available";
            return false;
        }
        // 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) {
        if (_mavlinkChannel != 0) {
            qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel);
        }
        _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);

    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) {
        _sendVibration();
        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 (_mavlinkStarted && _connected) {
        _sendHeartBeat();
        if (_sendGPSPositionDelayCount > 0) {
            // We delay gps position for better testing
            _sendGPSPositionDelayCount--;
        } else {
            _sendGpsRawInt();
        }
    }
}

void MockLink::_run500HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
        _paramRequestListWorker();
        _logDownloadWorker();
    }
}