Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: fill UTM_GLOBAL_POSITION accuracy fields from EKF covariance

Browse Source 
Use AP_AHRS::get_pos_vel_uncertainty() to populate h_acc, v_acc and
vel_acc in UTM_GLOBAL_POSITION rather than sending zero.  The values
come from the EKF3 state error covariance P matrix:

  h_acc   [mm]   = sqrt(P[7][7] + P[8][8]) * 1000  (2D RMS)
  v_acc   [mm]   = sqrt(P[9][9]) * 1000
  vel_acc [cm/s] = sqrt(max(P[4..6][4..6])) * 100

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Peter Barker
2026-03-26 12:56:59 +11:00
committed by Peter Barker
co-authored by Claude Sonnet 4.6
parent 6cf75c55a5
commit c34bb45ca5
1 changed files with 13 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/GCS_MAVLink/GCS_Common.cpp
+13 -3
View File
@@ -6257,6 +6257,16 @@ void GCS_MAVLINK::send_utm_global_position() const
break;
}
uint16_t h_acc_mm = 0, v_acc_mm = 0, vel_acc_cms = 0;
{
float pos_horiz_m, pos_vert_m, vel_m_s;
if (ahrs.get_pos_vel_uncertainty(pos_horiz_m, pos_vert_m, vel_m_s)) {
h_acc_mm = (uint16_t)constrain_float(pos_horiz_m * 1000.0f, 0, UINT16_MAX);
v_acc_mm = (uint16_t)constrain_float(pos_vert_m * 1000.0f, 0, UINT16_MAX);
vel_acc_cms = (uint16_t)constrain_float(vel_m_s * 100.0f, 0, UINT16_MAX);
}
}
mavlink_utm_global_position_t msg_utm {
time : time_usec,
lat : lat,
@@ -6269,9 +6279,9 @@ void GCS_MAVLINK::send_utm_global_position() const
vx : vx,
vy : vy,
vz : vz,
h_acc : 0, // not available from AHRS
v_acc : 0, // not available from AHRS
vel_acc : 0, // not available from AHRS
h_acc : h_acc_mm,
v_acc : v_acc_mm,
vel_acc : vel_acc_cms,
update_rate : 0, // unknown/data-driven
flight_state : flight_state,
flags : flags,