MockLink.cc

/*=====================================================================

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

 ======================================================================*/

#include "MockLink.h"
#include "QGCLoggingCategory.h"
#include "QGCApplication.h"

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

#include <string.h>

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

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

enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
};

enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};

union px4_custom_mode {
    struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
    float data_float;
};

float MockLink::_vehicleLatitude =  47.633033f;
float MockLink::_vehicleLongitude = -122.08794f;
float MockLink::_vehicleAltitude =  3.5f;

const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";
const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";

MockLink::MockLink(MockConfiguration* config)
    : _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol())
    , _name("MockLink")
    , _connected(false)
    , _vehicleSystemId(128)     // FIXME: Pull from eventual parameter manager
    , _vehicleComponentId(200)  // FIXME: magic number?
    , _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)
    , _fileServer(NULL)
    , _sendStatusText(false)
    , _apmSendHomePositionOnEmptyList(false)
    , _sendHomePositionDelayCount(2)
{
    _config = config;
    if (_config) {
        _firmwareType = config->firmwareType();
        _vehicleType = config->vehicleType();
        _sendStatusText = config->sendStatusText();
        _config->setLink(this);
    }

    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();
}

bool MockLink::_connect(void)
{
    if (!_connected) {
        _connected = true;
        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  _timer50HzTasks;

    QObject::connect(&_timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::connect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::connect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);

    _timer1HzTasks.start(1000);
    _timer10HzTasks.start(100);
    _timer50HzTasks.start(20);

    exec();

    QObject::disconnect(&_timer1HzTasks,  &QTimer::timeout, this, &MockLink::_run1HzTasks);
    QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
    QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);

    _missionItemHandler.shutdown();
}

void MockLink::_run1HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
        _sendHeartBeat();
        _sendVibration();
        if (_sendHomePositionDelayCount > 0) {
            // We delay home position a bit to be more realistic
            _sendHomePositionDelayCount--;
        } else {
            _sendHomePosition();
        }
        if (_sendStatusText) {
            _sendStatusText = false;
            _sendStatusTextMessages();
        }
    }
}

void MockLink::_run10HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
        _sendGpsRawInt();
    }
}

void MockLink::_run50HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
    }
}

void MockLink::_loadParams(void)