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);