From b6867e085d213f7d76f3df0c53def800bd6fc82e Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 20 Sep 2016 10:42:06 +1000 Subject: [PATCH] AP_NavEKF2: Don't report range finder variance on mavlink if not required --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index acb7d15a91..bc7de76d20 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -520,8 +520,17 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) Vector2f offset; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + // Only report range finder normalised innovation levels if the EKF needs the data. + // This prevents false alarms at the GCS if a range finder is fitted for other applications + float temp; + if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE) { + temp = sqrtf(auxRngTestRatio); + } else { + temp = 0.0f; + } + // send message - mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), sqrtf(auxRngTestRatio)); + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp); }