dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: log optical flow gyro bias

This commit is contained in:
Daniel Agar 2023-09-26 10:57:19 -04:00
parent 99197919d7
commit 291af17b64
3 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/VehicleOpticalFlowVel.msg
View File

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

1
src/modules/ekf2/EKF/ekf.h
View File

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

2
src/modules/ekf2/EKF2.cpp
View File

@ -2001,6 +2001,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
_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);