From 1b13dd053bc8ef1cb73a0e228eb3438d38f3f249 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Jul 2022 14:37:31 +1000 Subject: [PATCH] AP_NavEKF3: use send_mesage for sending status report --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index d99dff3594..4c63c0c6b3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -575,19 +575,27 @@ void NavEKF3_core::send_status_report(GCS_MAVLINK &link) const Vector2f offset; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + // 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 - float temp; + float temp = 0; if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { temp = sqrtF(auxRngTestRatio); - } else { - temp = 0.0f; } - const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); + + const mavlink_ekf_status_report_t packet{ + velVar, + posVar, + hgtVar, + fmaxF(fmaxF(magVar.x,magVar.y),magVar.z), + temp, + flags, + tasVar + }; // send message - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar); + mavlink_msg_ekf_status_report_send_struct(link.get_chan(), &packet); } #endif // HAL_GCS_ENABLED