Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use send_mesage for sending status report

Browse Source
This commit is contained in:
Peter Barker 2022-07-08 14:37:31 +10:00 committed by Andrew Tridgell
parent 35c2653a87
commit 1b13dd053b
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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