From 26c3295042ec7522a78022560835f275b7b37482 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 13 Oct 2016 15:20:16 +0900 Subject: [PATCH] Copter: ignore first ekf core switch The ekf core is initialised to -1 but after initialisation changes to zero. Ignore this first change. --- ArduCopter/Copter.h | 2 +- ArduCopter/ekf_check.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 716cb04e05..ebd58288a3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -223,7 +223,7 @@ private: // system time in milliseconds of last recorded yaw reset from ekf uint32_t ekfYawReset_ms = 0; - uint8_t ekf_primary_core; + int8_t ekf_primary_core; // GCS selection AP_SerialManager serial_manager; diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 8cbc36f374..21393e0f08 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -183,10 +183,10 @@ void Copter::check_ekf_reset() #if AP_AHRS_NAVEKF_AVAILABLE // check for change in primary EKF (log only, AC_WPNav handles position target adjustment) - if (EKF2.getPrimaryCoreIndex() != ekf_primary_core) { + if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) { ekf_primary_core = EKF2.getPrimaryCoreIndex(); Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core); - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "EKF primary changed:%u\n", (unsigned)ekf_primary_core); + gcs_send_text_fmt(MAV_SEVERITY_WARNING, "EKF primary changed:%d\n", (unsigned)ekf_primary_core); } #endif }