Skip to content

Q
qgroundcontrol
Commit 17883dbb authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge pull request #428 from DrTon/alt_speed_fix 

Altitude and speed rewrite
parents 9dea5ac9 fd497bc6
Show whitespace changes
Inline Side-by-side
Showing with 193 additions and 270 deletions
src/comm/QGCXPlaneLink.cc
View file @ 17883dbb
......@@ -911,14 +911,14 @@ void QGCXPlaneLink::setRandomPosition()
double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
double offAlt = rand() / static_cast<double>(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);
......
src/uas/UAS.cc
View file @ 17883dbb
......@@ -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);
}
positionLock = true;
isGlobalPositionKnown = true;
// Smaller than threshold and not NaN
setAltitudeAMSL(altitude_gps);
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
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);
// 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));
}
}
......
src/uas/UAS.h
View file @ 17883dbb
......@@ -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
......@@ -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);
......
src/uas/UASInterface.h
View file @ 17883dbb
......@@ -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);
......
src/ui/PrimaryFlightDisplay.cc
View file @ 17883dbb
......@@ -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,9 +983,10 @@ 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)<markerHalfHeight) return; // hidden behind marker.
if (!isnan(climbRate)) {
float vvPixHeight = -climbRate/ALTIMETER_VVI_SPAN * effectiveHalfHeight;
if (abs (vvPixHeight) < markerHalfHeight)
return; // hidden behind marker.
float vvSign = vvPixHeight>0 ? 1 : -1; // reverse y sign
......@@ -1079,7 +1009,7 @@ void PrimaryFlightDisplay::drawAltimeter(
}
// print secondary altitude
if (secondaryAltitude != UNKNOWN_ALTITUDE) {
if (!isnan(secondaryAltitude)) {
QRectF saBox(area.x(), area.y()-secondaryAltitudeBoxHeight, w, secondaryAltitudeBoxHeight);
painter.resetTransform();
painter.translate(saBox.center());
......@@ -1093,9 +1023,7 @@ void PrimaryFlightDisplay::drawAltimeter(
void PrimaryFlightDisplay::drawVelocityMeter(
QPainter& painter,
QRectF area,
float speed,
float secondarySpeed
QRectF area
) {
painter.resetTransform();
......@@ -1117,8 +1045,8 @@ void PrimaryFlightDisplay::drawVelocityMeter(
// Select between air and ground speed:
float centerScaleSpeed =
speed == UNKNOWN_SPEED ? 0 : speed;
float speed = (isAirplane() && isnan(airSpeed)) ? airSpeed : groundSpeed;
float centerScaleSpeed = isnan(speed) ? 0 : speed;
float start = centerScaleSpeed - AIRSPEED_LINEAR_SPAN/2;
float end = centerScaleSpeed + AIRSPEED_LINEAR_SPAN/2;
......@@ -1166,7 +1094,7 @@ void PrimaryFlightDisplay::drawVelocityMeter(
pen.setColor(Qt::white);
painter.setPen(pen);
QString s_alt;
if (speed == UNKNOWN_SPEED)
if (isnan(speed))
s_alt.sprintf("---");
else
s_alt.sprintf("%3.1f", speed);
......@@ -1505,9 +1433,9 @@ void PrimaryFlightDisplay::doPaint() {
painter.setClipping(hadClip);
drawAltimeter(painter, altimeterArea, primaryAltitude, GPSAltitude, verticalVelocity);
drawAltimeter(painter, altimeterArea);
drawVelocityMeter(painter, velocityMeterArea, primarySpeed, groundspeed);
drawVelocityMeter(painter, velocityMeterArea);
/*
drawSensorsStatsPanel(painter, sensorsStatsArea);
......
src/ui/PrimaryFlightDisplay.h
View file @ 17883dbb
......@@ -18,11 +18,8 @@ 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 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 updateSpeed(UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
void updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp);
void updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError);
/** @brief Set the currently monitored UAS */
......@@ -102,8 +99,8 @@ private:
void drawAICompassDisk(QPainter& painter, QRectF area, float halfspan);
void drawSeparateCompassDisk(QPainter& painter, QRectF area);
void drawAltimeter(QPainter& painter, QRectF area, float altitude, float secondaryAltitude, float vv);
void drawVelocityMeter(QPainter& painter, QRectF area, float speed, float secondarySpeed);
void drawAltimeter(QPainter& painter, QRectF area);
void drawVelocityMeter(QPainter& painter, QRectF area);
void fillInstrumentBackground(QPainter& painter, QRectF edge);
void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge);
void drawInstrumentBackground(QPainter& painter, QRectF edge);
......@@ -125,24 +122,23 @@ private:
SpeedMode speedMode;
*/
bool didReceivePrimaryAltitude;
bool didReceivePrimarySpeed;
bool didReceiveSpeed;
float roll;
float pitch;
float heading;
float primaryAltitude;
float GPSAltitude;
float altitudeAMSL;
float altitudeRelative;
// APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message.
// Do !!!NOT!!! ever do altitude calculations at the ground station. There are enough pitfalls already.
// If the MP "set home altitude" button is migrated to here, it must set the UAS home altitude, not a GS-local one.
float aboveHomeAltitude;
float primarySpeed;
float groundspeed;
float verticalVelocity;
float groundSpeed;
float airSpeed;
float climbRate;
float navigationAltitudeError;
float navigationSpeedError;
......
src/ui/WaypointList.cc
View file @ 17883dbb
......@@ -306,7 +306,7 @@ void WaypointList::addEditable(bool onCurrentPosition)
} else {
updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint."));
}
wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitudeAMSL(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
WPM->addWaypointEditable(wp);
} else {
......
src/ui/map/QGCMapWidget.cc
View file @ 17883dbb
......@@ -462,7 +462,7 @@ void QGCMapWidget::updateGlobalPosition()
// Set new lat/lon position of UAV icon
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitudeAMSL());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
......@@ -490,7 +490,7 @@ void QGCMapWidget::updateLocalPosition()
// Set new lat/lon position of UAV icon
internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitude());
uav->SetUAVPos(pos_lat_lon, system->getAltitudeAMSL());
// Follow status
if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
// Convert from radians to degrees and apply
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ 17883dbb
......@@ -682,7 +682,7 @@ Pixhawk3DWidget::selectTargetHeading(void)
if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
{
double altitude = mActiveUAS->getAltitude();
double altitude = mActiveUAS->getAltitudeAMSL();
QPointF cursorWorldCoords =
m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);
......@@ -722,7 +722,7 @@ Pixhawk3DWidget::selectTarget(void)
if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
{
double altitude = mActiveUAS->getAltitude();
double altitude = mActiveUAS->getAltitudeAMSL();
QPointF cursorWorldCoords =
m3DWidget->worldCursorPosition(mCachedMousePos, altitude);
......@@ -789,7 +789,7 @@ Pixhawk3DWidget::insertWaypoint(void)
{
double latitude = mActiveUAS->getLatitude();
double longitude = mActiveUAS->getLongitude();
double altitude = mActiveUAS->getAltitude();
double altitude = mActiveUAS->getAltitudeAMSL();
double x, y;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
......@@ -845,7 +845,7 @@ Pixhawk3DWidget::moveWaypointPosition(void)
{
double latitude = mActiveUAS->getLatitude();
double longitude = mActiveUAS->getLongitude();
double altitude = mActiveUAS->getAltitude();
double altitude = mActiveUAS->getAltitudeAMSL();
double x, y;
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
......@@ -1700,7 +1700,7 @@ Pixhawk3DWidget::getPose(UASInterface* uas,
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double altitude = uas->getAltitudeAMSL();
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
......@@ -1742,7 +1742,7 @@ Pixhawk3DWidget::getPosition(UASInterface* uas,
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double altitude = uas->getAltitudeAMSL();
Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
z = -altitude;
......
src/ui/map3D/QGCGoogleEarthView.cc
View file @ 17883dbb
......@@ -632,7 +632,7 @@ void QGCGoogleEarthView::updateState()
uasId = currMav->getUASID();
lat = currMav->getLatitude();
lon = currMav->getLongitude();
alt = currMav->getAltitude();
alt = currMav->getAltitudeAMSL();
roll = currMav->getRoll();
pitch = currMav->getPitch();
yaw = currMav->getYaw();
......
src/ui/map3D/WaypointGroupNode.cc
View file @ 17883dbb
......@@ -65,7 +65,7 @@ WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude();
double altitude = uas->getAltitudeAMSL();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
......@@ -75,7 +75,7 @@ WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
{
double latitude = uas->getLatitude();
double longitude = uas->getLongitude();
double altitude = uas->getAltitude() + UASManager::instance()->getHomeAltitude();
double altitude = uas->getAltitudeRelative();
QString utmZone;
Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
......
src/ui/uas/UASListWidget.cc
View file @ 17883dbb
......@@ -165,7 +165,6 @@ void UASListWidget::addUAS(UASInterface* uas)
uWidget = NULL;
}
}
if (!uasViews.contains(uas))
{
// Only display the UAS in a single link.
......
src/ui/uas/UASQuickView.cc
View file @ 17883dbb
......@@ -29,11 +29,10 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent)
//If we don't have any predefined settings, set some defaults.
if (uasPropertyValueMap.size() == 0)
{
valueEnabled("altitude");
valueEnabled("altitudeAMSL");
valueEnabled("altitudeRelative");
valueEnabled("groundSpeed");
valueEnabled("distToWP");
valueEnabled("yaw");
valueEnabled("roll");
}
QAction *action = new QAction("Add/Remove Items",this);
......
src/ui/uas/UASQuickViewTextItem.cc
View file @ 17883dbb
......@@ -10,7 +10,6 @@ UASQuickViewTextItem::UASQuickViewTextItem(QWidget *parent) : UASQuickViewItem(p
// Create the title label. Scale the font based on available size.
titleLabel = new QLabel(this);
// <<<<<<< HEAD
titleLabel->setAlignment(Qt::AlignHCenter);
titleLabel->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Minimum);
titleLabel->setObjectName(QString::fromUtf8("title"));
......@@ -25,18 +24,6 @@ UASQuickViewTextItem::UASQuickViewTextItem(QWidget *parent) : UASQuickViewItem(p
valueLabel->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Minimum);
valueLabel->setObjectName(QString::fromUtf8("value"));
valueLabel->setText("0.00");
// =======
// titleLabel->setSizePolicy(QSizePolicy::Ignored,QSizePolicy::Ignored);
// titleLabel->setAlignment(Qt::AlignHCenter | Qt::AlignBottom);
// this->layout()->addWidget(titleLabel);
// valueLabel = new QLabel(this);
// valueLabel->setSizePolicy(QSizePolicy::Ignored,QSizePolicy::Ignored);
// valueLabel->setAlignment(Qt::AlignHCenter | Qt::AlignTop);
// valueLabel->setText("0.00");
// this->layout()->addWidget(valueLabel);
// //spacerItem = new QSpacerItem(20,40,QSizePolicy::Minimum,QSizePolicy::Ignored);
// //layout->addSpacerItem(spacerItem);
// >>>>>>> 34eaf1fb422146f5df3b01fad4d756343b3127c9
QFont valuefont = valueLabel->font();
valuefont.setPixelSize(this->height() / 2.0);
valueLabel->setFont(valuefont);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment