From 231eebe553d521146868f3844b688b3ea02b2cb6 Mon Sep 17 00:00:00 2001 From: Oleksiy Protas Date: Tue, 17 Sep 2024 19:15:17 +0300 Subject: [PATCH] AntennaTracker: refuse to track (0;0) This prevents erratic movements if the vehicle's GPS gives out zeroes at startup --- AntennaTracker/tracking.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index facd7141b5..8a1240d2ee 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -131,6 +131,11 @@ void Tracker::update_tracking(void) */ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) { + // reject (0;0) coordinates + if (!msg.lat && !msg.lon) { + return; + } + vehicle.location.lat = msg.lat; vehicle.location.lng = msg.lon; vehicle.location.alt = msg.alt/10; @@ -139,7 +144,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.last_update_us = AP_HAL::micros(); vehicle.last_update_ms = AP_HAL::millis(); #if HAL_LOGGING_ENABLED - // log vehicle as GPS2 + // log vehicle as VPOS if (should_log(MASK_LOG_GPS)) { Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel); }