UAS.h 14.2 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25


/**
 * @file
 *   @brief Definition of Unmanned Aerial Vehicle object
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#ifndef _UAS_H_
#define _UAS_H_

#include "UASInterface.h"
#include <MAVLinkProtocol.h>
#include <QVector3D>
#include "QGCMAVLink.h"
26
#include "Vehicle.h"
27
#include "FirmwarePluginManager.h"
28

dogmaphobic's avatar
dogmaphobic committed
29
#ifndef __mobile__
30
#include "FileManager.h"
31 32
#include "QGCHilLink.h"
#include "QGCFlightGearLink.h"
33
#include "QGCJSBSimLink.h"
34
#include "QGCXPlaneLink.h"
dogmaphobic's avatar
dogmaphobic committed
35
#endif
36

37
Q_DECLARE_LOGGING_CATEGORY(UASLog)
tstellanova's avatar
tstellanova committed
38

39 40
class Vehicle;

41 42 43 44 45 46 47 48 49 50 51 52
/**
 * @brief A generic MAVLINK-connected MAV/UAV
 *
 * This class represents one vehicle. It can be used like the real vehicle, e.g. a call to halt()
 * will automatically send the appropriate messages to the vehicle. The vehicle state will also be
 * automatically updated by the comm architecture, so when writing code to e.g. control the vehicle
 * no knowledge of the communication infrastructure is needed.
 */
class UAS : public UASInterface
{
    Q_OBJECT
public:
53
    UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager);
54

55 56
    float lipoFull;  ///< 100% charged voltage
    float lipoEmpty; ///< Discharged voltage
57 58 59 60 61 62 63 64 65 66 67

    /* MANAGEMENT */

    /** @brief Get the unique system id */
    int getUASID() const;
    /** @brief Get the components */
    QMap<int, QString> getComponents();

    /** @brief The time interval the robot is switched on */
    quint64 getUptime() const;

68 69 70
    Q_PROPERTY(double   roll                    READ getRoll                WRITE setRoll               NOTIFY rollChanged)
    Q_PROPERTY(double   pitch                   READ getPitch               WRITE setPitch              NOTIFY pitchChanged)
    Q_PROPERTY(double   yaw                     READ getYaw                 WRITE setYaw                NOTIFY yawChanged)
71

Don Gagne's avatar
Don Gagne committed
72 73
    /// Vehicle is about to go away
    void shutdownVehicle(void);
74

75 76 77 78 79 80
    void setRoll(double val)
    {
        roll = val;
        emit rollChanged(val,"roll");
    }

81 82 83 84
    double getRoll() const
    {
        return roll;
    }
85 86 87 88 89 90 91

    void setPitch(double val)
    {
        pitch = val;
        emit pitchChanged(val,"pitch");
    }

92 93 94 95
    double getPitch() const
    {
        return pitch;
    }
96 97 98 99 100 101 102

    void setYaw(double val)
    {
        yaw = val;
        emit yawChanged(val,"yaw");
    }

103 104 105 106
    double getYaw() const
    {
        return yaw;
    }
107

108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160
    // Setters for HIL noise variance
    void setXaccVar(float var){
        xacc_var = var;
    }

    void setYaccVar(float var){
        yacc_var = var;
    }

    void setZaccVar(float var){
        zacc_var = var;
    }

    void setRollSpeedVar(float var){
        rollspeed_var = var;
    }

    void setPitchSpeedVar(float var){
        pitchspeed_var = var;
    }

    void setYawSpeedVar(float var){
        pitchspeed_var = var;
    }

    void setXmagVar(float var){
        xmag_var = var;
    }

    void setYmagVar(float var){
        ymag_var = var;
    }

    void setZmagVar(float var){
        zmag_var = var;
    }

    void setAbsPressureVar(float var){
        abs_pressure_var = var;
    }

    void setDiffPressureVar(float var){
        diff_pressure_var = var;
    }

    void setPressureAltVar(float var){
        pressure_alt_var = var;
    }

    void setTemperatureVar(float var){
        temperature_var = var;
    }

161
#ifndef __mobile__
162
    friend class FileManager;
163
#endif
164 165

protected: //COMMENTS FOR TEST UNIT
166
    /// LINK ID AND STATUS
167
    int uasId;                    ///< Unique system ID
168
    QMap<int, QString> components;///< IDs and names of all detected onboard components
169

170 171
    QList<int> unknownPackets;    ///< Packet IDs which are unknown and have been received
    MAVLinkProtocol* mavlink;     ///< Reference to the MAVLink instance
172 173
    float receiveDropRate;        ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
    float sendDropRate;           ///< Percentage of packets that were not received from the MAV by the GCS
174

175 176
    /// BASIC UAS TYPE, NAME AND STATE
    int status;                   ///< The current status of the MAV
177

178 179
    /// TIMEKEEPING
    quint64 startTime;            ///< The time the UAS was switched on
180 181
    quint64 onboardTimeOffset;

182
    /// MANUAL CONTROL
183 184 185 186 187 188 189 190 191
    bool controlRollManual;     ///< status flag, true if roll is controlled manually
    bool controlPitchManual;    ///< status flag, true if pitch is controlled manually
    bool controlYawManual;      ///< status flag, true if yaw is controlled manually
    bool controlThrustManual;   ///< status flag, true if thrust is controlled manually

    double manualRollAngle;     ///< Roll angle set by human pilot (radians)
    double manualPitchAngle;    ///< Pitch angle set by human pilot (radians)
    double manualYawAngle;      ///< Yaw angle set by human pilot (radians)
    double manualThrust;        ///< Thrust set by human pilot (radians)
192 193

    /// POSITION
194
    bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
195

196
#ifndef __mobile__
197
    FileManager   fileManager;
198
#endif
199 200 201 202 203

    /// ATTITUDE
    bool attitudeKnown;             ///< True if attitude was received, false else
    bool attitudeStamped;           ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
    quint64 lastAttitude;           ///< Timestamp of last attitude measurement
204 205 206 207
    double roll;
    double pitch;
    double yaw;

208 209
    // dongfang: This looks like a candidate for being moved off to a separate class.
    /// IMAGING
210 211
    int imageSize;              ///< Image size being transmitted (bytes)
    int imagePackets;           ///< Number of data packets being sent for this image
nopeppermint's avatar
nopeppermint committed
212
    int imagePacketsArrived;    ///< Number of data packets received
213 214 215 216 217 218 219 220
    int imagePayload;           ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases.
    int imageQuality;           ///< Quality of the transmitted image (percentage)
    int imageType;              ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit)
    int imageWidth;             ///< Width of the image stream
    int imageHeight;            ///< Width of the image stream
    QByteArray imageRecBuffer;  ///< Buffer for the incoming bytestream
    QImage image;               ///< Image data of last completely transmitted image
    quint64 imageStart;
221
    bool blockHomePositionChanges;   ///< Block changes to the home position
222
    bool receivedMode;          ///< True if mode was retrieved from current conenction to UAS
223

224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
    /// SIMULATION NOISE
    float xacc_var;             ///< variance of x acclerometer noise for HIL sim (mg)
    float yacc_var;             ///< variance of y acclerometer noise for HIL sim (mg)
    float zacc_var;             ///< variance of z acclerometer noise for HIL sim (mg)
    float rollspeed_var;        ///< variance of x gyroscope noise for HIL sim (rad/s)
    float pitchspeed_var;       ///< variance of y gyroscope noise for HIL sim (rad/s)
    float yawspeed_var;         ///< variance of z gyroscope noise for HIL sim (rad/s)
    float xmag_var;             ///< variance of x magnatometer noise for HIL sim (???)
    float ymag_var;             ///< variance of y magnatometer noise for HIL sim (???)
    float zmag_var;             ///< variance of z magnatometer noise for HIL sim (???)
    float abs_pressure_var;     ///< variance of absolute pressure noise for HIL sim (hPa)
    float diff_pressure_var;    ///< variance of differential pressure noise for HIL sim (hPa)
    float pressure_alt_var;     ///< variance of altitude pressure noise for HIL sim (hPa)
    float temperature_var;      ///< variance of temperature noise for HIL sim (C)

239
    /// SIMULATION
dogmaphobic's avatar
dogmaphobic committed
240
#ifndef __mobile__
241
    QGCHilLink* simulation;         ///< Hardware in the loop simulation link
dogmaphobic's avatar
dogmaphobic committed
242
#endif
243 244 245 246 247

public:
    /** @brief Get the human-readable status message for this code */
    void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);

248 249 250
#ifndef __mobile__
    virtual FileManager* getFileManager() { return &fileManager; }
#endif
251

252
    /** @brief Get the HIL simulation */
dogmaphobic's avatar
dogmaphobic committed
253
#ifndef __mobile__
254 255 256
    QGCHilLink* getHILSimulation() const {
        return simulation;
    }
dogmaphobic's avatar
dogmaphobic committed
257
#endif
258

259 260 261 262
    QImage getImage();
    void requestImage();

public slots:
263 264 265
    /** @brief Order the robot to pair its receiver **/
    void pairRX(int rxType, int rxSubType);

266
    /** @brief Enable / disable HIL */
dogmaphobic's avatar
dogmaphobic committed
267
#ifndef __mobile__
268
    void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration);
269
    void enableHilJSBSim(bool enable, QString options);
270 271 272
    void enableHilXPlane(bool enable);

    /** @brief Send the full HIL state to the MAV */
273 274
    void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
                        float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
275 276 277 278 279
                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);

    void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
                        float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
280

Lorenz Meier's avatar
Lorenz Meier committed
281
    /** @brief RAW sensors for sensor HIL */
282 283
    void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed);
Lorenz Meier's avatar
Lorenz Meier committed
284

285 286 287 288
    /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/
    void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
                            float flow_comp_m_y, quint8 quality, float ground_distance);

289 290
    float addZeroMeanNoise(float truth_meas, float noise_var);

Lorenz Meier's avatar
Lorenz Meier committed
291 292 293 294 295 296 297 298 299 300 301 302
    /**
     * @param time_us
     * @param lat
     * @param lon
     * @param alt
     * @param fix_type
     * @param eph
     * @param epv
     * @param vel
     * @param cog course over ground, in radians, -pi..pi
     * @param satellites
     */
303
    void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd,  float cog, int satellites);
Lorenz Meier's avatar
Lorenz Meier committed
304 305


306 307 308 309 310
    /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
    void startHil();

    /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/
    void stopHil();
dogmaphobic's avatar
dogmaphobic committed
311
#endif
312 313

    /** @brief Set the values for the manual control of the vehicle */
314
    void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
315 316

    /** @brief Set the values for the 6dof manual control of the vehicle */
dogmaphobic's avatar
dogmaphobic committed
317
#ifndef __mobile__
318
    void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
dogmaphobic's avatar
dogmaphobic committed
319
#endif
320 321

    /** @brief Receive a message from one of the communication links. */
322
    virtual void receiveMessage(mavlink_message_t message);
323

324 325
    void startCalibration(StartCalibrationType calType);
    void stopCalibration(void);
326

327 328 329
    void startBusConfig(StartBusConfigType calType);
    void stopBusConfig(void);

330
    /** @brief Send command to map a RC channel to a parameter */
331
    void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax);
332 333 334

    /** @brief Send command to disable all bindings/maps between RC and parameters */
    void unsetRCToParameterMap();
335 336 337 338 339
signals:
    void imageStarted(quint64 timestamp);
    /** @brief A new camera image has arrived */
    void imageReady(UASInterface* uas);
    /** @brief HIL controls have changed */
340
    void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
341

342 343 344
    void rollChanged(double val,QString name);
    void pitchChanged(double val,QString name);
    void yawChanged(double val,QString name);
Don Gagne's avatar
Don Gagne committed
345

346 347 348 349 350 351 352
protected:
    /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
    quint64 getUnixTime(quint64 time=0);
    /** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */
    quint64 getUnixTimeFromMs(quint64 time);
    /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
    quint64 getUnixReferenceTime(quint64 time);
353

354 355
    virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);

356 357 358 359
    int componentID[256];
    bool componentMulti[256];
    bool connectionLost; ///< Flag indicates a timed out connection
    quint64 connectionLossTime; ///< Time the connection was interrupted
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
360
    quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred
361 362
    quint64 lastNonNullTime;    ///< The last timestamp from the MAV that was not null
    unsigned int onboardTimeOffsetInvalidCount;     ///< Count when the offboard time offset estimation seemed wrong
Don Gagne's avatar
Don Gagne committed
363
    bool hilEnabled;
364
    bool sensorHil;             ///< True if sensor HIL is enabled
365
    quint64 lastSendTimeGPS;     ///< Last HIL GPS message sent
366 367
    quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent
    quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
368

369 370
private:
    void _say(const QString& text, int severity = 6);
371

372
private:
373 374
    Vehicle*                _vehicle;
    FirmwarePluginManager*  _firmwarePluginManager;
375 376 377 378
};


#endif // _UAS_H_