diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9b8ede22705246d99fff019393bc83493bd6d5be..f0318d102f2dfaf363bd50e27fead8993bcbe9e9 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -711,14 +711,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); } - // dongfang: For APM, this altitude is the mix altitude[m] - emit altitudeChanged(this, PRIMARY_ALTITUDE, hud.alt, time); + // The primary altitude is the one that the UAV uses for navigation. + // We assume! that the HUD message reports that as altitude. + emit primaryAltitudeChanged(this, hud.alt, time); - // This makes the lateral velocity zero when it might really not be, problematic. - // emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, 0.0f, hud.climb, time); - emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, time); - emit speedChanged(this, GROUNDSPEED_BY_UAV, hud.groundspeed, time); - emit climbRateChanged(this, BAROMETRIC_ALTITUDE, hud.groundspeed, time); + emit primarySpeedChanged(this, hud.airspeed, time); + emit gpsSpeedChanged(this, hud.groundspeed, time); + emit climbRateChanged(this, hud.climb, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION_NED: @@ -739,9 +738,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) localZ = pos.z; // Emit - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - emit velocityChanged(this, MAV_FRAME_LOCAL_NED, pos.vx, pos.vy, pos.vz, time); + emit velocityChanged_NED(this, pos.vx, pos.vy, pos.vz, time); // Set internal state if (!positionLock) { @@ -786,12 +784,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); // dongfang: The altitude is GPS altitude. Bugger. It needs to be changed to primary. - emit altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time); + emit gpsAltitudeChanged(this, getAltitude(), time); // We had some frame mess here, global and local axes were mixed. - emit velocityChanged(this, MAV_FRAME_GLOBAL, speedX, speedY, speedZ, time); + emit velocityChanged_NED(this, speedX, speedY, speedZ, time); double groundspeed = qSqrt(speedX*speedX+speedY*speedY); - emit speedChanged(this, GROUNDSPEED_BY_GPS, groundspeed, time); + emit gpsSpeedChanged(this, groundspeed, time); // Set internal state if (!positionLock) @@ -838,7 +836,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) setLongitude(longitude_gps); setAltitude(altitude_gps); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); - emit altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time); + emit gpsAltitudeChanged(this, getAltitude(), time); } positionLock = true; @@ -853,7 +851,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) { // TODO: Other sources also? Actually this condition does not quite belong here. - emit speedChanged(this, GROUNDSPEED_BY_GPS, vel, time); + emit gpsSpeedChanged(this, vel, time); } else { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index bab3b65993040863fc292e056ccd46149cf814bf..ffae8f2fc609621e6e36243ae5c4d83a8c74f671 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -852,10 +852,9 @@ signals: void distToWaypointChanged(double val,QString name); void bearingToWaypointChanged(double val,QString name); - void altitudeChanged(UASInterface*, AltitudeMeasurementSource source, double altitude, quint64 usec); - //void speedChanged(UASInterface*, SpeedMeasurementSource source, double speed, quint64 usec); - //void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed, quint64 usec); - void velocityChanged(UASInterface*, MAV_FRAME, double vx, double vy, double vz, quint64 usec); + //void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec); + //void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec); + //void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec); protected: /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ quint64 getUnixTime(quint64 time=0); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index f80c308e9dc0882eea64d187344762c7565a59a5..83f07f7497dd97faf8e9256048eece0b6ab73fcb 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -61,6 +61,7 @@ enum BatteryType AGZN = 5 }; ///< The type of battery used +/* enum SpeedMeasurementSource { PRIMARY_SPEED = 0, // ArduPlane: Measured airspeed or estimated airspeed. ArduCopter: Ground (XY) speed. @@ -84,6 +85,7 @@ enum AltitudeMeasurementFrame ABSOLUTE = 0, // Altitude is pressure altitude ABOVE_HOME_POSITION = 1 }; ///< For altitude data, a reference frame +*/ /** * @brief Interface for all robots. @@ -520,16 +522,18 @@ signals: void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); - void altitudeChanged(UASInterface*, AltitudeMeasurementSource source, double altitude, quint64 usec); + void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec); + void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec); /** @brief Update the status of one satellite used for localization */ void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used); // The horizontal speed (a scalar) - void speedChanged(UASInterface*, SpeedMeasurementSource source, double speed, quint64 usec); + void primarySpeedChanged(UASInterface*, double speed, quint64 usec); + void gpsSpeedChanged(UASInterface*, double speed, quint64 usec); // The vertical speed (a scalar) - void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed, quint64 usec); + void climbRateChanged(UASInterface*, double climb, quint64 usec); // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are. - void velocityChanged(UASInterface*, MAV_FRAME frame, double vx, double vy, double vz, quint64 usec); + void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec); void navigationControllerErrorsChanged(UASInterface*, double altitudeError, double speedError, double xtrackError); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index ea9cb27f4c7b53a53c1c6aeb5ee8c28dcd064fc4..d18ebc038f79757c4f4a449e93b4cce69fe9e51e 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -867,7 +867,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); @@ -898,7 +898,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index a84ac970f0f6e845a875c0d9299674421b4cc369..e4556ff62afa3929e9df3730f47d75c09c6f08d0 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -270,7 +270,7 @@ void HUD::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(velocityChanged_NEDspeedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); // Try to disconnect the image link @@ -293,7 +293,7 @@ void HUD::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); // Try to connect the image link diff --git a/src/ui/PrimaryFlightDisplay.cpp b/src/ui/PrimaryFlightDisplay.cpp index eb3d6d83cccf83d20ed8521a959ca52ccc26ede4..de82c6e674c7f954b05f7231665407f9fcf11d8a 100644 --- a/src/ui/PrimaryFlightDisplay.cpp +++ b/src/ui/PrimaryFlightDisplay.cpp @@ -58,6 +58,9 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren font("Bitstream Vera Sans"), refreshTimer(new QTimer(this)), + navigationCrosstrackError(INFINITY), + navigationTargetBearing(UNKNOWN_ATTITUDE), + layout(COMPASS_INTEGRATED), style(OVERLAY_HSI), @@ -172,6 +175,13 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) // Disconnect any previously connected active MAV disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64))); + disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); + disconnect(this->uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64))); + disconnect(this->uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSSpeed(UASInterface*,double,quint64))); + disconnect(this->uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); + disconnect(this->uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64))); + disconnect(this->uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSAltitude(UASInterface*, double, quint64))); + disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); //disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); //disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); @@ -179,14 +189,7 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) //disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); //disconnect(this->uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); //disconnect(this->uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString))); - disconnect(this->uas, SIGNAL(localizationChanged(UASInterface* uas, int fix)), this, SLOT(updateGPSFixType(UASInterface*,int))); - - //disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - //disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); - disconnect(this->uas, SIGNAL(speedChanged(UASInterface*, SpeedMeasurementSource, double, quint64)), this, SLOT(updateSpeed(UASInterface*,SpeedMeasurementSource,double,quint64))); - disconnect(this->uas, SIGNAL(climbRateChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); - disconnect(this->uas, SIGNAL(altitudeChanged(UASInterface*, AltitudeMeasurementSource, double)), this, SLOT(updateAltitude(UASInterface*, AltitudeMeasurementSource, double, quint64))); + //disconnect(this->uas, SIGNAL(localizationChanged(UASInterface* uas, int fix)), this, SLOT(updateGPSFixType(UASInterface*,int))); } if (uas) { @@ -205,9 +208,12 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) //connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); //connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); - connect(uas, SIGNAL(speedChanged(UASInterface*, SpeedMeasurementSource, double, quint64)), this, SLOT(updateSpeed(UASInterface*,SpeedMeasurementSource,double,quint64))); - connect(uas, SIGNAL(climbRateChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); - connect(uas, SIGNAL(altitudeChanged(UASInterface*, AltitudeMeasurementSource, double, quint64)), this, SLOT(updateAltitude(UASInterface*, AltitudeMeasurementSource, double, quint64))); + connect(uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64))); + connect(uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSSpeed(UASInterface*,double,quint64))); + connect(uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)), this, SLOT(updateClimbRate(UASInterface*, AltitudeMeasurementSource, double, quint64))); + connect(uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64))); + connect(uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSAltitude(UASInterface*, double, quint64))); + connect(uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); // Set new UAS this->uas = uas; @@ -257,40 +263,54 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, doub * TODO! Examine what data comes with this call, should we consider it airspeed, ground speed or * should we not consider it at all? */ -void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, SpeedMeasurementSource source, double speed, quint64 timestamp) +void PrimaryFlightDisplay::updatePrimarySpeed(UASInterface* uas, double speed, quint64 timestamp) +{ + Q_UNUSED(uas); + Q_UNUSED(timestamp); + + primarySpeed = speed; + didReceivePrimarySpeed = true; +} + +void PrimaryFlightDisplay::updateGPSSpeed(UASInterface* uas, double speed, quint64 timestamp) { Q_UNUSED(uas); Q_UNUSED(timestamp); - if (source == PRIMARY_SPEED) { + groundspeed = speed; + if (!didReceivePrimarySpeed) primarySpeed = speed; - didReceivePrimarySpeed = true; - } else if (source == GROUNDSPEED_BY_GPS || source == GROUNDSPEED_BY_UAV) { - groundspeed = speed; - if (!didReceivePrimarySpeed) - primarySpeed = speed; - } } -void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double climbRate, quint64 timestamp) { +void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, double climbRate, quint64 timestamp) { Q_UNUSED(uas); Q_UNUSED(timestamp); verticalVelocity = climbRate; } -void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp) { +void PrimaryFlightDisplay::updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp) { + Q_UNUSED(uas); + Q_UNUSED(timestamp); + primaryAltitude = altitude; + didReceivePrimaryAltitude = true; +} + +void PrimaryFlightDisplay::updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp) { Q_UNUSED(uas); Q_UNUSED(timestamp); - if (source == PRIMARY_ALTITUDE) { + GPSAltitude = altitude; + if (!didReceivePrimaryAltitude) primaryAltitude = altitude; - didReceivePrimaryAltitude = true; - } else if (source == GPS_ALTITUDE) { - GPSAltitude = altitude; - if (!didReceivePrimaryAltitude) - primaryAltitude = altitude; - } } +void PrimaryFlightDisplay::updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError) { + Q_UNUSED(uas); + this->navigationAltitudeError = altitudeError; + this->navigationSpeedError = speedError; + this->navigationCrosstrackError = xtrackError; +} + + /* * Private and such */ @@ -310,6 +330,13 @@ bool PrimaryFlightDisplay::isAirplane() { } } +// TODO: Implement. Should return true when navigating. +// That would be (APM) in AUTO and RTL modes. +// This could forward to a virtual on UAS bool isNavigatingAutonomusly() or whatever. +bool PrimaryFlightDisplay::shouldDisplayNavigationData() { + return true; +} + void PrimaryFlightDisplay::drawTextCenter ( QPainter& painter, QString text, @@ -646,7 +673,6 @@ void PrimaryFlightDisplay::drawRollScale( } } - void PrimaryFlightDisplay::drawAIAttitudeScales( QPainter& painter, QRectF area, @@ -769,10 +795,41 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo painter.setPen(pen); drawTextCenter(painter, s_digitalCompass, largeTextSize, 0, -radius*0.38-digitalCompassUpshift); -} -// TODO: Merge common code above and below this line..... +// dummy +// navigationTargetBearing = 10; +// navigationCrosstrackError = 500; + + // The CDI + if (shouldDisplayNavigationData() && navigationTargetBearing != UNKNOWN_ATTITUDE && !isinf(navigationCrosstrackError)) { + painter.resetTransform(); + painter.translate(area.center()); + // TODO : Sign might be wrong? + // TODO : The case where error exceeds max. Truncate to max. and make that visible somehow. + bool errorBeyondRadius = false; + if (abs(navigationCrosstrackError) > CROSSTRACK_MAX) { + errorBeyondRadius = true; + navigationCrosstrackError = navigationCrosstrackError>0 ? CROSSTRACK_MAX : -CROSSTRACK_MAX; + } + + float r = radius * CROSSTRACK_RADIUS; + float x = navigationCrosstrackError / CROSSTRACK_MAX * r; + float y = qSqrt(r*r - x*x); // the positive y, there is also a negative. + float sillyHeading = 0; + float angle = sillyHeading - navigationTargetBearing; // TODO: sign. + painter.rotate(-angle); + + QPen pen; + pen.setWidthF(lineWidth); + pen.setColor(Qt::black); + painter.setPen(pen); + + painter.drawLine(QPointF(x, y), QPointF(x, -y)); + } +} + +/* void PrimaryFlightDisplay::drawSeparateCompassDisk(QPainter& painter, QRectF area) { float radius = area.width()/2; float innerRadius = radius * 0.96; @@ -852,7 +909,6 @@ void PrimaryFlightDisplay::drawSeparateCompassDisk(QPainter& painter, QRectF are // if (heading < 0) heading += 360; // else if (heading >= 360) heading -= 360; - /* final safeguard for really stupid systems */ int yawCompass = static_cast(heading) % 360; QString yawAngle; @@ -865,6 +921,7 @@ void PrimaryFlightDisplay::drawSeparateCompassDisk(QPainter& painter, QRectF are drawTextCenter(painter, yawAngle, largeTextSize, 0, radius/4); } +*/ void PrimaryFlightDisplay::drawAltimeter( QPainter& painter, @@ -1402,9 +1459,9 @@ void PrimaryFlightDisplay::doPaint() { drawAIAttitudeScales(painter, AIMainArea, compassAIIntrusion); drawAIAirframeFixedFeatures(painter, AIMainArea); - if(layout ==COMPASS_SEPARATED) - drawSeparateCompassDisk(painter, compassArea); - else + // if(layout ==COMPASS_SEPARATED) + //drawSeparateCompassDisk(painter, compassArea); + // else drawAICompassDisk(painter, compassArea, compassHalfSpan); painter.setClipping(hadClip); diff --git a/src/ui/PrimaryFlightDisplay.h b/src/ui/PrimaryFlightDisplay.h index 7fcde0d28f1adb5f33a72ba9285c01e8907d8afd..4fad0f4f17cdcff29746840ddcf39b1e02fa7357 100644 --- a/src/ui/PrimaryFlightDisplay.h +++ b/src/ui/PrimaryFlightDisplay.h @@ -58,6 +58,9 @@ #define COMPASS_DISK_MARKERWIDTH 0.2 #define COMPASS_DISK_MARKERHEIGHT 0.133 +#define CROSSTRACK_MAX 1000 +#define CROSSTRACK_RADIUS 0.6 + #define TAPE_GAUGES_TICKWIDTH_MAJOR 0.25 #define TAPE_GAUGES_TICKWIDTH_MINOR 0.15 @@ -107,9 +110,13 @@ public slots: /** @brief Attitude from one specific component / redundant autopilot */ void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); - void updateSpeed(UASInterface* uas, SpeedMeasurementSource source, double speed, quint64 timstamp); - void updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp); - void updateAltitude(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp); + void updatePrimarySpeed(UASInterface* uas, double speed, quint64 timstamp); + void updateGPSSpeed(UASInterface* uas, double speed, quint64 timstamp); + void updateClimbRate(UASInterface* uas, double altitude, quint64 timestamp); + void updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp); + void updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp); + void updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError); + /** @brief Set the currently monitored UAS */ virtual void setActiveUAS(UASInterface* uas); @@ -168,6 +175,7 @@ private: * - Airplane show absolute altutude in altimeter, copter shows relative to home */ bool isAirplane(); + bool shouldDisplayNavigationData(); void drawTextCenter(QPainter& painter, QString text, float fontSize, float x, float y); void drawTextLeftCenter(QPainter& painter, QString text, float fontSize, float x, float y); @@ -222,6 +230,11 @@ private: float groundspeed; float verticalVelocity; + float navigationAltitudeError; + float navigationSpeedError; + float navigationCrosstrackError; + float navigationTargetBearing; + Layout layout; // The display layout. Style style; // The AI style (tapes translusent or opague) diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index e709a19513ef7362097ad2268e860b08707d9163..c414df0d9b67dd64d045ba0411cbb44acba82ea4 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -38,7 +38,7 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) //connect standar messages connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double))); diff --git a/src/ui/uas/UASControlParameters.cpp b/src/ui/uas/UASControlParameters.cpp index 214c85ce25f042b4d8737cb869aa36b0b625733e..3947c2d325a12f197269982c44c719bbc2fdea67 100644 --- a/src/ui/uas/UASControlParameters.cpp +++ b/src/ui/uas/UASControlParameters.cpp @@ -93,7 +93,7 @@ void UASControlParameters::activeUasSet(UASInterface *uas) { if(uas) { connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) ); diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 6e2b475a0746ed7aad6394bd1b7c39f6ea10a262..7a6b7e6b045810df4366c6f976fe1351bc103c10 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -91,7 +91,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString))); connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));