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 35 36 37 38 39

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

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

#ifndef HSIDISPLAY_H
#define HSIDISPLAY_H

#include <QtGui/QWidget>
#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*, double rollDesired, double pitchDesired, double yawDesired, double 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);
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);
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);
201 202
    /** @brief Draw one waypoint */
    void drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& list, int i, 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 */
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);
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

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