From 8be1755b1210a642be12e9d71a78567bc8568daf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Apr 2020 16:37:11 +1100 Subject: [PATCH] AP_NavEKF3: make mag variance reporting consistent logged scaled variance should match the value used in MAVLink EKF_STATUS_REPORT --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index ed18c8bba4..29cdc85fcf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -598,10 +598,10 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) } else { temp = 0.0f; } + const float mag_max = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z); // 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