Commit 1c5065a1 authored by lm's avatar lm

Ignoring GPS messages without fix

parent e1c4f50e
...@@ -365,8 +365,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -365,8 +365,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// only accept values in a realistic range // only accept values in a realistic range
// quint64 time = getUnixTime(pos.usec); // quint64 time = getUnixTime(pos.usec);
quint64 time = MG::TIME::getGroundTimeNow(); quint64 time = MG::TIME::getGroundTimeNow();
if (pos.fix_type > 0)
{
emit valueChanged(uasId, "lat", pos.lat, time); emit valueChanged(uasId, "lat", pos.lat, time);
emit valueChanged(uasId, "lon", pos.lon, time); emit valueChanged(uasId, "lon", pos.lon, time);
emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
// Check for NaN // Check for NaN
int alt = pos.alt; int alt = pos.alt;
if (alt != alt) if (alt != alt)
...@@ -386,7 +390,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -386,7 +390,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{ {
emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
} }
emit globalPositionChanged(this, pos.lon, pos.lat, alt, time); }
} }
break; break;
case MAVLINK_MSG_ID_GPS_STATUS: case MAVLINK_MSG_ID_GPS_STATUS:
......
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