From 291af17b64eea8cecf66fe8c3ff0a93d1e4f0e20 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Sep 2023 10:57:19 -0400 Subject: [PATCH] ekf2: log optical flow gyro bias --- msg/VehicleOpticalFlowVel.msg | 2 ++ src/modules/ekf2/EKF/ekf.h | 1 + src/modules/ekf2/EKF2.cpp | 2 ++ 3 files changed, 5 insertions(+) diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg index c89207f4ab..940f03650d 100644 --- a/msg/VehicleOpticalFlowVel.msg +++ b/msg/VehicleOpticalFlowVel.msg @@ -10,4 +10,6 @@ float32[2] flow_compensated_integral # integrated optical flow measurement com float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) +float32[3] gyro_bias + # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 87b39db92f..28a5adf795 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -183,6 +183,7 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } + const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; } #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AUXVEL) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 426707abb9..a6a67c6763 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2001,6 +2001,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); _ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral); + _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel);