diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 89cf722a54ad0cfe2574811d86d1e34c9d8d31f8..6b19986431223291867d61047883be0a9a45f725 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -911,14 +911,14 @@ void QGCXPlaneLink::setRandomPosition() double offLon = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; double offAlt = rand() / static_cast(RAND_MAX) * 200.0 + 100.0; - if (mav->getAltitude() + offAlt < 0) + if (mav->getAltitudeAMSL() + offAlt < 0) { offAlt *= -1.0; } setPositionAttitude(mav->getLatitude() + offLat, mav->getLongitude() + offLon, - mav->getAltitude() + offAlt, + mav->getAltitudeAMSL() + offAlt, mav->getRoll(), mav->getPitch(), mav->getYaw()); @@ -935,7 +935,7 @@ void QGCXPlaneLink::setRandomAttitude() setPositionAttitude(mav->getLatitude(), mav->getLongitude(), - mav->getAltitude(), + mav->getAltitudeAMSL(), roll, pitch, yaw); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cc3daa6ad69140f8935f69ed69d613423e0d9d71..69ee6646cff34c67bbb96b4da4b9b1d2657a5179 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -112,7 +112,15 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), latitude(0.0), longitude(0.0), - altitude(0.0), + altitudeAMSL(0.0), + altitudeRelative(0.0), + + airSpeed(NAN), + groundSpeed(NAN), + + speedX(0.0), + speedY(0.0), + speedZ(0.0), globalEstimatorActive(false), latitude_gps(0.0), @@ -828,18 +836,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!attitudeKnown) { - //yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI); - setYaw(QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI)); + setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI)); emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), 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); - - emit primarySpeedChanged(this, hud.airspeed, time); - emit gpsSpeedChanged(this, hud.groundspeed, time); - emit climbRateChanged(this, hud.climb, time); + setAltitudeAMSL(hud.alt); + setGroundSpeed(hud.groundspeed); + if (!isnan(hud.airspeed)) + setAirSpeed(hud.airspeed); + speedZ = -hud.climb; + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); + emit speedChanged(this, groundSpeed, airSpeed, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION_NED: @@ -855,13 +862,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!wrongComponent) { - localX = pos.x; - localY = pos.y; - localZ = pos.z; + setLocalX(pos.x); + setLocalY(pos.y); + setLocalZ(pos.z); + + speedX = pos.vx; + speedY = pos.vy; + speedZ = pos.vz; // Emit - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - emit velocityChanged_NED(this, pos.vx, pos.vy, pos.vz, time); + emit localPositionChanged(this, localX, localY, localZ, time); + emit velocityChanged_NED(this, speedX, speedY, speedZ, time); // Set internal state if (!positionLock) { @@ -888,15 +899,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_global_position_int_t pos; mavlink_msg_global_position_int_decode(&message, &pos); + quint64 time = getUnixTime(); + setLatitude(pos.lat/(double)1E7); setLongitude(pos.lon/(double)1E7); - - // dongfang: Beware. There are 2 altitudes in this message; neither is the primary. - // pos.alt is GPS altitude and pos.relative_alt is above-home altitude. - // It would be nice if APM could be modified to present the primary (mix) alt. instead - // of the GPS alt. in this message. - setAltitude(pos.alt/1000.0); + setAltitudeAMSL(pos.alt/1000.0); + setAltitudeRelative(pos.relative_alt/1000.0); globalEstimatorActive = true; @@ -904,14 +913,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) speedY = pos.vy/100.0; speedZ = pos.vz/100.0; - emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); - // dongfang: The altitude is GPS altitude. Bugger. It needs to be changed to primary. - emit gpsAltitudeChanged(this, getAltitude(), time); + emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); // We had some frame mess here, global and local axes were mixed. emit velocityChanged_NED(this, speedX, speedY, speedZ, time); - double groundspeed = qSqrt(speedX*speedX+speedY*speedY); - emit gpsSpeedChanged(this, groundspeed, time); + setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY)); + emit speedChanged(this, groundSpeed, airSpeed, time); // Set internal state if (!positionLock) @@ -930,9 +938,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_gps_raw_int_t pos; mavlink_msg_gps_raw_int_decode(&message, &pos); - // SANITY CHECK - // only accept values in a realistic range - // quint64 time = getUnixTime(pos.time_usec); quint64 time = getUnixTime(pos.time_usec); emit gpsLocalizationChanged(this, pos.fix_type); @@ -947,6 +952,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (pos.fix_type > 2) { + positionLock = true; + isGlobalPositionKnown = true; latitude_gps = pos.lat/(double)1E7; longitude_gps = pos.lon/(double)1E7; @@ -956,29 +963,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!globalEstimatorActive) { setLatitude(latitude_gps); setLongitude(longitude_gps); - setAltitude(altitude_gps); - emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time); - emit gpsAltitudeChanged(this, getAltitude(), time); - } + setAltitudeAMSL(altitude_gps); + emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); - positionLock = true; - isGlobalPositionKnown = true; - - // Smaller than threshold and not NaN - - float vel = pos.vel/100.0f; - - // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. - if (!globalEstimatorActive) { - if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) - { - //emit speedChanged(this, vel, 0.0, 0.0, time); + float vel = pos.vel/100.0f; + // Smaller than threshold and not NaN + if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) { setGroundSpeed(vel); - // TODO: Other sources also? Actually this condition does not quite belong here. - emit gpsSpeedChanged(this, vel, time); - } - else - { + emit speedChanged(this, groundSpeed, airSpeed, time); + } else { emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 25b8ffc944b29e662b76d2d4629de9c59cf7a935..69dc667498c37552784641a0050c3ca723df1a84 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -103,7 +103,11 @@ public: Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged) + Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged) Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged) + Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged) + Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged) + Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) void setGroundSpeed(double val) { @@ -115,17 +119,24 @@ public: { return groundSpeed; } - Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged) - // dongfang: There is not only one altitude; there are at least (APM) GPS altitude, mix altitude and mix-altitude relative to home. - // I have made this property refer to the mix-altitude ASL as this is the one actually used in navigation by APM. - Q_PROPERTY(double altitude READ getAltitude WRITE setAltitude NOTIFY altitudeChanged) + void setAirSpeed(double val) + { + airSpeed = val; + emit airSpeedChanged(val,"airSpeed"); + emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime()); + } + + double getAirSpeed() const + { + return airSpeed; + } void setLocalX(double val) { localX = val; emit localXChanged(val,"localX"); - emit valueChanged(this->uasId,"localX","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime()); } double getLocalX() const @@ -137,7 +148,7 @@ public: { localY = val; emit localYChanged(val,"localY"); - emit valueChanged(this->uasId,"localY","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime()); } double getLocalY() const { @@ -148,7 +159,7 @@ public: { localZ = val; emit localZChanged(val,"localZ"); - emit valueChanged(this->uasId,"localZ","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime()); } double getLocalZ() const { @@ -179,23 +190,35 @@ public: return longitude; } - void setAltitude(double val) + void setAltitudeAMSL(double val) { - altitude = val; - emit altitudeChanged(val, "altitude"); - emit valueChanged(this->uasId,"altitude","M",QVariant(val),getUnixTime()); + altitudeAMSL = val; + emit altitudeAMSLChanged(val, "altitudeAMSL"); + emit valueChanged(this->uasId,"altitudeAMSL","m",QVariant(val),getUnixTime()); } - double getAltitude() const + double getAltitudeAMSL() const { - return altitude; + return altitudeAMSL; + } + + void setAltitudeRelative(double val) + { + altitudeRelative = val; + emit altitudeRelativeChanged(val, "altitudeRelative"); + emit valueChanged(this->uasId,"altitudeRelative","m",QVariant(val),getUnixTime()); + } + + double getAltitudeRelative() const + { + return altitudeRelative; } void setSatelliteCount(double val) { satelliteCount = val; emit satelliteCountChanged(val,"satelliteCount"); - emit valueChanged(this->uasId,"satelliteCount","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime()); } double getSatelliteCount() const @@ -217,7 +240,7 @@ public: { distToWaypoint = val; emit distToWaypointChanged(val,"distToWaypoint"); - emit valueChanged(this->uasId,"distToWaypoint","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime()); } double getDistToWaypoint() const @@ -229,7 +252,7 @@ public: { bearingToWaypoint = val; emit bearingToWaypointChanged(val,"bearingToWaypoint"); - emit valueChanged(this->uasId,"bearingToWaypoint","M",QVariant(val),getUnixTime()); + emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime()); } double getBearingToWaypoint() const @@ -414,8 +437,8 @@ protected: //COMMENTS FOR TEST UNIT /// POSITION bool positionLock; ///< Status if position information is available or not - bool isLocalPositionKnown; ///< If the local position has been received for this MAV - bool isGlobalPositionKnown; ///< If the global position has been received for this MAV + bool isLocalPositionKnown; ///< If the local position has been received for this MAV + bool isGlobalPositionKnown; ///< If the global position has been received for this MAV double localX; double localY; @@ -423,7 +446,8 @@ protected: //COMMENTS FOR TEST UNIT double latitude; ///< Global latitude as estimated by position estimator double longitude; ///< Global longitude as estimated by position estimator - double altitude; ///< Global altitude as estimated by position estimator + double altitudeAMSL; ///< Global altitude as estimated by position estimator + double altitudeRelative; ///< Altitude above home as estimated by position estimator double satelliteCount; ///< Number of satellites visible to raw GPS bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position @@ -439,7 +463,8 @@ protected: //COMMENTS FOR TEST UNIT /// WAYPOINT NAVIGATION double distToWaypoint; ///< Distance to next waypoint - double groundSpeed; ///< GPS Groundspeed + double airSpeed; ///< Airspeed + double groundSpeed; ///< Groundspeed double bearingToWaypoint; ///< Bearing to next waypoint UASWaypointManager waypointManager; @@ -921,18 +946,16 @@ signals: void localZChanged(double val,QString name); void longitudeChanged(double val,QString name); void latitudeChanged(double val,QString name); - void altitudeChanged(double val,QString name); + void altitudeAMSLChanged(double val,QString name); + void altitudeRelativeChanged(double val,QString name); void rollChanged(double val,QString name); void pitchChanged(double val,QString name); void yawChanged(double val,QString name); void satelliteCountChanged(double val,QString name); void distToWaypointChanged(double val,QString name); void groundSpeedChanged(double val, QString name); + void airSpeedChanged(double val, QString name); void bearingToWaypointChanged(double val,QString name); - - //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 fba940b3b23b80676899cb152c54d95b8fa48fba..c5473df355c36ba5c36a428585c675a7fa0ec0bf 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -124,7 +124,8 @@ public: virtual double getLatitude() const = 0; virtual double getLongitude() const = 0; - virtual double getAltitude() const = 0; + virtual double getAltitudeAMSL() const = 0; + virtual double getAltitudeRelative() const = 0; virtual bool globalPositionKnown() const = 0; virtual double getRoll() const = 0; @@ -523,16 +524,12 @@ 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 primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec); - void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec); + void altitudeChanged(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, 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 primarySpeedChanged(UASInterface*, double speed, quint64 usec); - void gpsSpeedChanged(UASInterface*, double speed, quint64 usec); - // The vertical speed (a scalar) - void climbRateChanged(UASInterface*, double climb, quint64 usec); + void speedChanged(UASInterface* uas, double groundSpeed, double airSpeed, quint64 usec); // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are. void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec); diff --git a/src/ui/PrimaryFlightDisplay.cc b/src/ui/PrimaryFlightDisplay.cc index cce53d63126473ab90ec851af9a22682a0c8a439..b963bde695135653e4e0d7c835af699dc9ee81f8 100644 --- a/src/ui/PrimaryFlightDisplay.cc +++ b/src/ui/PrimaryFlightDisplay.cc @@ -87,10 +87,6 @@ static const int AIRSPEED_LINEAR_SPAN = 15; static const int AIRSPEED_LINEAR_RESOLUTION = 1; static const int AIRSPEED_LINEAR_MAJOR_RESOLUTION = 5; -static const int UNKNOWN_ATTITUDE = -1000; -static const int UNKNOWN_ALTITUDE = -1000; -static const int UNKNOWN_SPEED = -1; - /* *@TODO: * global fixed pens (and painters too?) @@ -125,26 +121,19 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren uas(NULL), - /* - altimeterMode(GPS_MAIN), - altimeterFrame(ASL), - speedMode(GROUND_MAIN), - */ - - roll(UNKNOWN_ATTITUDE), - pitch(UNKNOWN_ATTITUDE), - heading(UNKNOWN_ATTITUDE), + roll(0), + pitch(0), + heading(0), - primaryAltitude(UNKNOWN_ALTITUDE), - GPSAltitude(UNKNOWN_ALTITUDE), - aboveHomeAltitude(UNKNOWN_ALTITUDE), + altitudeAMSL(NAN), + altitudeRelative(NAN), - primarySpeed(UNKNOWN_SPEED), - groundspeed(UNKNOWN_SPEED), - verticalVelocity(UNKNOWN_ALTITUDE), + groundSpeed(NAN), + airSpeed(NAN), + climbRate(NAN), navigationCrosstrackError(0), - navigationTargetBearing(UNKNOWN_ATTITUDE), + navigationTargetBearing(NAN), layout(COMPASS_INTEGRATED), style(OVERLAY_HSI), @@ -262,32 +251,11 @@ void PrimaryFlightDisplay::forgetUAS(UASInterface* uas) { if (this->uas != NULL && this->uas == 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*, 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))); - //disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - //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(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(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64))); + disconnect(this->uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, quint64))); + disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); } } @@ -308,24 +276,8 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas) // Setup communication connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64))); - - //connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int))); - //connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString))); - //connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - //connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - //connect(uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool))); - //connect(uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString))); - - //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(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*, 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(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64))); + connect(uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, quint64))); connect(uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double))); // Set new UAS @@ -338,20 +290,20 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, double roll, double Q_UNUSED(uas); Q_UNUSED(timestamp); // Called from UAS.cc l. 616 - if (isnan(roll) || isinf(roll)) { - this->roll = UNKNOWN_ATTITUDE; + if (isinf(roll)) { + this->roll = NAN; } else { this->roll = roll * (180.0 / M_PI); } - if (isnan(pitch) || isinf(pitch)) { - this->pitch = UNKNOWN_ATTITUDE; + if (isinf(pitch)) { + this->pitch = NAN; } else { this->pitch = pitch * (180.0 / M_PI); } - if (isnan(yaw) || isinf(yaw)) { - this->heading = UNKNOWN_ATTITUDE; + if (isinf(yaw)) { + this->heading = NAN; } else { yaw = yaw * (180.0 / M_PI); if (yaw<0) yaw+=360; @@ -366,44 +318,21 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, doub this->updateAttitude(uas, roll, pitch, yaw, 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) +void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp) { Q_UNUSED(uas); Q_UNUSED(timestamp); - groundspeed = speed; - if (!didReceivePrimarySpeed) - primarySpeed = speed; + groundSpeed = _groundSpeed; + airSpeed = _airSpeed; } -void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, double climbRate, quint64 timestamp) { +void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp) { Q_UNUSED(uas); Q_UNUSED(timestamp); - verticalVelocity = climbRate; -} - -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); - GPSAltitude = altitude; - if (!didReceivePrimaryAltitude) - primaryAltitude = altitude; + altitudeAMSL = _altitudeAMSL; + altitudeRelative = _altitudeRelative; + climbRate = _climbRate; } void PrimaryFlightDisplay::updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError) { @@ -540,7 +469,7 @@ void PrimaryFlightDisplay::fillInstrumentOpagueBackground(QPainter& painter, QRe } qreal pitchAngleToTranslation(qreal viewHeight, float pitch) { - if (pitch == UNKNOWN_ATTITUDE) + if (isnan(pitch)) return 0; return pitch * viewHeight / PITCHTRANSLATION; } @@ -602,7 +531,7 @@ void PrimaryFlightDisplay::drawAIGlobalFeatures( QRectF paintArea) { float displayRoll = this->roll; - if(displayRoll == UNKNOWN_ATTITUDE) + if (isnan(displayRoll)) displayRoll = 0; painter.resetTransform(); @@ -678,7 +607,7 @@ void PrimaryFlightDisplay::drawPitchScale( ) { float displayPitch = this->pitch; - if (displayPitch == UNKNOWN_ATTITUDE) + if (isnan(displayPitch)) displayPitch = 0; // The area should be quadratic but if not width is the major size. @@ -727,7 +656,7 @@ void PrimaryFlightDisplay::drawPitchScale( else if (displayDegrees<-90) displayDegrees = -180 - displayDegrees; if (SHOW_ZERO_ON_SCALES || degrees) { QString s_number; - if (this->pitch == UNKNOWN_ATTITUDE) + if (isnan(this->pitch)) s_number.sprintf("-"); else s_number.sprintf("%d", displayDegrees); @@ -794,7 +723,7 @@ void PrimaryFlightDisplay::drawAIAttitudeScales( float intrusion ) { float displayRoll = this->roll; - if (displayRoll == UNKNOWN_ATTITUDE) + if (isnan(displayRoll)) displayRoll = 0; // To save computations, we do these transformations once for both scales: painter.resetTransform(); @@ -809,7 +738,7 @@ void PrimaryFlightDisplay::drawAIAttitudeScales( void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, float halfspan) { float displayHeading = this->heading; - if(displayHeading == UNKNOWN_ATTITUDE) + if (isnan(displayHeading)) displayHeading = 0; float start = displayHeading - halfspan; @@ -845,7 +774,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo bool isMajor = displayTick % COMPASS_DISK_MAJORTICK == 0; // If heading unknown, still draw marks but no numbers. - if (this->heading != UNKNOWN_ATTITUDE && + if (!isnan(this->heading) && (displayTick==30 || displayTick==60 || displayTick==120 || displayTick==150 || displayTick==210 || displayTick==240 || @@ -870,7 +799,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo drewArrow = true; } // If heading unknown, still draw marks but no N S E W. - if (this->heading != UNKNOWN_ATTITUDE && displayTick%90 == 0) { + if (!isnan(this->heading) && displayTick%90 == 0) { // Also draw a label QString name = compassWindNames[displayTick / 45]; painter.setPen(scalePen); @@ -911,7 +840,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo QString s_digitalCompass; - if (this->heading == UNKNOWN_ATTITUDE) + if (isnan(this->heading)) s_digitalCompass.sprintf("---"); else { /* final safeguard for really stupid systems */ @@ -931,7 +860,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo // navigationCrosstrackError = 500; // The CDI - if (shouldDisplayNavigationData() && navigationTargetBearing != UNKNOWN_ATTITUDE && !isinf(navigationCrosstrackError)) { + if (shouldDisplayNavigationData() && !isnan(navigationTargetBearing) && !isinf(navigationCrosstrackError)) { painter.resetTransform(); painter.translate(area.center()); // TODO : Sign might be wrong? @@ -964,12 +893,12 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo void PrimaryFlightDisplay::drawAltimeter( QPainter& painter, - QRectF area, // the area where to draw the tape. - float primaryAltitude, - float secondaryAltitude, - float vv + QRectF area ) { + float primaryAltitude = altitudeAMSL; + float secondaryAltitude = 0; + painter.resetTransform(); fillInstrumentBackground(painter, area); @@ -986,7 +915,7 @@ void PrimaryFlightDisplay::drawAltimeter( float effectiveHalfHeight = h*0.45; // not yet implemented: Display of secondary altitude. - if (secondaryAltitude != UNKNOWN_ALTITUDE) { + if (!isnan(secondaryAltitude)) { effectiveHalfHeight-= secondaryAltitudeBoxHeight; } @@ -998,7 +927,7 @@ void PrimaryFlightDisplay::drawAltimeter( float tickmarkRightMinor = tickmarkLeft+TAPE_GAUGES_TICKWIDTH_MINOR*w; float numbersLeft = 0.42*w; float markerTip = (tickmarkLeft*2+tickmarkRightMajor)/3; - float scaleCenterAltitude = primaryAltitude == UNKNOWN_ALTITUDE ? 0 : primaryAltitude; + float scaleCenterAltitude = isnan(primaryAltitude) ? 0 : primaryAltitude; // altitude scale float start = scaleCenterAltitude - ALTIMETER_LINEAR_SPAN/2; @@ -1045,7 +974,7 @@ void PrimaryFlightDisplay::drawAltimeter( painter.setPen(pen); QString s_alt; - if(primaryAltitude == UNKNOWN_ALTITUDE) + if (isnan(primaryAltitude)) s_alt.sprintf("---"); else s_alt.sprintf("%3.0f", primaryAltitude); @@ -1054,32 +983,33 @@ void PrimaryFlightDisplay::drawAltimeter( drawTextCenter(painter, s_alt, mediumTextSize, xCenter, 0); // draw simple in-tape VVI. - if (vv != UNKNOWN_ALTITUDE) { - float vvPixHeight = -vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight; - if (abs (vvPixHeight)