From 485ae596fc594238937598412ec5f657efa1ba3c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 23 Mar 2014 22:05:17 +1100 Subject: [PATCH] AP_Compass: fixed last_update for secondary compass with primary failed this ensures EKF and DCM will use a secondary compass if the primary fails --- libraries/AP_Compass/AP_Compass_PX4.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index 3b50a0c24e..347b0f1072 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -129,9 +129,9 @@ bool AP_Compass_PX4::read(void) _count[i] = 0; } - last_update = _last_timestamp[0]; + last_update = _last_timestamp[_get_primary()]; - return _healthy[0]; + return _healthy[_get_primary()]; } void AP_Compass_PX4::accumulate(void)