From 6e5f61935c12a1e435f4f97d4fc02bf7cab26d2e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Apr 2020 16:37:11 +1100 Subject: [PATCH] AP_NavEKF2: make mag variance reporting consistent logged scaled variance should match the value used in MAVLink EKF_STATUS_REPORT --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index ccb958b05b..88c013a514 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -578,6 +578,8 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) Vector2f offset; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); + // Only report range finder normalised innovation levels if the EKF needs the data for primary // height estimation or optical flow operation. This prevents false alarms at the GCS if a // range finder is fitted for other applications @@ -589,7 +591,7 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) } // send message - mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar); + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); } // report the reason for why the backend is refusing to initialise