Skip to content
MAVLinkSimulationLink.h 3.98 KiB
Newer Older
pixhawk's avatar
pixhawk committed
/*=====================================================================

QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed

This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk 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.

    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.

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

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

/**
 * @file
 *   @brief Definition of MAVLinkSimulationLink
pixhawk's avatar
pixhawk committed
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#ifndef MAVLINKSIMULATIONLINK_H
#define MAVLINKSIMULATIONLINK_H

#include <QFile>
#include <QTimer>
#include <QTextStream>
#include <QQueue>
#include <QMutex>
lm's avatar
lm committed
#include <QMap>
pixhawk's avatar
pixhawk committed
#include <inttypes.h>
pixhawk's avatar
pixhawk committed
#include "QGCMAVLink.h"
lm's avatar
lm committed

pixhawk's avatar
pixhawk committed
#include "LinkInterface.h"
pixhawk's avatar
pixhawk committed

class MAVLinkSimulationLink : public LinkInterface
{
pixhawk's avatar
pixhawk committed
    Q_OBJECT
pixhawk's avatar
pixhawk committed
public:
pixhawk's avatar
pixhawk committed
    MAVLinkSimulationLink(QString readFile="", QString writeFile="", int rate=5, QObject* parent = 0);
pixhawk's avatar
pixhawk committed
    ~MAVLinkSimulationLink();
    bool isConnected();
    qint64 bytesAvailable();

    void run();

    bool connect();
    bool disconnect();

    /* Extensive statistics for scientific purposes */
    qint64 getNominalDataRate();
    qint64 getTotalUpstream();
    qint64 getShortTermUpstream();
    qint64 getCurrentUpstream();
    qint64 getMaxUpstream();
    qint64 getTotalDownstream();
    qint64 getShortTermDownstream();
    qint64 getCurrentDownstream();
    qint64 getMaxDownstream();
    qint64 getBitsSent();
    qint64 getBitsReceived();

    QString getName();
    int getId();
    int getBaudRate();
    int getBaudRateType();
    int getFlowType();
    int getParityType();
    int getDataBitsType();
    int getStopBitsType();

    int getLinkQuality();
    bool isFullDuplex();

public slots:
    void writeBytes(const char* data, qint64 size);
    /** @brief Mainloop simulating the mainloop of the MAV */
pixhawk's avatar
pixhawk committed
    virtual void mainloop();
    bool connectLink(bool connect);
    void sendMAVLinkMessage(const mavlink_message_t* msg);
pixhawk's avatar
pixhawk committed


protected:

    // UAS properties
    float roll, pitch, yaw;
    double x, y, z;
    double spX, spY, spZ, spYaw;
pixhawk's avatar
pixhawk committed
    int battery;

    QTimer* timer;
    /** File which contains the input data (simulated robot messages) **/
    QFile* simulationFile;
    QFile* mavlinkLogFile;
pixhawk's avatar
pixhawk committed
    QString simulationHeader;
    /** File where the commands sent by the groundstation are stored **/
    QFile* receiveFile;
pixhawk's avatar
pixhawk committed
    QTextStream* fileStream;
    QTextStream* outStream;
    /** Buffer which can be read from connected protocols through readBytes(). **/
    QMutex readyBufferMutex;
    bool _isConnected;
    quint64 rate;
    int maxTimeNoise;
    quint64 lastSent;
    static const int streamlength = 4096;
    unsigned int streampointer;
    //const int testoffset = 0;
    uint8_t stream[streamlength];
pixhawk's avatar
pixhawk committed

    int readyBytes;
    QQueue<uint8_t> readyBuffer;

    int id;
    QString name;
pixhawk's avatar
pixhawk committed
    qint64 timeOffset;
pixhawk's avatar
pixhawk committed
    mavlink_sys_status_t status;
    mavlink_heartbeat_t system;
lm's avatar
lm committed
    QMap<QString, float> onboardParams;
pixhawk's avatar
pixhawk committed

pixhawk's avatar
pixhawk committed
    void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg);

    static const uint8_t systemId = 220;
    static const uint8_t componentId = 200;
pixhawk's avatar
pixhawk committed
    void valueChanged(int uasId, QString curve, double value, quint64 usec);
    void messageReceived(const mavlink_message_t& message);
pixhawk's avatar
pixhawk committed

};

#endif // MAVLINKSIMULATIONLINK_H