Added support for IR/Ultrasound and waypoints in HSI

src/uas/UASInterface.h

@@ -340,6 +340,11 @@ signals:
      * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
      */
     void visionLocalizationChanged(UASInterface* uas, int fix);
+    /**
+     * @brief IR/U localization quality changed
+     * @param fix 0: No IR/Ultrasound sensor, N > 0: Found N active sensors
+     */
+    void irUltraSoundLocalizationChanged(UASInterface* uas, int fix);
 };
 
 Q_DECLARE_INTERFACE(UASInterface, "org.qgroundcontrol/1.0");

src/ui/HSIDisplay.cc

@@ -145,6 +145,7 @@ void HSIDisplay::paintDisplay()
     drawPositionLock(2,  5, tr("POS"), positionFix, painter);
     drawPositionLock(22, 5, tr("VIS"), visionFix, painter);
     drawPositionLock(44, 5, tr("GPS"), gpsFix, painter);
+    drawPositionLock(66, 5, tr("IRU"), iruFix, painter);
 
 
     // Draw base instrument
@@ -422,6 +423,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
     connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
     connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
+    connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
 
     // Now connect the new UAS
 
@@ -569,6 +571,15 @@ void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix)
     visionFix = fix;
 }
 
+/**
+ * @param fix 0: lost, 1-N: Localized with N ultrasound or infrared sensors
+ */
+void HSIDisplay::updateInfraredUltrasoundLocalization(UASInterface* uas, int fix)
+{
+    Q_UNUSED(uas);
+    iruFix = fix;
+}
+
 QColor HSIDisplay::getColorForSNR(float snr)
 {
     QColor color;
@@ -615,6 +626,29 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
     drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
 }
 
+void HSIDisplay::drawWaypoints(QPainter& painter)
+{
+    QColor color = uas->getColor();
+    float x = 1.1;
+    float y = 1.1;
+    float radius = vwidth / 20.0f;
+    QPen pen(color);
+    pen.setWidthF(refLineWidthToPen(0.4f));
+    pen.setColor(color);
+    painter.setPen(pen);
+    painter.setBrush(Qt::NoBrush);
+    QPointF in(x, y);
+    // Transform from body to world coordinates
+    in = metricWorldToBody(in);
+    // Scale from metric to screen reference coordinates
+    QPointF p = metricBodyToRef(in);
+    drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
+    radius *= 0.8;
+    drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
+    painter.setBrush(color);
+    drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
+}
+
 void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter)
 {
     QPen pen(color);

src/ui/HSIDisplay.h

@@ -65,22 +65,16 @@ public slots:
     /** @brief Heading control enabled/disabled */
     void updatePositionYawControllerEnabled(bool enabled);
 
-    /**
-     * @brief Localization quality changed
-     * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
-     */
+    /** @brief Localization quality changed */
     void updateLocalization(UASInterface* uas, int localization);
-    /**
-     * @brief GPS localization quality changed
-     * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization
-     */
+    /** @brief GPS localization quality changed */
     void updateGpsLocalization(UASInterface* uas, int localization);
-    /**
-     * @brief Vision localization quality changed
-     * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
-     */
+    /** @brief Vision localization quality changed */
     void updateVisionLocalization(UASInterface* uas, int localization);
 
+    /** @brief Ultrasound/Infrared localization changed */
+    void updateInfraredUltrasoundLocalization(UASInterface* uas, int fix);
+
     void paintEvent(QPaintEvent * event);
     /** @brief Update state from joystick */
     void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat);
@@ -101,6 +95,8 @@ protected slots:
     void sendBodySetPointCoordinates();
     /** @brief Draw one setpoint */
     void drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter);
+    /** @brief Draw waypoints of this system */
+    void drawWaypoints(QPainter& painter);
     /** @brief Draw the limiting safety area */
     void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight,  const QColor &color, QPainter &painter);
 
@@ -208,18 +204,19 @@ protected:
     float metricWidth;         ///< Width the instrument represents in meters (the width of the ground shown by the widget)
 
     //
-    float xCenterPos;
-    float yCenterPos;
+    float xCenterPos;         ///< X center of instrument in virtual coordinates
+    float yCenterPos;         ///< Y center of instrument in virtual coordinates
 
     bool positionLock;
-    bool attControlEnabled;
-    bool xyControlEnabled;
-    bool zControlEnabled;
-    bool yawControlEnabled;
-    int positionFix;
-    int gpsFix;
-    int visionFix;
-    int laserFix;
+    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
     bool mavInitialized;      ///< The MAV is initialized once the setpoint has been received
     float bottomMargin;       ///< Margin on the bottom of the page, in virtual coordinates
     float topMargin;          ///< Margin on top of the page, in virtual coordinates