HSIDisplay.cc

        connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)),
                this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
        connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)),
                this, SLOT(updateActuatorStatus(bool,bool,bool)));
        connect(uas, &UASInterface::navigationControllerErrorsChanged,
                this, &HSIDisplay::UpdateNavErrors);
        statusClearTimer.start(3000);
    }
    else
    {
        statusClearTimer.stop();
    }

    resetMAVState();
}

void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time)
{
    Q_UNUSED(uas);
    Q_UNUSED(time);
    this->vx = vx;
    this->vy = vy;
    this->vz = vz;
    this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0));
}

void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{
    if (uas)
    {
        userSetPointSet = true;
        userXYSetPointSet = true;
        // Set coordinates and send them out to MAV

        QPointF sp(x, y);
        sp = metricBodyToWorld(sp);
        uiXSetCoordinate = sp.x();
        uiYSetCoordinate = sp.y();

        qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;


        //sendBodySetPointCoordinates();
        statusMessage = "POSITION SET, PRESS <ENTER> TO SEND";
        actionPending = true;
        statusClearTimer.start();
    }
}

void HSIDisplay::setBodySetpointCoordinateZ(double z)
{
    if (uas)
    {
        userSetPointSet = true;
        userZSetPointSet = true;
        // Set coordinates and send them out to MAV
        uiZSetCoordinate = z+uas->getLocalZ();
        statusMessage = "Z SET, PRESS <ENTER> TO SEND";
        actionPending = true;
        statusClearTimer.start();
    }
    //sendBodySetPointCoordinates();
}

void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
    if (uas)
    {
        if (!userXYSetPointSet && setPointKnown)
        {
            uiXSetCoordinate = bodyXSetCoordinate;
            uiYSetCoordinate = bodyYSetCoordinate;
        }
        else if (!userXYSetPointSet && mavInitialized)
        {
            QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
            uiXSetCoordinate = coord.x();
            uiYSetCoordinate = coord.y();
        }
        userSetPointSet = true;
        userYawSetPointSet = true;
        // Set coordinates and send them out to MAV
        uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw()));
        statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
        statusClearTimer.start();
        actionPending = true;
    }
    //sendBodySetPointCoordinates();
}

void HSIDisplay::sendBodySetPointCoordinates()
{
    // Send the coordinates to the MAV
    if (uas)
    {
        uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
    }
}

void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    attXSet = pitchDesired;
    attYSet = rollDesired;
    attYawSet = yawDesired;
    altitudeSet = thrustDesired;
}

void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time)
{
    Q_UNUSED(uas);
    Q_UNUSED(time);
    this->roll = roll;
    this->pitch = pitch;
    this->yaw = yaw;
}

void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
{
    Q_UNUSED(uasid);
    uiXSetCoordinate = xDesired;
    uiYSetCoordinate = yDesired;
    uiZSetCoordinate = zDesired;
    uiYawSet = yawDesired;
    userXYSetPointSet = true;
    userZSetPointSet = true;
    userYawSetPointSet = true;
    userSetPointSet = true;
}

void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec)
{
    Q_UNUSED(uasid);
    Q_UNUSED(usec);
    bodyXSetCoordinate = xDesired;
    bodyYSetCoordinate = yDesired;
    bodyZSetCoordinate = zDesired;
    bodyYawSet = yawDesired;
    mavInitialized = true;
    setPointKnown = true;
    positionSetPointKnown = true;

    if (!userSetPointSet && !dragStarted)
    {
        uiXSetCoordinate = bodyXSetCoordinate;
        uiYSetCoordinate = bodyYSetCoordinate;
        //        uiZSetCoordinate = bodyZSetCoordinate;
        uiYawSet= bodyYawSet;
    }
}

void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
{
    this->x = x;
    this->y = y;
    this->z = z;
    localAvailable = usec;
}

void HSIDisplay::UpdateNavErrors(UASInterface *uas, double altitudeError, double airspeedError, double crosstrackError)
{
    Q_UNUSED(altitudeError);
    Q_UNUSED(airspeedError);
    if (this->uas == uas) {
        this->crosstrackError = crosstrackError;
    }
}

void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec)
{
    Q_UNUSED(altAMSL);
    this->lat = lat;
    this->lon = lon;
    this->alt = altWGS84;
    globalAvailable = usec;
}

void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
{
    Q_UNUSED(uasid);
    // If slot is empty, insert object
    if (satid != 0) { // Satellite PRNs currently range from 1-32, but are never zero
        if (gpsSatellites.contains(satid)) {
            gpsSatellites.value(satid)->update(satid, elevation, azimuth, snr, used);
        } else {
            gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
        }
    }
}

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::drawWaypoint(QPainter& painter, const QColor& color, float width, const Waypoint *w, const QPointF& p)
{
    painter.setBrush(Qt::NoBrush);

    // Setup pen for foreground
    QPen pen(color);
    pen.setWidthF(width);

    // 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()));

    float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
    float acceptRadius = w->getAcceptanceRadius();
    double yawDiff = w->getYaw()/180.0*M_PI-yaw;

    // Draw background
    pen.setColor(Qt::black);
    painter.setPen(pen);
    drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f*3.0f), Qt::black, &painter);
    drawPolygon(poly, &painter);
    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.0, Qt::black, &painter);

    // Draw foreground
    pen.setColor(color);
    painter.setPen(pen);
    drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f), color, &painter);
    drawPolygon(poly, &painter);
    drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
}

void HSIDisplay::drawWaypoints(QPainter& painter)
{
    if (uas)
    {
        // Grab all waypoints.
        const QList<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();
        const int numWaypoints = list.size();

        // Do not work on empty lists
        if (list.size() == 0)
        {
            return;
        }

        // Make sure any drawn shapes are not filled-in.
        painter.setBrush(Qt::NoBrush);

        QPointF lastWaypoint;
        for (int i = 0; i < numWaypoints; i++)
        {
            const Waypoint *w = list.at(i);
            QPointF in;
            // Use local coordinates as-is.
            int frameRef = w->getFrame();
            if (frameRef == MAV_FRAME_LOCAL_NED)
            {
                in = QPointF(w->getX(), w->getY());
            }
            else if (frameRef == MAV_FRAME_LOCAL_ENU)
            {
                in = QPointF(w->getY(), w->getX());
            }
            // Convert global coordinates into the local ENU frame, then display them.
            else if (frameRef == MAV_FRAME_GLOBAL || frameRef == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
                // Get the position of the GPS origin for the MAV.

                // Transform the lat/lon for this waypoint into the local frame
                double e, n, u;
                HomePositionManager::instance()->wgs84ToEnu(w->getX(), w->getY(),w->getZ(), &e, &n, &u);
                in = QPointF(n, e);
            }
            // Otherwise we don't process this waypoint.
            // FIXME: This code will probably fail if the last waypoint found is not a valid one.
            else {
                continue;
            }

            // Transform from world to body coordinates
            in = metricWorldToBody(in);
            // Scale from metric to screen reference coordinates
            QPointF p = metricBodyToRef(in);

            // Select color based on if this is the current waypoint.
            if (w->getCurrent())
            {
                drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p);
            }
            else
            {
                drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), w, p);
            }

            // Draw connecting line from last waypoint to this one.
            if (!lastWaypoint.isNull())
            {
                drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f*2.0f), Qt::black, &painter);
                drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), QGC::colorCyan, &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 = yCenterPos;

    const float yawDeg = ((yaw/M_PI)*180.0f);
    int yawRotate = static_cast<int>(yawDeg) % 360;
    // XXX check rotation direction

    // 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-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f-yawRotate)/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);
}