HSIDisplay.cc

void HSIDisplay::updatePositionYawControllerEnabled(bool enabled)
{
    yawControlEnabled = enabled;
    yawControlKnown = true;
}

/**
 * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    positionFix = fix;
    positionFixKnown = true;
    //qDebug() << "LOCALIZATION FIX CALLED";
}
/**
 * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    gpsFix = fix;
    gpsFixKnown = true;
}
/**
 * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
 */
void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix)
{
    Q_UNUSED(uas);
    visionFix = fix;
    visionFixKnown = true;
    //qDebug() << "VISION FIX GOT CALLED";
}

/**
 * @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;
    iruFixKnown = true;
}

QColor HSIDisplay::getColorForSNR(float snr)
{
    QColor color;
    if (snr > 0 && snr < 30) {
        color = QColor(250, 10, 10);
    } else if (snr >= 30 && snr < 35) {
        color = QColor(230, 230, 10);
    } else if (snr >= 35 && snr < 40) {
        color = QColor(90, 200, 90);
    } else if (snr >= 40) {
        color = QColor(20, 200, 20);
    } else {
        color = QColor(180, 180, 180);
    }
    return color;
}

void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter)
{
    if (uas)
    {
        float radius = vwidth / 18.0f;
        QPen pen(color);
        pen.setWidthF(refLineWidthToPen(1.6f));
        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);

        QPolygonF poly(4);
        // Top point
        poly.replace(0, QPointF(p.x()-radius, p.y()-radius));
        // Right point
        poly.replace(1, QPointF(p.x()+radius, p.y()-radius));
        // Bottom point
        poly.replace(2, QPointF(p.x()+radius, p.y()+radius));
        poly.replace(3, QPointF(p.x()-radius, p.y()+radius));

        // Label
        paintText(QString("z: %1").arg(z, 3, 'f', 1, QChar(' ')), color, 2.1f, p.x()-radius, p.y()-radius-3.5f, &painter);

        drawPolygon(poly, &painter);

        radius *= 0.8f;
        drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.6f), color, &painter);

        // Draw center dot
        painter.setBrush(color);
        drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter);
    }
}

void HSIDisplay::drawWaypoints(QPainter& painter)
{
    if (uas)
    {
        const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();

        QColor color;
        painter.setBrush(Qt::NoBrush);

        QPointF lastWaypoint;

        for (int i = 0; i < list.size(); i++) {
            QPointF in;
            if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED)
            {
                // Do not transform
                in = QPointF(list.at(i)->getX(), list.at(i)->getY());
            } else {
                // Transform to local coordinates first
                double x = list.at(i)->getX();
                double y = list.at(i)->getY();
                in = QPointF(x, y);
            }
            // Transform from world to body coordinates
            in = metricWorldToBody(in);
            // Scale from metric to screen reference coordinates
            QPointF p = metricBodyToRef(in);

            // Setup pen
            QPen pen(color);
            painter.setBrush(Qt::NoBrush);

            // DRAW WAYPOINT
            float waypointSize = vwidth / 20.0f * 2.0f;
            QPolygonF poly(4);
            // Top point
            poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
            // Right point
            poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
            // Bottom point
            poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
            poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));

            // Select color based on if this is the current waypoint
            if (list.at(i)->getCurrent()) {
                color = QGC::colorYellow;//uas->getColor();
                pen.setWidthF(refLineWidthToPen(0.8f));
            } else {
                color = QGC::colorCyan;
                pen.setWidthF(refLineWidthToPen(0.4f));
            }

            pen.setColor(color);
            painter.setPen(pen);
            float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
            drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
            drawPolygon(poly, &painter);
            float acceptRadius = list.at(i)->getAcceptanceRadius();
            drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);

            // DRAW CONNECTING LINE
            // Draw line from last waypoint to this one
            if (!lastWaypoint.isNull())
            {
                pen.setWidthF(refLineWidthToPen(0.4f));
                painter.setPen(pen);
                color = QGC::colorCyan;
                drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter);
            }
            lastWaypoint = p;
        }
    }
}

void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter)
{
    QPen pen(color);
    pen.setWidthF(refLineWidthToPen(0.1f));
    pen.setColor(color);
    pen.setBrush(Qt::NoBrush);
    painter.setPen(pen);
    painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
}

void HSIDisplay::drawGPS(QPainter &painter)
{
    float xCenter = xCenterPos;
    float yCenter = xCenterPos;
    // Max satellite circle radius

    const float margin = 0.15f;  // 20% margin of total width on each side
    float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
    quint64 currTime = MG::TIME::getGroundTimeNowUsecs();

    // Draw satellite labels
    //    QString label;
    //    label.sprintf("%05.1f", value);
    //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);

    QMapIterator<int, GPSSatellite*> i(gpsSatellites);
    while (i.hasNext()) {
        i.next();
        GPSSatellite* sat = i.value();

        // Check if update is not older than 10 seconds, else delete satellite
        if (sat->lastUpdate + 10000000 < currTime) {
            // Delete and go to next satellite
            gpsSatellites.remove(i.key());
            if (i.hasNext()) {
                i.next();
                sat = i.value();
            } else {
                continue;
            }
        }

        if (sat) {
            // Draw satellite
            QBrush brush;
            QColor color = getColorForSNR(sat->snr);
            brush.setColor(color);
            if (sat->used) {
                brush.setStyle(Qt::SolidPattern);
            } else {
                brush.setStyle(Qt::NoBrush);
            }
            painter.setPen(Qt::SolidLine);
            painter.setPen(color);
            painter.setBrush(brush);

            float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;

            // Draw circle for satellite, filled for used satellites
            drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
            // Draw satellite PRN
            paintText(QString::number(sat->id), QColor(255, 255, 255), 2.9f, xPos+1.7f, yPos+2.0f, &painter);
        }
    }
}

void HSIDisplay::drawObjects(QPainter &painter)
{
    Q_UNUSED(painter);
}

void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
{
    if (xyControlKnown && xyControlEnabled) {
        // Draw the needle
        const float maxWidth = radius / 5.0f;
        const float minWidth = maxWidth * 0.3f;

        float angle = atan2(posXSet, -posYSet);
        angle -= (float)M_PI/2.0f;

        QPolygonF p(6);

        //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation);

        radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation);

        p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(4, QPointF(xRef,               yRef-radius * 0.36f));
        p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));

        rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef));

        QBrush indexBrush;
        indexBrush.setColor(color);
        indexBrush.setStyle(Qt::SolidPattern);
        painter->setPen(Qt::SolidLine);
        painter->setPen(color);
        painter->setBrush(indexBrush);
        drawPolygon(p, painter);

        //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle;
    }
}

void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
{
    if (attControlKnown && attControlEnabled) {
        // Draw the needle
        const float maxWidth = radius / 10.0f;
        const float minWidth = maxWidth * 0.3f;

        float angle = atan2(attYSet, -attXSet);

        radius *= sqrt(attXSet*attXSet + attYSet*attYSet) / sqrt(attXSaturation*attXSaturation + attYSaturation*attYSaturation);

        QPolygonF p(6);

        p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f));
        p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f));
        p.replace(4, QPointF(xRef,               yRef-radius * 0.36f));
        p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f));

        rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef));

        QBrush indexBrush;
        indexBrush.setColor(color);
        indexBrush.setStyle(Qt::SolidPattern);
        painter->setPen(Qt::SolidLine);
        painter->setPen(color);
        painter->setBrush(indexBrush);
        drawPolygon(p, painter);

        // TODO Draw Yaw indicator

        //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle;
    }
}

void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter)
{
    if (zControlKnown && zControlEnabled) {
        // Draw the circle
        QPen circlePen(Qt::SolidLine);
        circlePen.setWidth(refLineWidthToPen(0.5f));
        circlePen.setColor(color);
        painter->setBrush(Qt::NoBrush);
        painter->setPen(circlePen);
        drawCircle(xRef, yRef, radius, 200.0f, color, painter);
        //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter);

        //    // Draw the value
        //    QString label;
        //    label.sprintf("%05.1f", value);
        //    paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter);
    }
}

void HSIDisplay::wheelEvent(QWheelEvent* event)
{
    double zoomScale = 0.005; // Scaling of zoom value
    if(event->delta() > 0) {
        // Reduce width -> Zoom in
        metricWidth -= event->delta() * zoomScale;
    } else {
        // Increase width -> Zoom out
        metricWidth -= event->delta() * zoomScale;
    }
    metricWidth = qBound(0.5, metricWidth, 9999.0);
    emit metricWidthChanged(metricWidth);
}

void HSIDisplay::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
    Q_UNUSED(event) {
        refreshTimer->start(updateInterval);
    }
}

void HSIDisplay::hideEvent(QHideEvent* event)
{
    // React only to internal (post-display)
    // events
    Q_UNUSED(event) {
        refreshTimer->stop();
    }
}

void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat)
{
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    Q_UNUSED(yaw);
    Q_UNUSED(thrust);
    Q_UNUSED(xHat);
    Q_UNUSED(yHat);
}

void HSIDisplay::pressKey(int key)
{
    Q_UNUSED(key);
}