HSIDisplay.h 15.4 KB
Newer Older
1
2
/*=====================================================================

3
QGroundControl Open Source Ground Control Station
4

5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6

7
This file is part of the QGROUNDCONTROL project
8

9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
10
11
12
13
    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.

14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
15
16
17
18
19
    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
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
21
22
23
24
25
26
27
28
29
30
31
32
33
34

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

/**
 * @file
 *   @brief Definition of of Horizontal Situation Indicator class
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#ifndef HSIDISPLAY_H
#define HSIDISPLAY_H

35
#include <QWidget>
36
37
38
39
#include <QColor>
#include <QTimer>
#include <QMap>
#include <QPair>
40
#include <QMouseEvent>
41
42
43
#include <cmath>

#include "HDDisplay.h"
lm's avatar
lm committed
44
#include "MG.h"
45

46
47
class HSIDisplay : public HDDisplay
{
48
49
    Q_OBJECT
public:
50
    HSIDisplay(QWidget *parent = 0);
51
    ~HSIDisplay();
52
53
54

public slots:
    void setActiveUAS(UASInterface* uas);
55
56
    /** @brief Set the width in meters this widget shows from top */
    void setMetricWidth(double width);
lm's avatar
lm committed
57
    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
58
    void updateAttitudeSetpoints(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
lm's avatar
lm committed
59
    void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
60
    void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
lm's avatar
lm committed
61
    void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
lm's avatar
lm committed
62
63
    void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
    void updateGlobalPosition(UASInterface*, double lat, double lon, double alt, quint64 usec);
64
    void updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time);
lm's avatar
lm committed
65
    void updatePositionLock(UASInterface* uas, bool lock);
pixhawk's avatar
pixhawk committed
66
67
68
    void updateAttitudeControllerEnabled(bool enabled);
    void updatePositionXYControllerEnabled(bool enabled);
    void updatePositionZControllerEnabled(bool enabled);
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
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

    /** @brief Optical flow status changed */
    void updateOpticalFlowStatus(bool supported, bool enabled, bool ok) {
        if (supported && enabled && ok) {
            visionFix = true;
        } else {
            visionFix = false;
        }
    }

    /** @brief Vision based localization status changed */
    void updateVisionLocalizationStatus(bool supported, bool enabled, bool ok) {
        if (enabled && ok) {
            visionFix = true;
        } else {
            visionFix = false;
        }
        visionFixKnown = supported;
    }
    /** @brief Infrared / Ultrasound status changed */
    void updateDistanceSensorStatus(bool supported, bool enabled, bool ok) {
        if (enabled && ok) {
            iruFix = true;
        } else {
            iruFix = false;
        }
        iruFixKnown = supported;
    }
    /** @brief Gyroscope status changed */
    void updateGyroStatus(bool supported, bool enabled, bool ok) {
        gyroKnown = supported;
        gyroON = enabled;
        gyroOK = ok;
    }
    /** @brief Accelerometer status changed */
    void updateAccelStatus(bool supported, bool enabled, bool ok) {
        accelKnown = supported;
        accelON = enabled;
        accelOK = ok;
    }
    /** @brief Magnetometer status changed */
    void updateMagSensorStatus(bool supported, bool enabled, bool ok) {
        magKnown = supported;
        magON = enabled;
        magOK = ok;
    }
    /** @brief Barometer status changed */
    void updateBaroStatus(bool supported, bool enabled, bool ok) {
        pressureKnown = supported;
        pressureON = enabled;
        pressureOK = ok;
    }
    /** @brief Differential pressure / airspeed status changed */
    void updateAirspeedStatus(bool supported, bool enabled, bool ok) {
        diffPressureKnown = supported;
        diffPressureON = enabled;
        diffPressureOK = ok;
    }
    /** @brief Actuator status changed */
    void updateActuatorStatus(bool supported, bool enabled, bool ok) {
        actuatorsKnown = supported;
        actuatorsON = enabled;
        actuatorsOK = ok;
    }
    /** @brief Laser scanner status changed */
    void updateLaserStatus(bool supported, bool enabled, bool ok) {
        laserKnown = supported;
        laserON = enabled;
        laserOK = ok;
    }
    /** @brief Vicon / Leica Geotracker status changed */
    void updateGroundTruthSensorStatus(bool supported, bool enabled, bool ok) {
        viconKnown = supported;
        viconON = enabled;
        viconOK = ok;
    }

LM's avatar
LM committed
146
    void updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance);
pixhawk's avatar
pixhawk committed
147
    /** @brief Heading control enabled/disabled */
148
    void updatePositionYawControllerEnabled(bool enabled);
pixhawk's avatar
pixhawk committed
149

150
    /** @brief Localization quality changed */
pixhawk's avatar
pixhawk committed
151
    void updateLocalization(UASInterface* uas, int localization);
152
    /** @brief GPS localization quality changed */
pixhawk's avatar
pixhawk committed
153
    void updateGpsLocalization(UASInterface* uas, int localization);
154
    /** @brief Vision localization quality changed */
pixhawk's avatar
pixhawk committed
155
156
    void updateVisionLocalization(UASInterface* uas, int localization);

157
158
159
    /** @brief Ultrasound/Infrared localization changed */
    void updateInfraredUltrasoundLocalization(UASInterface* uas, int fix);

160
    /** @brief Repaint the widget */
lm's avatar
lm committed
161
    void paintEvent(QPaintEvent * event);
162
163
164
    /** @brief Update state from joystick */
    void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat);
    void pressKey(int key);
165
166
    /** @brief Reset the state of the view */
    void resetMAVState();
167
168
169
170
    /** @brief Clear the status message */
    void clearStatusMessage()
    {
        statusMessage = "";
171
172
        if (actionPending) statusMessage = "TIMED OUT, NO ACTION";
        statusClearTimer.start();
pixhawk's avatar
pixhawk committed
173
        userSetPointSet = false;
pixhawk's avatar
pixhawk committed
174
        actionPending = false;
175
    }
176

177
178
179
signals:
    void metricWidthChanged(double width);

180
protected slots:
181
    void renderOverlay();
182
183
184
185
    void drawGPS(QPainter &painter);
    void drawObjects(QPainter &painter);
    void drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
    void drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
lm's avatar
lm committed
186
    void drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter);
187
188
    /** @brief Draw a status flag indicator */
    void drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter);
189
    void drawStatusFlag(float x, float y, QString label, bool status, bool known, bool ok, QPainter& painter);
190
191
    /** @brief Draw a position lock indicator */
    void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter);
192
    void setBodySetpointCoordinateXY(double x, double y);
193
    void setBodySetpointCoordinateYaw(double yaw);
194
195
196
197
    void setBodySetpointCoordinateZ(double z);
    /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */
    void sendBodySetPointCoordinates();
    /** @brief Draw one setpoint */
198
    void drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter);
199
200
    /** @brief Draw waypoints of this system */
    void drawWaypoints(QPainter& painter);
Lorenz Meier's avatar
Lorenz Meier committed
201
    /** @brief Draw one waypoint */
202
    void drawWaypoint(QPainter& painter, const QColor& color, float width, const Waypoint *w, const QPointF& p);
203
204
    /** @brief Draw the limiting safety area */
    void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight,  const QColor &color, QPainter &painter);
pixhawk's avatar
pixhawk committed
205
    /** @brief Receive mouse clicks */
206
    void mouseDoubleClickEvent(QMouseEvent* event);
207
208
209
    void mousePressEvent(QMouseEvent * event);
    void mouseReleaseEvent(QMouseEvent * event);
    void mouseMoveEvent(QMouseEvent * event);
210
211
    /** @brief Receive mouse wheel events */
    void wheelEvent(QWheelEvent* event);
pixhawk's avatar
pixhawk committed
212
213
    /** @brief Read out send keys */
    void keyPressEvent(QKeyEvent* event);
214
215
    /** @brief Ignore context menu event */
    void contextMenuEvent (QContextMenuEvent* event);
216
217
218
219
220
221
    /** @brief Set status message on screen */
    void setStatusMessage(const QString& message)
    {
        statusMessage = message;
        statusClearTimer.start();
    }
222
223

protected:
224

225
    void showEvent(QShowEvent* event);
226
    void hideEvent(QHideEvent* event);
pixhawk's avatar
pixhawk committed
227
    /** @brief Get color from GPS signal-to-noise colormap */
lm's avatar
lm committed
228
    static QColor getColorForSNR(float snr);
pixhawk's avatar
pixhawk committed
229
230
231
232
    /** @brief Metric world coordinates to metric body coordinates */
    QPointF metricWorldToBody(QPointF world);
    /** @brief Metric body coordinates to metric world coordinates */
    QPointF metricBodyToWorld(QPointF body);
233
234
235
236
237
    /** @brief Screen coordinates of widget to metric coordinates in body frame */
    QPointF screenToMetricBody(QPointF ref);
    /** @brief Reference coordinates to metric coordinates */
    QPointF refToMetricBody(QPointF &ref);
    /** @brief Metric coordinates to reference coordinates */
pixhawk's avatar
pixhawk committed
238
    QPointF metricBodyToRef(QPointF &metric);
239
240
241
242
    /** @brief Metric length to reference coordinates */
    double metricToRef(double metric);
    /** @bried Reference coordinates to metric length */
    double refToMetric(double ref);
pixhawk's avatar
pixhawk committed
243
244
    /** @brief Metric body coordinates to screen coordinates */
    QPointF metricBodyToScreen(QPointF metric);
lm's avatar
lm committed
245
246
247
248
249
250
251

    /**
     * @brief Private data container class to be used within the HSI widget
     */
    class GPSSatellite
    {
    public:
lm's avatar
lm committed
252
        GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) :
253
254
255
256
257
258
            id(id),
            elevation(elevation),
            azimuth(azimuth),
            snr(snr),
            used(used),
            lastUpdate(MG::TIME::getGroundTimeNowUsecs()) {
lm's avatar
lm committed
259
260
261

        }

262
        void update(int id, float elevation, float azimuth, float snr, bool used) {
lm's avatar
lm committed
263
            this->id = id;
lm's avatar
lm committed
264
            this->elevation = elevation;
lm's avatar
lm committed
265
266
267
            this->azimuth = azimuth;
            this->snr = snr;
            this->used = used;
lm's avatar
lm committed
268
            this->lastUpdate = MG::TIME::getGroundTimeNowUsecs();
lm's avatar
lm committed
269
270
271
        }

        int id;
lm's avatar
lm committed
272
        float elevation;
lm's avatar
lm committed
273
274
275
        float azimuth;
        float snr;
        bool used;
lm's avatar
lm committed
276
        quint64 lastUpdate;
lm's avatar
lm committed
277
278
279
280

        friend class HSIDisplay;
    };

281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
    QMap<int, QString> objectNames;
    QMap<int, int> objectTypes;
    QMap<int, float> objectQualities;
    QMap<int, float> objectBearings;
    QMap<int, float> objectDistances;
    bool dragStarted;
    bool leftDragStarted;
    bool mouseHasMoved;
    float startX;
    float startY;
    QTimer statusClearTimer;
    QString statusMessage;
    bool actionPending;
    bool directSending;

lm's avatar
lm committed
296
    QMap<int, GPSSatellite*> gpsSatellites;
pixhawk's avatar
pixhawk committed
297
    unsigned int satellitesUsed;
298

lm's avatar
lm committed
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
    // Current controller values
    float attXSet;
    float attYSet;
    float attYawSet;
    float altitudeSet;

    float posXSet;
    float posYSet;
    float posZSet;

    // Controller saturation values
    float attXSaturation;
    float attYSaturation;
    float attYawSaturation;

    float posXSaturation;
    float posYSaturation;
    float altitudeSaturation;

    // Position
    float lat;
    float lon;
    float alt;
322
    quint64 globalAvailable;   ///< Last global position update time
lm's avatar
lm committed
323
324
325
    float x;
    float y;
    float z;
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
    float vx;
    float vy;
    float vz;
    float speed;
    quint64 localAvailable;    ///< Last local position update time
    float roll;
    float pitch;
    float yaw;
    float bodyXSetCoordinate;  ///< X Setpoint coordinate active on the MAV
    float bodyYSetCoordinate;  ///< Y Setpoint coordinate active on the MAV
    float bodyZSetCoordinate;  ///< Z Setpoint coordinate active on the MAV
    float bodyYawSet;          ///< Yaw setpoint coordinate active on the MAV
    float uiXSetCoordinate;    ///< X Setpoint coordinate wanted by the UI
    float uiYSetCoordinate;    ///< Y Setpoint coordinate wanted by the UI
    float uiZSetCoordinate;    ///< Z Setpoint coordinate wanted by the UI
    float uiYawSet;            ///< Yaw Setpoint wanted by the UI
342
    double metricWidth;        ///< Width the instrument represents in meters (the width of the ground shown by the widget)
343
344

    //
345
346
    float xCenterPos;         ///< X center of instrument in virtual coordinates
    float yCenterPos;         ///< Y center of instrument in virtual coordinates
lm's avatar
lm committed
347

lm's avatar
lm committed
348
    bool positionLock;
349
    bool rateControlEnabled;  ///< Rate control enabled
350
351
352
353
354
355
356
357
358
    bool attControlEnabled;   ///< Attitude control enabled
    bool xyControlEnabled;    ///< Horizontal control enabled
    bool zControlEnabled;     ///< Vertical control enabled
    bool yawControlEnabled;   ///< Yaw angle position control enabled
    int positionFix;          ///< Total dimensions the MAV is localizaed in
    int gpsFix;               ///< Localization dimensions based on GPS
    int visionFix;            ///< Localizaiton dimensions based on computer vision
    int laserFix;             ///< Localization dimensions based on laser
    int iruFix;               ///< Localization dimensions based on ultrasound
359

pixhawk's avatar
pixhawk committed
360
    bool mavInitialized;      ///< The MAV is initialized once the setpoint has been received
pixhawk's avatar
pixhawk committed
361
    float topMargin;          ///< Margin on top of the page, in virtual coordinates
362
    float bottomMargin;       ///< Margin on the bottom of the page, in virtual coordinates
lm's avatar
lm committed
363

364
    bool rateControlKnown;     ///< Rate control status known flag
365
366
367
368
369
370
371
372
373
374
375
    bool attControlKnown;     ///< Attitude control status known flag
    bool xyControlKnown;      ///< XY control status known flag
    bool zControlKnown;       ///< Z control status known flag
    bool yawControlKnown;     ///< Yaw control status known flag

    // Position lock indicators
    bool positionFixKnown;    ///< Position fix status known flag
    bool visionFixKnown;      ///< Vision fix status known flag
    bool gpsFixKnown;         ///< GPS fix status known flag
    bool iruFixKnown;         ///< Infrared/Ultrasound fix status known flag

376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
    // System state indicators
    bool gyroKnown;
    bool gyroON;
    bool gyroOK;

    bool accelKnown;
    bool accelON;
    bool accelOK;

    bool magKnown;
    bool magON;
    bool magOK;

    bool pressureKnown;
    bool pressureON;
    bool pressureOK;

    bool diffPressureKnown;
    bool diffPressureON;
    bool diffPressureOK;

    bool flowKnown;
    bool flowON;
    bool flowOK;

    bool laserKnown;
    bool laserON;
    bool laserOK;

    bool viconKnown;
    bool viconON;
    bool viconOK;

    bool actuatorsKnown;
    bool actuatorsON;
    bool actuatorsOK;

413
414
    // Data indicators
    bool setPointKnown;       ///< Controller setpoint known status flag
415
    bool positionSetPointKnown; ///< Position setpoint known status flag
416
417
    bool userSetPointSet;     ///< User set X, Y and Z
    bool userXYSetPointSet;   ///< User set the X/Y position already
LM's avatar
LM committed
418
419
    bool userZSetPointSet;   ///< User set the Z position already
    bool userYawSetPointSet;   ///< User set the YAW position already
420

421
422
423
424
private:
};

#endif // HSIDISPLAY_H