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 <QTimer>
#include <QDebug>
#include <QFile>

#include <string.h>

QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")

/// @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,
};

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

MockLink::MockLink(MockConfiguration* config) :
    _name("MockLink"),
    _connected(false),
    _vehicleSystemId(128),     // FIXME: Pull from eventual parameter manager
    _vehicleComponentId(200),  // FIXME: magic number?
    _inNSH(false),
    _mavlinkStarted(false),
    _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
    _mavState(MAV_STATE_STANDBY),
    _autopilotType(MAV_AUTOPILOT_PX4)
{
    _config = config;
    union px4_custom_mode   px4_cm;

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

    _missionItemHandler = new MockLinkMissionItemHandler(_vehicleSystemId, this);
    Q_CHECK_PTR(_missionItemHandler);

    moveToThread(this);
    _loadParams();
    QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes);
}

MockLink::~MockLink(void)
{
    _disconnect();
}

void MockLink::readBytes(void)
{
    // FIXME: This is a bad virtual from LinkInterface?
}

bool MockLink::_connect(void)
{
    if (!_connected) {
        _connected = true;
        start();
        emit connected();
    }

    return true;
}

bool MockLink::_disconnect(void)
{
    if (_connected) {
        _connected = false;
        exit();
        emit disconnected();
    }

    return true;
}

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

void MockLink::_run1HzTasks(void)
{
    if (_mavlinkStarted && _connected) {
        _sendHeartBeat();
    }
}

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

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

void MockLink::_loadParams(void)
{
    QFile paramFile(":/unittest/MockLink.params");

    bool success = paramFile.open(QFile::ReadOnly);
    Q_UNUSED(success);
    Q_ASSERT(success);

    QTextStream paramStream(&paramFile);

    while (!paramStream.atEnd()) {
        QString line = paramStream.readLine();

        if (line.startsWith("#")) {
            continue;
        }

        QStringList paramData = line.split("\t");
        Q_ASSERT(paramData.count() == 5);

        int componentId = paramData.at(1).toInt();
        QString paramName = paramData.at(2);
        QString valStr = paramData.at(3);
        uint paramType = paramData.at(4).toUInt();

        QVariant paramValue;
        switch (paramType) {
            case MAV_PARAM_TYPE_REAL32:
                paramValue = QVariant(valStr.toFloat());
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramValue = QVariant(valStr.toUInt());
                break;
            case MAV_PARAM_TYPE_INT32:
                paramValue = QVariant(valStr.toInt());
                break;
            case MAV_PARAM_TYPE_INT8:
                paramValue = QVariant((unsigned char)valStr.toUInt());
                break;
            default:
                Q_ASSERT(false);
                break;
        }

        qCDebug(MockLinkLog) << "Loading param" << paramName << paramValue;

        _mapParamName2Value[componentId][paramName] = paramValue;
        _mapParamName2MavParamType[paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
    }
}

void MockLink::_sendHeartBeat(void)
{
    mavlink_message_t   msg;
    uint8_t             buffer[MAVLINK_MAX_PACKET_LEN];

    mavlink_msg_heartbeat_pack(_vehicleSystemId,
                               _vehicleComponentId,
                               &msg,
                               MAV_TYPE_QUADROTOR,  // MAV_TYPE
                               _autopilotType,      // MAV_AUTOPILOT
                               _mavBaseMode,        // MAV_MODE
                               _mavCustomMode,      // custom mode
                               _mavState);          // MAV_STATE

    int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
    QByteArray bytes((char *)buffer, cBuffer);
    emit bytesReceived(this, bytes);
}

/// @brief Called when QGC wants to write bytes to the MAV
void MockLink::writeBytes(const char* bytes, qint64 cBytes)
{
    // Package up the data so we can signal it over to the right thread
    QByteArray byteArray(bytes, cBytes);

    emit _incomingBytes(byteArray);
}

/// @brief Handles bytes from QGC on the thread
void MockLink::_handleIncomingBytes(const QByteArray bytes)
{
    if (_inNSH) {
        _handleIncomingNSHBytes(bytes.constData(), bytes.count());
    } else {
        if (bytes.startsWith(QByteArray("\r\r\r"))) {
            _inNSH  = true;
            _handleIncomingNSHBytes(&bytes.constData()[3], bytes.count() - 3);
        }

        _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count());
    }
}

/// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell
void MockLink::_handleIncomingNSHBytes(const char* bytes, int cBytes)
{
    Q_UNUSED(cBytes);

    // Drop back out of NSH
    if (cBytes == 4 && bytes[0] == '\r' && bytes[1] == '\r' && bytes[2] == '\r') {
        _inNSH  = false;
        return;
    }

    if (cBytes > 0) {
        qDebug() << "NSH:" << (const char*)bytes;

        if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
            // This is the mavlink start command
            _mavlinkStarted = true;
        }
    }
}

/// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol
void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
{
    mavlink_message_t msg;
    mavlink_status_t comm;

    for (qint64 i=0; i<cBytes; i++)
    {
        if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) {
            continue;
        }

        Q_ASSERT(_missionItemHandler);
        _missionItemHandler->handleMessage(msg);

        switch (msg.msgid) {
            case MAVLINK_MSG_ID_HEARTBEAT:
                _handleHeartBeat(msg);
                break;

            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
                _handleParamRequestList(msg);
                break;

            case MAVLINK_MSG_ID_SET_MODE:
                _handleSetMode(msg);
                break;

            case MAVLINK_MSG_ID_PARAM_SET:
                _handleParamSet(msg);
                break;

            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
                _handleParamRequestRead(msg);
                break;

            case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
                _handleMissionRequestList(msg);
                break;

            case MAVLINK_MSG_ID_MISSION_REQUEST:
                _handleMissionRequest(msg);
                break;

            case MAVLINK_MSG_ID_MISSION_ITEM:
                _handleMissionItem(msg);
                break;

#if 0
            case MAVLINK_MSG_ID_MISSION_COUNT:
                _handleMissionCount(msg);
                break;
#endif

            default:
                qDebug() << "MockLink: Unhandled mavlink message, id:" << msg.msgid;
                break;
        }
    }
}

void MockLink::_emitMavlinkMessage(const mavlink_message_t& msg)
{
    uint8_t outputBuffer[MAVLINK_MAX_PACKET_LEN];

    int cBuffer = mavlink_msg_to_send_buffer(outputBuffer, &msg);
    QByteArray bytes((char *)outputBuffer, cBuffer);
    emit bytesReceived(this, bytes);
}

void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
{
    Q_UNUSED(msg);
#if 0
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(&msg, &heartbeat);
#endif
}

void MockLink::_handleSetMode(const mavlink_message_t& msg)
{
    mavlink_set_mode_t request;
    mavlink_msg_set_mode_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);

    _mavBaseMode = request.base_mode;
    _mavCustomMode = request.custom_mode;
}

void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
{
    mavlink_param_union_t   valueUnion;

    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));

    valueUnion.param_float = paramFloat;

    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];

    QVariant paramVariant;

    switch (paramType) {
        case MAV_PARAM_TYPE_INT8:
            paramVariant = QVariant::fromValue(valueUnion.param_int8);
            break;

        case MAV_PARAM_TYPE_INT32:
            paramVariant = QVariant::fromValue(valueUnion.param_int32);
            break;

        case MAV_PARAM_TYPE_UINT32:
            paramVariant = QVariant::fromValue(valueUnion.param_uint32);
            break;

        case MAV_PARAM_TYPE_REAL32:
            paramVariant = QVariant::fromValue(valueUnion.param_float);
            break;

        default:
            qCritical() << "Invalid parameter type" << paramType;
    }

    qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
    _mapParamName2Value[componentId][paramName] = paramVariant;
}

/// Convert from a parameter variant to the float value from mavlink_param_union_t
float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
{
    mavlink_param_union_t   valueUnion;

    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));

    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
    QVariant paramVar = _mapParamName2Value[componentId][paramName];

    switch (paramType) {
        case MAV_PARAM_TYPE_INT8:
            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
            } else {
                valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
            }
            break;

        case MAV_PARAM_TYPE_INT32:
            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                valueUnion.param_float = paramVar.toInt();
            } else {
                valueUnion.param_int32 = paramVar.toInt();
            }
            break;

        case MAV_PARAM_TYPE_UINT32:
            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                valueUnion.param_float = paramVar.toUInt();
            } else {
                valueUnion.param_uint32 = paramVar.toUInt();
            }
            break;

        case MAV_PARAM_TYPE_REAL32:
                valueUnion.param_float = paramVar.toFloat();
            break;

        default:
            qCritical() << "Invalid parameter type" << paramType;
    }

    return valueUnion.param_float;
}

void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
{
    mavlink_param_request_list_t request;

    mavlink_msg_param_request_list_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);
    Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);

    foreach (int componentId, _mapParamName2Value.keys()) {
        uint16_t paramIndex = 0;
        int cParameters = _mapParamName2Value[componentId].count();
        
        foreach(QString paramName, _mapParamName2Value[componentId].keys()) {
            char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
            mavlink_message_t       responseMsg;

            Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
            Q_ASSERT(_mapParamName2MavParamType.contains(paramName));

            MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];

            Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
            strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);

            qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];

            mavlink_msg_param_value_pack(_vehicleSystemId,
                                         componentId,                       // component id
                                         &responseMsg,                      // Outgoing message
                                         paramId,                           // Parameter name
                                         _floatUnionForParam(componentId, paramName),    // Parameter value
                                         paramType,                         // MAV_PARAM_TYPE
                                         cParameters,                       // Total number of parameters
                                         paramIndex++);                     // Index of this parameter
            _emitMavlinkMessage(responseMsg);
        }
    }
}

void MockLink::_handleParamSet(const mavlink_message_t& msg)
{
    mavlink_param_set_t request;
    mavlink_msg_param_set_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);
    int componentId = request.target_component;

    // Param may not be null terminated if exactly fits
    char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
    strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);

    Q_ASSERT(_mapParamName2Value.contains(componentId));
    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
    Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);

    // Save the new value
    _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);

    // Respond with a param_value to ack
    mavlink_message_t responseMsg;
    mavlink_msg_param_value_pack(_vehicleSystemId,
                                 componentId,                                               // component id
                                 &responseMsg,                                              // Outgoing message
                                 paramId,                                                   // Parameter name
                                 request.param_value,                                       // Send same value back
                                 request.param_type,                                        // Send same type back
                                 _mapParamName2Value[componentId].count(),                  // Total number of parameters
                                 _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
    _emitMavlinkMessage(responseMsg);
}

void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
{
    mavlink_param_request_read_t request;
    mavlink_msg_param_request_read_decode(&msg, &request);
    
    int componentId = request.target_component;
    Q_ASSERT(_mapParamName2Value.contains(componentId));

    char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
    paramId[0] = 0;

    Q_ASSERT(request.target_system == _vehicleSystemId);

    if (request.param_index == -1) {
        // Request is by param name. Param may not be null terminated if exactly fits
        strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
    } else {
        // Request is by index

        Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count());

        QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
        Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
        strcpy(paramId, key.toLocal8Bit().constData());
    }

    Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
    Q_ASSERT(_mapParamName2MavParamType.contains(paramId));

    mavlink_message_t   responseMsg;

    mavlink_msg_param_value_pack(_vehicleSystemId,
                                 componentId,                                               // component id
                                 &responseMsg,                                              // Outgoing message
                                 paramId,                                                   // Parameter name
                                 _floatUnionForParam(componentId, paramId),                 // Parameter value
                                 _mapParamName2MavParamType[paramId],                       // Parameter type
                                 _mapParamName2Value[componentId].count(),                  // Total number of parameters
                                 _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter
    _emitMavlinkMessage(responseMsg);
}

void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
{
    mavlink_mission_request_list_t request;

    mavlink_msg_mission_request_list_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);

    mavlink_message_t   responseMsg;

    mavlink_msg_mission_count_pack(_vehicleSystemId,
                                   _vehicleComponentId,
                                   &responseMsg,            // Outgoing message
                                   msg.sysid,               // Target is original sender
                                   msg.compid,              // Target is original sender
                                   _missionItems.count());  // Number of mission items
    _emitMavlinkMessage(responseMsg);
}

void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
{
    mavlink_mission_request_t request;

    mavlink_msg_mission_request_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);
    Q_ASSERT(request.seq < _missionItems.count());

    mavlink_message_t   responseMsg;

    mavlink_mission_item_t item = _missionItems[request.seq];

    mavlink_msg_mission_item_pack(_vehicleSystemId,
                                  _vehicleComponentId,
                                  &responseMsg,            // Outgoing message
                                  msg.sysid,               // Target is original sender
                                  msg.compid,              // Target is original sender
                                  request.seq,             // Index of mission item being sent
                                  item.frame,
                                  item.command,
                                  item.current,
                                  item.autocontinue,
                                  item.param1, item.param2, item.param3, item.param4,
                                  item.x, item.y, item.z);
    _emitMavlinkMessage(responseMsg);
}

void MockLink::_handleMissionItem(const mavlink_message_t& msg)
{
    mavlink_mission_item_t request;

    mavlink_msg_mission_item_decode(&msg, &request);

    Q_ASSERT(request.target_system == _vehicleSystemId);

    // FIXME: What do you do with duplication sequence numbers?
    Q_ASSERT(!_missionItems.contains(request.seq));

    _missionItems[request.seq] = request;
}

void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
{
    uint16_t chanRaw[18];
    
    for (int i=0; i<18; i++) {
        chanRaw[i] = UINT16_MAX;
    }
    chanRaw[channel] = raw;
    
    mavlink_message_t responseMsg;
    mavlink_msg_rc_channels_pack(_vehicleSystemId,
                                 _vehicleComponentId,
                                 &responseMsg,          // Outgoing message
                                 0,                     // time since boot, ignored
                                 18,                    // channel count
                                 chanRaw[0],            // channel raw value
                                 chanRaw[1],            // channel raw value
                                 chanRaw[2],            // channel raw value
                                 chanRaw[3],            // channel raw value
                                 chanRaw[4],            // channel raw value
                                 chanRaw[5],            // channel raw value
                                 chanRaw[6],            // channel raw value
                                 chanRaw[7],            // channel raw value
                                 chanRaw[8],            // channel raw value
                                 chanRaw[9],            // channel raw value
                                 chanRaw[10],           // channel raw value
                                 chanRaw[11],           // channel raw value
                                 chanRaw[12],           // channel raw value
                                 chanRaw[13],           // channel raw value
                                 chanRaw[14],           // channel raw value
                                 chanRaw[15],           // channel raw value
                                 chanRaw[16],           // channel raw value
                                 chanRaw[17],           // channel raw value
                                 0);                    // rss
    _emitMavlinkMessage(responseMsg);
}