Skip to content

Q
qgroundcontrol
Commit 1c5065a1 authored by lm's avatar lm
Browse files
Options

Ignoring GPS messages without fix

parent e1c4f50e
Hide whitespace changes
Inline Side-by-side
Showing with 24 additions and 20 deletions
src/uas/UAS.cc
View file @ 1c5065a1
...@@ -365,28 +365,32 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -365,28 +365,32 @@ 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();
emit valueChanged(uasId, "lat", pos.lat, time); if (pos.fix_type > 0)
emit valueChanged(uasId, "lon", pos.lon, time);
// Check for NaN
int alt = pos.alt;
if (alt != alt)
{
alt = 0;
emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
}
emit valueChanged(uasId, "alt", pos.alt, time);
// Smaller than threshold and not NaN
if (pos.v < 1000000 && pos.v == pos.v)
{
emit valueChanged(uasId, "speed", pos.v, time);
//qDebug() << "GOT GPS RAW";
emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
}
else
{ {
emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); emit valueChanged(uasId, "lat", pos.lat, time);
emit valueChanged(uasId, "lon", pos.lon, time);
emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
// Check for NaN
int alt = pos.alt;
if (alt != alt)
{
alt = 0;
emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
}
emit valueChanged(uasId, "alt", pos.alt, time);
// Smaller than threshold and not NaN
if (pos.v < 1000000 && pos.v == pos.v)
{
emit valueChanged(uasId, "speed", pos.v, time);
//qDebug() << "GOT GPS RAW";
emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
}
else
{
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