UAS.h 12.6 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

Donald Gagne's avatar
Donald Gagne committed
10 11 12
// NO NEW CODE HERE
// UASInterface, UAS.h/cc are deprecated. All new functionality should go into Vehicle.h/cc
//
13

14
#pragma once
15 16 17 18 19

#include "UASInterface.h"
#include <MAVLinkProtocol.h>
#include <QVector3D>
#include "QGCMAVLink.h"
20
#include "Vehicle.h"
21
#include "FirmwarePluginManager.h"
22

dogmaphobic's avatar
dogmaphobic committed
23
#ifndef __mobile__
24
#include "FileManager.h"
25
#include "QGCHilLink.h"
26
#include "QGCJSBSimLink.h"
27
#include "QGCXPlaneLink.h"
dogmaphobic's avatar
dogmaphobic committed
28
#endif
29

30
Q_DECLARE_LOGGING_CATEGORY(UASLog)
tstellanova's avatar
tstellanova committed
31

32 33
class Vehicle;

34 35 36 37 38 39 40 41 42 43 44 45
/**
 * @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:
46
    UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager);
47

48 49
    float lipoFull;  ///< 100% charged voltage
    float lipoEmpty; ///< Discharged voltage
50 51 52 53 54 55 56 57 58

    /* MANAGEMENT */

    /** @brief Get the unique system id */
    int getUASID() const;

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

59 60 61
    /// Vehicle is about to go away
    void shutdownVehicle(void);

62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
    // 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;
    }

115
#ifndef __mobile__
116
    friend class FileManager;
117
#endif
118

DonLakeFlyer's avatar
DonLakeFlyer committed
119
protected:
120
    /// LINK ID AND STATUS
121
    int uasId;                    ///< Unique system ID
122

123 124
    QList<int> unknownPackets;    ///< Packet IDs which are unknown and have been received
    MAVLinkProtocol* mavlink;     ///< Reference to the MAVLink instance
125 126
    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
127

128 129
    /// BASIC UAS TYPE, NAME AND STATE
    int status;                   ///< The current status of the MAV
130

131 132
    /// TIMEKEEPING
    quint64 startTime;            ///< The time the UAS was switched on
133 134
    quint64 onboardTimeOffset;

135
    /// MANUAL CONTROL
136 137 138 139 140
    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

141
    /// POSITION
142
    bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
143

144
#ifndef __mobile__
145
    FileManager   fileManager;
146
#endif
147 148 149 150 151

    /// 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
152

153 154
    // dongfang: This looks like a candidate for being moved off to a separate class.
    /// IMAGING
155 156
    int imageSize;              ///< Image size being transmitted (bytes)
    int imagePackets;           ///< Number of data packets being sent for this image
nopeppermint's avatar
nopeppermint committed
157
    int imagePacketsArrived;    ///< Number of data packets received
158 159 160 161 162 163 164 165
    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;
166
    bool blockHomePositionChanges;   ///< Block changes to the home position
167
    bool receivedMode;          ///< True if mode was retrieved from current conenction to UAS
168

169 170 171 172 173 174 175 176 177 178 179 180 181 182 183
    /// 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)

184
    /// SIMULATION
dogmaphobic's avatar
dogmaphobic committed
185
#ifndef __mobile__
186
    QGCHilLink* simulation;         ///< Hardware in the loop simulation link
dogmaphobic's avatar
dogmaphobic committed
187
#endif
188 189 190 191 192

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

193 194 195
#ifndef __mobile__
    virtual FileManager* getFileManager() { return &fileManager; }
#endif
196

197
    /** @brief Get the HIL simulation */
dogmaphobic's avatar
dogmaphobic committed
198
#ifndef __mobile__
199 200 201
    QGCHilLink* getHILSimulation() const {
        return simulation;
    }
dogmaphobic's avatar
dogmaphobic committed
202
#endif
203

204 205 206 207
    QImage getImage();
    void requestImage();

public slots:
208 209 210
    /** @brief Order the robot to pair its receiver **/
    void pairRX(int rxType, int rxSubType);

211
    /** @brief Enable / disable HIL */
dogmaphobic's avatar
dogmaphobic committed
212
#ifndef __mobile__
213
    void enableHilJSBSim(bool enable, QString options);
214 215 216
    void enableHilXPlane(bool enable);

    /** @brief Send the full HIL state to the MAV */
217 218
    void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
                        float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
219 220 221 222 223
                        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);
224

Lorenz Meier's avatar
Lorenz Meier committed
225
    /** @brief RAW sensors for sensor HIL */
226 227
    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
228

229 230 231 232
    /** @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);

233 234
    float addZeroMeanNoise(float truth_meas, float noise_var);

Lorenz Meier's avatar
Lorenz Meier committed
235 236 237 238 239 240 241 242 243 244 245 246
    /**
     * @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
     */
247
    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
248 249


250 251 252 253 254
    /** @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
255
#endif
256 257

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

    /** @brief Set the values for the 6dof manual control of the vehicle */
dogmaphobic's avatar
dogmaphobic committed
261
#ifndef __mobile__
262
    void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
dogmaphobic's avatar
dogmaphobic committed
263
#endif
264 265

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

268 269
    void startCalibration(StartCalibrationType calType);
    void stopCalibration(void);
270

271 272 273
    void startBusConfig(StartBusConfigType calType);
    void stopBusConfig(void);

274
    /** @brief Send command to map a RC channel to a parameter */
275
    void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax);
276 277 278

    /** @brief Send command to disable all bindings/maps between RC and parameters */
    void unsetRCToParameterMap();
279 280 281 282 283
signals:
    void imageStarted(quint64 timestamp);
    /** @brief A new camera image has arrived */
    void imageReady(UASInterface* uas);
    /** @brief HIL controls have changed */
284
    void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
285

286 287 288
    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
289

290 291 292 293 294 295 296
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);
297

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

Gus Grubba's avatar
Gus Grubba committed
300 301 302
    QMap<int, int>componentID;
    QMap<int, bool>componentMulti;

303 304
    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
305
    quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred
306 307
    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
308
    bool hilEnabled;
309
    bool sensorHil;             ///< True if sensor HIL is enabled
310
    quint64 lastSendTimeGPS;     ///< Last HIL GPS message sent
311 312
    quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent
    quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
313

314
private:
315 316
    Vehicle*                _vehicle;
    FirmwarePluginManager*  _firmwarePluginManager;
317 318 319
};