Commit 6dc95c6b authored by DonLakeFlyer's avatar DonLakeFlyer

Handle ArduPilot GPS message

parent 08abea5b
......@@ -669,6 +669,11 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
mavlink_global_position_int_t globalPositionInt;
mavlink_msg_global_position_int_decode(&message, &globalPositionInt);
// ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat/alt 0/0/0 even when it has not gps signal
if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0 && globalPositionInt.alt == 0) {
return;
}
_globalPositionIntMessageAvailable = true;
//-- Set these here and emit a single signal instead of 3 for the same variable (_coordinate)
_coordinate.setLatitude(globalPositionInt.lat / (double)1E7);
......
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