Skip to content
MAVLinkSimulationLink.cc 30.4 KiB
Newer Older
pixhawk's avatar
pixhawk committed
/*=====================================================================
lm's avatar
lm committed
QGroundControl Open Source Ground Control Station
lm's avatar
lm committed
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
lm's avatar
lm committed
This file is part of the QGROUNDCONTROL project
lm's avatar
lm committed
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
    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.
lm's avatar
lm committed
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
    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.
pixhawk's avatar
pixhawk committed
    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
======================================================================*/
pixhawk's avatar
pixhawk committed
/**
 * @file
 *   @brief Implementation of simulated system link
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <cmath>
#include <QTime>
#include <QImage>
#include <QDebug>
#include <QFileInfo>
pixhawk's avatar
pixhawk committed
#include "LinkManager.h"
#include "MAVLinkProtocol.h"
pixhawk's avatar
pixhawk committed
#include "MAVLinkSimulationLink.h"
pixhawk's avatar
pixhawk committed
#include "QGC.h"
#include "MAVLinkSimulationMAV.h"
pixhawk's avatar
pixhawk committed

/**
 * Create a simulated link. This link is connected to an input and output file.
 * The link sends one line at a time at the specified sendrate. The timing of
 * the sendrate is free of drift, which means it is stable on the long run.
 * However, small deviations are mixed in to vary the sendrate slightly
 * in order to simulate disturbances on a real communication link.
 *
 * @param readFile The file with the messages to read (must be in ASCII format, line breaks can be Unix or Windows style)
 * @param writeFile The received messages are written to that file
 * @param rate The rate at which the messages are sent (in intervals of milliseconds)
 **/
MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile, int rate) :
    readyBytes(0),
    timeOffset(0)
pixhawk's avatar
pixhawk committed
{
    this->rate = rate;
    _isConnected = false;

lm's avatar
lm committed
    onboardParams = QMap<QString, float>();
pixhawk's avatar
pixhawk committed
    onboardParams.insert("PID_ROLL_K_P", 0.5f);
    onboardParams.insert("PID_PITCH_K_P", 0.5f);
    onboardParams.insert("PID_YAW_K_P", 0.5f);
    onboardParams.insert("PID_XY_K_P", 100.0f);
pixhawk's avatar
pixhawk committed
    onboardParams.insert("PID_ALT_K_P", 0.5f);
    onboardParams.insert("SYS_TYPE", 1);
    onboardParams.insert("SYS_ID", systemId);
    onboardParams.insert("RC4_REV", 0);
    onboardParams.insert("RC5_REV", 1);
LM's avatar
LM committed
    onboardParams.insert("HDNG2RLL_P", 0.7f);
    onboardParams.insert("HDNG2RLL_I", 0.01f);
    onboardParams.insert("HDNG2RLL_D", 0.02f);
    onboardParams.insert("HDNG2RLL_IMAX", 500.0f);
    onboardParams.insert("RLL2SRV_P", 0.4f);
    onboardParams.insert("RLL2SRV_I", 0.0f);
    onboardParams.insert("RLL2SRV_D", 0.0f);
    onboardParams.insert("RLL2SRV_IMAX", 500.0f);
lm's avatar
lm committed

pixhawk's avatar
pixhawk committed
    // Comments on the variables can be found in the header file

pixhawk's avatar
pixhawk committed
    simulationFile = new QFile(readFile, this);
Lorenz Meier's avatar
Lorenz Meier committed
    if (simulationFile->exists() && simulationFile->open(QIODevice::ReadOnly | QIODevice::Text))
    {
pixhawk's avatar
pixhawk committed
        simulationHeader = simulationFile->readLine();
    }
    receiveFile = new QFile(writeFile, this);
    lastSent = QGC::groundTimeMilliseconds();
pixhawk's avatar
pixhawk committed

Lorenz Meier's avatar
Lorenz Meier committed
    if (simulationFile->exists())
    {
pixhawk's avatar
pixhawk committed
        this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName();
Lorenz Meier's avatar
Lorenz Meier committed
    }
    else
    {
pixhawk's avatar
pixhawk committed
        this->name = "MAVLink simulation link";
    }

pixhawk's avatar
pixhawk committed
    // Initialize the pseudo-random number generator
    srand(QTime::currentTime().msec());
    maxTimeNoise = 0;
pixhawk's avatar
pixhawk committed
    this->id = getNextLinkId();
    LinkManager::instance()->addLink(this);
pixhawk's avatar
pixhawk committed
}

MAVLinkSimulationLink::~MAVLinkSimulationLink()
{
    //TODO Check destructor
    //    fileStream->flush();
    //    outStream->flush();
Lorenz Meier's avatar
Lorenz Meier committed
    // Force termination, there is no
    // need for cleanup since
    // this thread is not manipulating
    // any relevant data
    terminate();
lm's avatar
lm committed
    delete simulationFile;
pixhawk's avatar
pixhawk committed
}

void MAVLinkSimulationLink::run()
{
    status.voltage_battery = 0;
    status.errors_comm = 0;
    system.base_mode = MAV_MODE_PREFLIGHT;
    system.custom_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED;
    system.system_status = MAV_STATE_UNINIT;
Lorenz Meier's avatar
Lorenz Meier committed
    forever
    {
pixhawk's avatar
pixhawk committed
        static quint64 last = 0;

Lorenz Meier's avatar
Lorenz Meier committed
        if (QGC::groundTimeMilliseconds() - last >= rate)
        {
            if (_isConnected)
            {
pixhawk's avatar
pixhawk committed
                mainloop();
pixhawk's avatar
pixhawk committed
            }
Lorenz Meier's avatar
Lorenz Meier committed
            else
            {
                // Sleep for substantially longer
                // if not connected
                QGC::SLEEP::msleep(500);
            }
            last = QGC::groundTimeMilliseconds();
pixhawk's avatar
pixhawk committed
        }
        QGC::SLEEP::msleep(3);
pixhawk's avatar
pixhawk committed
    }
}

void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
{
    // Allocate buffer with packet data
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);

    // Pack to link buffer
pixhawk's avatar
pixhawk committed
    readyBufferMutex.lock();
Lorenz Meier's avatar
Lorenz Meier committed
    for (unsigned int i = 0; i < bufferlength; i++)
    {
pixhawk's avatar
pixhawk committed
    readyBufferMutex.unlock();
pixhawk's avatar
pixhawk committed
void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
{
    // Allocate buffer with packet data
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
pixhawk's avatar
pixhawk committed
    unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
pixhawk's avatar
pixhawk committed
    //add data into datastream
    memcpy(stream+(*index),buf, bufferlength);
    (*index) += bufferlength;
}

pixhawk's avatar
pixhawk committed
void MAVLinkSimulationLink::mainloop()
{

    // Test for encoding / decoding packets

    // Test data stream
pixhawk's avatar
pixhawk committed

    // Fake system values

    static float fullVoltage = 4.2f * 3.0f;
    static float emptyVoltage = 3.35f * 3.0f;
pixhawk's avatar
pixhawk committed
    static float voltage = fullVoltage;
    static float drainRate = 0.025f; // x.xx% of the capacity is linearly drained per second
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
    mavlink_attitude_t attitude;
    memset(&attitude, 0, sizeof(mavlink_attitude_t));
pixhawk's avatar
pixhawk committed
    mavlink_raw_imu_t rawImuValues;
    memset(&rawImuValues, 0, sizeof(mavlink_raw_imu_t));
pixhawk's avatar
pixhawk committed

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength;
    mavlink_message_t msg;

    // Timers
    static unsigned int rate1hzCounter = 1;
    static unsigned int rate10hzCounter = 1;
    static unsigned int rate50hzCounter = 1;
    static unsigned int circleCounter = 0;
pixhawk's avatar
pixhawk committed

    // Vary values

    // VOLTAGE
    // The battery is drained constantly
    voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate);
    if (voltage < 3.550f * 3.0f) voltage = 3.550f * 3.0f;
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
    static int state = 0;
pixhawk's avatar
pixhawk committed

Lorenz Meier's avatar
Lorenz Meier committed
    if (state == 0)
    {
pixhawk's avatar
pixhawk committed
        state++;
    }


    // 50 HZ TASKS
Lorenz Meier's avatar
Lorenz Meier committed
    if (rate50hzCounter == 1000 / rate / 40)
    {
        if (simulationFile->isOpen())
        {
            if (simulationFile->atEnd()) {
pixhawk's avatar
pixhawk committed
                // We reached the end of the file, start from scratch
                simulationFile->reset();
                simulationHeader = simulationFile->readLine();
            }

            // Data was made available, read one line
            // first entry is the timestamp
            QString values = QString(simulationFile->readLine());
            QStringList parts = values.split("\t");
            QStringList keys = simulationHeader.split("\t");
            //qDebug() << simulationHeader;
            //qDebug() << values;
            bool ok;
            static quint64 lastTime = 0;
            static quint64 baseTime = 0;
            quint64 time = QString(parts.first()).toLongLong(&ok, 10);
            // FIXME Remove multiplicaton by 1000
            time *= 1000;
            if (ok) {
                if (timeOffset == 0) {
pixhawk's avatar
pixhawk committed
                    timeOffset = time;
                    baseTime = time;
                }

                if (lastTime > time) {
pixhawk's avatar
pixhawk committed
                    // We have wrapped around in the logfile
                    // Add the measurement time interval to the base time
                    baseTime += lastTime - timeOffset;
                }
                lastTime = time;

                time = time - timeOffset + baseTime;

                // Gather individual measurement values
                for (int i = 1; i < (parts.size() - 1); ++i) {
pixhawk's avatar
pixhawk committed
                    // Get one data field
                    bool res;
                    double d = QString(parts.at(i)).toDouble(&res);
                    if (!res) d = 0;

                    if (keys.value(i, "") == "Accel._X") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.xacc = d;
                    }

                    if (keys.value(i, "") == "Accel._Y") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.yacc = d;
                    }

                    if (keys.value(i, "") == "Accel._Z") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.zacc = d;
                    }
                    if (keys.value(i, "") == "Gyro_Phi") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.xgyro = d;
                        attitude.rollspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
                    if (keys.value(i, "") == "Gyro_Theta") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.ygyro = d;
                        attitude.pitchspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65;
                    if (keys.value(i, "") == "Gyro_Psi") {
pixhawk's avatar
pixhawk committed
                        rawImuValues.zgyro = d;
                        attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65;
pixhawk's avatar
pixhawk committed
                    }
                    if (keys.value(i, "") == "roll_IMU") {
pixhawk's avatar
pixhawk committed
                        attitude.roll = d;
                    }

                    if (keys.value(i, "") == "pitch_IMU") {
pixhawk's avatar
pixhawk committed
                        attitude.pitch = d;
                    }

                    if (keys.value(i, "") == "yaw_IMU") {
pixhawk's avatar
pixhawk committed
                        attitude.yaw = d;
                    }

                    //Accel._X	Accel._Y	Accel._Z	Battery	Bottom_Rotor	CPU_Load	Ground_Dist.	Gyro_Phi	Gyro_Psi	Gyro_Theta	Left_Servo	Mag._X	Mag._Y	Mag._Z	Pressure	Right_Servo	Temperature	Top_Rotor	pitch_IMU	roll_IMU	yaw_IMU

                }
                // Send out packets


                // ATTITUDE
                attitude.time_boot_ms = time/1000;
pixhawk's avatar
pixhawk committed
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
                mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude);
pixhawk's avatar
pixhawk committed
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

                // IMU
                rawImuValues.time_usec = time;
pixhawk's avatar
pixhawk committed
                rawImuValues.xmag = 0;
                rawImuValues.ymag = 0;
                rawImuValues.zmag = 0;
                // Pack message and get size of encoded byte string
pixhawk's avatar
pixhawk committed
                mavlink_msg_raw_imu_encode(systemId, componentId, &msg, &rawImuValues);
pixhawk's avatar
pixhawk committed
                // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
                bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
                //add data into datastream
                memcpy(stream+streampointer,buffer, bufferlength);
                streampointer += bufferlength;

                //qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;

                //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
pixhawk's avatar
pixhawk committed
        }
pixhawk's avatar
pixhawk committed

        rate50hzCounter = 1;
    }


    // 10 HZ TASKS
    if (rate10hzCounter == 1000 / rate / 9) {
pixhawk's avatar
pixhawk committed
        rate10hzCounter = 1;
        double lastX = x;
        double lastY = y;
        double lastZ = z;
        double hackDt = 0.1f; // 100 ms
pixhawk's avatar
pixhawk committed
        // Move X Position
        x = 12.0*sin(((double)circleCounter)/200.0);
        y = 5.0*cos(((double)circleCounter)/200.0);
        z = 1.8 + 1.2*sin(((double)circleCounter)/200.0);
        double xSpeed = (x - lastX)/hackDt;
        double ySpeed = (y - lastY)/hackDt;
        double zSpeed = (z - lastZ)/hackDt;
        circleCounter++;

//        x = (x > 5.0f) ? 5.0f : x;
//        y = (y > 5.0f) ? 5.0f : y;
//        z = (z > 3.0f) ? 3.0f : z;
//        x = (x < -5.0f) ? -5.0f : x;
//        y = (y < -5.0f) ? -5.0f : y;
//        z = (z < -3.0f) ? -3.0f : z;
pixhawk's avatar
pixhawk committed
        mavlink_message_t ret;

        // Send back new position
lm's avatar
lm committed
        mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
//        // GPS RAW
//        mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
pixhawk's avatar
pixhawk committed
        // GLOBAL POSITION
lm's avatar
lm committed
        mavlink_msg_global_position_int_pack(systemId, componentId, &ret, 0, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        // GLOBAL POSITION VEHICLE 2
        mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
//        // ATTITUDE VEHICLE 2
//        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
//        sendMAVLinkMessage(&ret);
//        mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//        //add data into datastream
//        memcpy(stream+streampointer,buffer, bufferlength);
//        streampointer += bufferlength;
        static int rcCounter = 0;
        if (rcCounter == 2) {
            mavlink_rc_channels_t chan;
lm's avatar
lm committed
            chan.time_boot_ms = 0;
            chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000);
            chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000);
            chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000);
            chan.chan4_raw = (chan.chan1_raw + chan.chan2_raw) / 2.0f;
            chan.chan5_raw = (chan.chan3_raw + chan.chan4_raw) / 2.0f;
            chan.chan6_raw = (chan.chan3_raw + chan.chan2_raw) / 2.0f;
            chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f;
            chan.chan8_raw = 0;
pixhawk's avatar
pixhawk committed
            chan.rssi = 100;
            mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &chan);
            // Allocate buffer with packet data
            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
            //add data into datastream
            memcpy(stream+streampointer,buffer, bufferlength);
            streampointer += bufferlength;
            rcCounter = 0;
        }
        rcCounter++;

pixhawk's avatar
pixhawk committed
    }

    // 1 HZ TASKS
    if (rate1hzCounter == 1000 / rate / 1) {
pixhawk's avatar
pixhawk committed
        // STATE
        static int statusCounter = 0;
        if (statusCounter == 100) {
            system.base_mode = (system.base_mode + 1) % MAV_MODE_ENUM_END;
pixhawk's avatar
pixhawk committed
            statusCounter = 0;
        }
        statusCounter++;

        static int detectionCounter = 6;
        if (detectionCounter % 10 == 0) {
        status.voltage_battery = voltage * 1000; // millivolts
        status.load = 33 * detectionCounter % 1000;
pixhawk's avatar
pixhawk committed

        // Pack message and get size of encoded byte string
        mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        // Pack debug text message
pixhawk's avatar
pixhawk committed
        mavlink_statustext_t text;
        text.severity = 0;
        strcpy((char*)(text.text), "Text message from system 32");
pixhawk's avatar
pixhawk committed
        mavlink_msg_statustext_encode(systemId, componentId, &msg, &text);
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
        /*
        // Pack message and get size of encoded byte string
        mavlink_msg_boot_pack(systemId, componentId, &msg, version);
pixhawk's avatar
pixhawk committed
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;*/

pixhawk's avatar
pixhawk committed
        // HEARTBEAT

pixhawk's avatar
pixhawk committed
        static int typeCounter = 0;
        uint8_t mavType;
lm's avatar
lm committed
        if (typeCounter < 10)
        {
lm's avatar
lm committed
            mavType = MAV_TYPE_QUADROTOR;
lm's avatar
lm committed
        }
        else
        {
            mavType = typeCounter % (MAV_TYPE_GCS);
pixhawk's avatar
pixhawk committed
        typeCounter++;

pixhawk's avatar
pixhawk committed
        // Pack message and get size of encoded byte string
        mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK, system.base_mode, system.custom_mode, system.system_status);
pixhawk's avatar
pixhawk committed
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
lm's avatar
lm committed
        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        // Pack message and get size of encoded byte string
        mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
        // Allocate buffer with packet data
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
        // Send controller states

        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
        memcpy(stream+streampointer, buffer, bufferlength);
        streampointer += bufferlength;

pixhawk's avatar
pixhawk committed
        // Pack message and get size of encoded byte string
        mavlink_msg_sys_status_encode(54, componentId, &msg, &status);
pixhawk's avatar
pixhawk committed
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
        rate1hzCounter = 1;
pixhawk's avatar
pixhawk committed
    }

    // FULL RATE TASKS
    // Default is 50 Hz

pixhawk's avatar
pixhawk committed
    /*
pixhawk's avatar
pixhawk committed
    // 50 HZ TASKS
    if (rate50hzCounter == 1000 / rate / 50)
    {

pixhawk's avatar
pixhawk committed
        //streampointer = 0;
pixhawk's avatar
pixhawk committed

        // Attitude

        // Pack message and get size of encoded byte string
        mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0);
pixhawk's avatar
pixhawk committed
        // Allocate buffer with packet data
pixhawk's avatar
pixhawk committed
        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
pixhawk's avatar
pixhawk committed
        //add data into datastream
        memcpy(stream+streampointer,buffer, bufferlength);
        streampointer += bufferlength;

        rate50hzCounter = 1;
pixhawk's avatar
pixhawk committed
    }*/
pixhawk's avatar
pixhawk committed

    readyBufferMutex.lock();
    for (unsigned int i = 0; i < streampointer; i++) {
pixhawk's avatar
pixhawk committed
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

    // Increment counters after full main loop
    rate1hzCounter++;
    rate10hzCounter++;
    rate50hzCounter++;
}


void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
{
pixhawk's avatar
pixhawk committed
    // Parse bytes
    mavlink_message_t msg;
    uint8_t stream[2048];
    int streampointer = 0;
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int bufferlength = 0;
    
    // Initialize drop count to 0 so it isn't referenced uninitialized when returned at the bottom of this function
    comm.packet_rx_drop_count = 0;
pixhawk's avatar
pixhawk committed
    // Output all bytes as hex digits
LM's avatar
LM committed
    for (int i=0; i<size; i++)
lm's avatar
lm committed
    {
        if (mavlink_parse_char(this->id, data[i], &msg, &comm))
        {
pixhawk's avatar
pixhawk committed
            // MESSAGE RECEIVED!
//            qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
LM's avatar
LM committed
            switch (msg.msgid)
            {
pixhawk's avatar
pixhawk committed
                // SET THE SYSTEM MODE
LM's avatar
LM committed
            case MAVLINK_MSG_ID_SET_MODE:
            {
                mavlink_set_mode_t mode;
                mavlink_msg_set_mode_decode(&msg, &mode);
                // Set mode indepent of mode.target
                system.base_mode = mode.base_mode;
            }
            break;
            // EXECUTE OPERATOR ACTIONS
LM's avatar
LM committed
            case MAVLINK_MSG_ID_COMMAND_LONG:
            {
pixhawk's avatar
pixhawk committed
                mavlink_command_long_t action;
                mavlink_msg_command_long_decode(&msg, &action);
//                qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
lm's avatar
lm committed
                // FIXME MAVLINKV10PORTINGNEEDED
//                switch (action.action) {
//                case MAV_ACTION_LAUNCH:
//                    status.status = MAV_STATE_ACTIVE;
//                    status.mode = MAV_MODE_AUTO;
//                    break;
//                case MAV_ACTION_RETURN:
//                    status.status = MAV_STATE_ACTIVE;
//                    break;
//                case MAV_ACTION_MOTORS_START:
//                    status.status = MAV_STATE_ACTIVE;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                case MAV_ACTION_MOTORS_STOP:
//                    status.status = MAV_STATE_STANDBY;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                case MAV_ACTION_EMCY_KILL:
//                    status.status = MAV_STATE_EMERGENCY;
//                    status.mode = MAV_MODE_MANUAL;
//                    break;
//                case MAV_ACTION_SHUTDOWN:
//                    status.status = MAV_STATE_POWEROFF;
//                    status.mode = MAV_MODE_LOCKED;
//                    break;
//                }
LM's avatar
LM committed
            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
            {
//                qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
                mavlink_param_request_list_t read;
                mavlink_msg_param_request_list_decode(&msg, &read);
LM's avatar
LM committed
                if (read.target_system == systemId)
                {
                    // Output all params
                    // Iterate through all components, through all parameters and emit them
                    QMap<QString, float>::iterator i;
                    // Iterate through all components / subsystems
                    int j = 0;
                    for (i = onboardParams.begin(); i != onboardParams.end(); ++i) {
                        if (j != 5) {
                            // Pack message and get size of encoded byte string
                            mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), MAV_PARAM_TYPE_REAL32, onboardParams.size(), j);
LM's avatar
LM committed
                            // Allocate buffer with packet data
                            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                            //add data into datastream
                            memcpy(stream+streampointer,buffer, bufferlength);
                            streampointer+=bufferlength;
                        }
                        j++;
//                    qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
LM's avatar
LM committed
                }
LM's avatar
LM committed
                break;
            case MAVLINK_MSG_ID_PARAM_SET:
            {
                // Drop on even milliseconds
LM's avatar
LM committed
                if (QGC::groundTimeMilliseconds() % 2 == 0)
                {
//                    qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER";
                    mavlink_param_set_t set;
                    mavlink_msg_param_set_decode(&msg, &set);
                    //                    if (set.target_system == systemId)
                    //                    {
                    QString key = QString((char*)set.param_id);
LM's avatar
LM committed
                    if (onboardParams.contains(key))
                    {
                        onboardParams.remove(key);
                        onboardParams.insert(key, set.param_value);

                        // Pack message and get size of encoded byte string
                        mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
                        // Allocate buffer with packet data
                        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                        //add data into datastream
                        memcpy(stream+streampointer,buffer, bufferlength);
                        streampointer+=bufferlength;
                    }
LM's avatar
LM committed
            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
            {
//                qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
                mavlink_param_request_read_t read;
                mavlink_msg_param_request_read_decode(&msg, &read);
                QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
                QString key = QString(bytes);
LM's avatar
LM committed
                if (onboardParams.contains(key))
                {
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
                    // Allocate buffer with packet data
                    bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                    //add data into datastream
                    memcpy(stream+streampointer,buffer, bufferlength);
                    streampointer+=bufferlength;
                    //qDebug() << "Sending PARAM" << key;
LM's avatar
LM committed
                }
                else if (read.param_index >= 0 && read.param_index < onboardParams.keys().size())
LM's avatar
LM committed
                {
                    key = onboardParams.keys().at(read.param_index);
                    float paramValue = onboardParams.value(key);

                    // Pack message and get size of encoded byte string
                    mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, MAV_PARAM_TYPE_REAL32, onboardParams.size(), onboardParams.keys().indexOf(key));
                    // Allocate buffer with packet data
                    bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
                    //add data into datastream
                    memcpy(stream+streampointer,buffer, bufferlength);
                    streampointer+=bufferlength;
                    //qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key;
pixhawk's avatar
pixhawk committed
    }
    // Log the amount and time written out for future data rate calculations.
    // While this interface doesn't actually write any data to external systems,
    // this data "transmit" here should still count towards the outgoing data rate.
    QMutexLocker dataRateLocker(&dataRateMutex);
    logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch());

LM's avatar
LM committed
    for (int i = 0; i < streampointer; i++)
    {
        readyBuffer.enqueue(*(stream + i));
    }
    readyBufferMutex.unlock();

pixhawk's avatar
pixhawk committed
    // Update comm status
    status.errors_comm = comm.packet_rx_drop_count;
pixhawk's avatar
pixhawk committed
}


void MAVLinkSimulationLink::readBytes()
{
pixhawk's avatar
pixhawk committed
    // Lock concurrent resource readyBuffer
    readyBufferMutex.lock();
    const qint64 maxLength = 2048;
    char data[maxLength];
pixhawk's avatar
pixhawk committed
    qint64 len = qMin((qint64)readyBuffer.size(), maxLength);
pixhawk's avatar
pixhawk committed

    for (unsigned int i = 0; i < len; i++) {
pixhawk's avatar
pixhawk committed
        *(data + i) = readyBuffer.takeFirst();
    }

    QByteArray b(data, len);
    emit bytesReceived(this, b);
    readyBufferMutex.unlock();

    // Log the amount and time received for future data rate calculations.
    QMutexLocker dataRateLocker(&dataRateMutex);
    logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, len, QDateTime::currentMSecsSinceEpoch());

pixhawk's avatar
pixhawk committed
}

/**
 * Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection
 * couldn't be disconnected.
 **/
bool MAVLinkSimulationLink::_disconnect(void)
pixhawk's avatar
pixhawk committed

Lorenz Meier's avatar
Lorenz Meier committed
    if(isConnected())
    {
pixhawk's avatar
pixhawk committed
        //        timer->stop();

        _isConnected = false;

        emit disconnected();

pixhawk's avatar
pixhawk committed
    }

    return true;
}

/**
 * Connect the link.
 *
 * @return True if connection has been established, false if connection
 * couldn't be established.
 **/
bool MAVLinkSimulationLink::_connect(void)
pixhawk's avatar
pixhawk committed
{
    _isConnected = true;
pixhawk's avatar
pixhawk committed

    start(LowPriority);
LM's avatar
LM committed
    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883);
//    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
//    Q_UNUSED(mav2);
pixhawk's avatar
pixhawk committed
    //    timer->start(rate);
    return true;
}

/**
 * Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
bool MAVLinkSimulationLink::isConnected() const
pixhawk's avatar
pixhawk committed
    return _isConnected;
}

int MAVLinkSimulationLink::getId() const
pixhawk's avatar
pixhawk committed
{
    return id;
}

QString MAVLinkSimulationLink::getName() const
pixhawk's avatar
pixhawk committed
{
    return name;
}

qint64 MAVLinkSimulationLink::getConnectionSpeed() const
pixhawk's avatar
pixhawk committed
    /* 100 Mbit is reasonable fast and sufficient for all embedded applications */
    return 100000000;
}

qint64 MAVLinkSimulationLink::getCurrentInDataRate() const
{
    return 0;
}

qint64 MAVLinkSimulationLink::getCurrentOutDataRate() const
{
    return 0;
}