From d69370c8b4e7e59b453ef786cade582e3fbc901d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 24 Jan 2024 12:36:55 -0500 Subject: [PATCH] vehicle_attitude: add RPY variance --- msg/VehicleAttitude.msg | 6 +++--- msg/VehicleLocalPosition.msg | 2 -- src/modules/ekf2/EKF2.cpp | 4 +++- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/msg/VehicleAttitude.msg b/msg/VehicleAttitude.msg index 46e1fc0bcb..e199791654 100644 --- a/msg/VehicleAttitude.msg +++ b/msg/VehicleAttitude.msg @@ -1,12 +1,12 @@ -# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use - uint64 timestamp # time since system start (microseconds) - uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame + float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter +float32[3] variance # roll/pitch/yaw variance (rad^2) + # TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude # TOPICS estimator_attitude diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index c1a0dffe14..7051ff5e0e 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -44,8 +44,6 @@ float32 delta_heading # Heading delta caused by latest heading reset [rad] uint8 heading_reset_counter # Index of latest heading reset bool heading_good_for_control -float32 tilt_var - # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) bool z_global # true if z has a valid global reference (ref_alt) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b13d5104fd..d836efdd9f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -920,6 +920,9 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) _ekf.getQuaternion().copyTo(att.q); _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); + + _ekf.getRotVarNed().copyTo(att.variance); + att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _attitude_pub.publish(att); @@ -1582,7 +1585,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.heading_var = _ekf.getYawVar(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); - lpos.tilt_var = _ekf.getTiltVariance(); #if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive