fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:54:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Optical flow: compute velociy from corrected flow data for logging (#920)

Browse Source 
This is really useful when debugging optical flow data
This commit is contained in:
Mathieu Bresciani 2020-10-26 13:27:52 +01:00 committed by GitHub
parent 9126282c03
commit dd3ffc4192
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/ekf.h
View File

@ -105,6 +105,12 @@ public:
void getFlowInnovRatio(float &flow_innov_ratio) const override;
Vector2f getFlowVelBody() const override;
Vector2f getFlowVelNE() const override;
Vector2f getFlowCompensated() const override;
Vector2f getFlowUncompensated() const override;
Vector3f getFlowGyro() const override;
void getHeadingInnov(float &heading_innov) const override;
void getHeadingInnovVar(float &heading_innov_var) const override;
@ -437,6 +443,8 @@ private:
Vector2f _flow_innov; ///< flow measurement innovation (rad/sec)
Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2)
Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
Vector2f _flow_vel_body; ///< velocity from corrected flow measurement (body frame)(m/s)
Vector2f _flow_vel_ne; ///< velocity from corrected flow measurement (local frame) (m/s)
Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)

25
EKF/ekf_helper.cpp
View File

@ -708,6 +708,31 @@ void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const
flow_innov_ratio = _optflow_test_ratio;
}
Vector2f Ekf::getFlowVelBody() const
{
return _flow_vel_body;
}
Vector2f Ekf::getFlowVelNE() const
{
return _flow_vel_ne;
}
Vector2f Ekf::getFlowCompensated() const
{
return _flow_compensated_XY_rad;
}
Vector2f Ekf::getFlowUncompensated() const
{
return _flow_sample_delayed.flow_xy_rad;
}
Vector3f Ekf::getFlowGyro() const
{
return _flow_sample_delayed.gyro_xyz;
}
void Ekf::getHeadingInnov(float &heading_innov) const
{
heading_innov = _heading_innov;

5
EKF/estimator_interface.h
View File

@ -91,6 +91,11 @@ public:
virtual void getFlowInnov(float flow_innov[2]) const = 0;
virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0;
virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0;
virtual Vector2f getFlowVelBody() const = 0;
virtual Vector2f getFlowVelNE() const = 0;
virtual Vector2f getFlowCompensated() const = 0;
virtual Vector2f getFlowUncompensated() const = 0;
virtual Vector3f getFlowGyro() const = 0;
virtual void getHeadingInnov(float &heading_innov) const = 0;
virtual void getHeadingInnovVar(float &heading_innov_var) const = 0;

6
EKF/optflow_fusion.cpp
View File

@ -102,6 +102,12 @@ void Ekf::fuseOptFlow()
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
// compute the velocities in body and local frames from corrected optical flow measurement
// for logging only
_flow_vel_body(0) = -opt_flow_rate(1) * range;
_flow_vel_body(1) = opt_flow_rate(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis