Commit e0534e82 authored by Lorenz Meier's avatar Lorenz Meier

Better differentiation between estimator position and raw GPS

parent 21bfa9e5
...@@ -80,6 +80,10 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), ...@@ -80,6 +80,10 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
latitude(0.0), latitude(0.0),
longitude(0.0), longitude(0.0),
altitude(0.0), altitude(0.0),
globalEstimatorActive(false),
latitude_gps(0.0),
longitude_gps(0.0),
altitude_gps(0.0),
roll(0.0), roll(0.0),
pitch(0.0), pitch(0.0),
yaw(0.0), yaw(0.0),
...@@ -733,6 +737,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -733,6 +737,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
latitude = pos.lat/(double)1E7; latitude = pos.lat/(double)1E7;
longitude = pos.lon/(double)1E7; longitude = pos.lon/(double)1E7;
altitude = pos.alt/1000.0; altitude = pos.alt/1000.0;
globalEstimatorActive = true;
speedX = pos.vx/100.0; speedX = pos.vx/100.0;
speedY = pos.vy/100.0; speedY = pos.vy/100.0;
speedZ = pos.vz/100.0; speedZ = pos.vz/100.0;
...@@ -772,10 +777,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -772,10 +777,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if (pos.fix_type > 2) if (pos.fix_type > 2)
{ {
emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); latitude_gps = pos.lat/(double)1E7;
latitude = pos.lat/(double)1E7; longitude_gps = pos.lon/(double)1E7;
longitude = pos.lon/(double)1E7; altitude_gps = pos.alt/1000.0;
altitude = pos.alt/1000.0;
if (!globalEstimatorActive) {
latitude = latitude_gps;
longitude = longitude_gps;
altitude = altitude_gps;
emit globalPositionChanged(this, latitude, longitude, altitude, time);
}
positionLock = true; positionLock = true;
isGlobalPositionKnown = true; isGlobalPositionKnown = true;
...@@ -784,18 +796,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -784,18 +796,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if (!isnan(alt) && !isinf(alt)) if (!isnan(alt) && !isinf(alt))
{ {
alt = 0; alt = 0;
//emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
} }
// FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
// Smaller than threshold and not NaN // Smaller than threshold and not NaN
float vel = pos.vel/100.0f; float vel = pos.vel/100.0f;
if (vel < 1000000 && !isnan(vel) && !isinf(vel)) if (!globalEstimatorActive && (vel < 1000000) && !isnan(vel) && !isinf(vel))
{ {
// FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time); emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
//qDebug() << "GOT GPS RAW";
// emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
} }
else else
{ {
...@@ -905,7 +914,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -905,7 +914,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Emit change // Emit change
emit parameterChanged(uasId, message.compid, parameterName, param); emit parameterChanged(uasId, message.compid, parameterName, param);
emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param);
// qDebug() << "RECEIVED PARAM:" << param;
} }
break; break;
case MAV_PARAM_TYPE_INT32: case MAV_PARAM_TYPE_INT32:
......
...@@ -278,9 +278,13 @@ protected: //COMMENTS FOR TEST UNIT ...@@ -278,9 +278,13 @@ protected: //COMMENTS FOR TEST UNIT
double localX; double localX;
double localY; double localY;
double localZ; double localZ;
double latitude; double latitude; ///< Global latitude as estimated by position estimator
double longitude; double longitude; ///< Global longitude as estimated by position estimator
double altitude; double altitude; ///< Global altitude as estimated by position estimator
bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position
double latitude_gps; ///< Global latitude as estimated by raw GPS
double longitude_gps; ///< Global longitude as estimated by raw GPS
double altitude_gps; ///< Global altitude as estimated by raw GPS
double speedX; ///< True speed in X axis double speedX; ///< True speed in X axis
double speedY; ///< True speed in Y axis double speedY; ///< True speed in Y axis
double speedZ; ///< True speed in Z axis double speedZ; ///< True speed in Z axis
......
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