fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fusion helpers return success/fail and set pos/vel update timestamps centrally (if healthy)

Browse Source
This commit is contained in:
Daniel Agar
2022-03-08 11:16:35 -05:00
parent c4bc062714
commit c10ea97967
6 changed files with 105 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+8 -9
View File
@@ -102,8 +102,8 @@ public:
void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
@@ -219,8 +219,8 @@ public:
{
const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
const bool is_mag_alignment_in_flight_complete = is_using_mag
&& _control_status.flags.mag_aligned_in_flight
&& ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
&& _control_status.flags.mag_aligned_in_flight
&& ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
return _control_status.flags.yaw_align
&& (is_mag_alignment_in_flight_complete || !is_using_mag);
}
@@ -380,7 +380,6 @@ private:
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
@@ -584,20 +583,20 @@ private:
// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
bool fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
bool fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// innovation : prediction - measurement
// variance : observaton variance
// gate_sigma : innovation consistency check gate size (Sigma)
// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
void updateQuaternion(const float innovation, const float variance, const float gate_sigma,
bool updateQuaternion(const float innovation, const float variance, const float gate_sigma,
const Vector4f &yaw_jacobian);
// fuse the yaw angle obtained from a dual antenna GPS unit
@@ -629,7 +628,7 @@ private:
void fuseEvHgt();
// fuse single velocity and position measurement
void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
void resetVelocity();
@@ -1024,7 +1023,7 @@ private:
void stopFakePosFusion();
void fuseFakePosition();
void setVelPosFaultStatus(const int index, const bool status);
void setVelPosStatus(const int index, const bool healthy);
// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
// yaw : Euler yaw angle (rad)