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

ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param

Browse Source
This commit is contained in:
Daniel Agar
2022-11-08 11:46:41 -05:00
parent 688dae1108
commit 5239993c88
18 changed files with 406 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_height_control.cpp
+22 -2
View File
@@ -94,8 +94,28 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
bias_est.setBias(-_state.pos(2) + measurement);
// reset vertical velocity
if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
switch (ev_sample.vel_frame) {
case VelocityFrame::LOCAL_FRAME_NED:
case VelocityFrame::LOCAL_FRAME_FRD: {
const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
}
break;
case VelocityFrame::BODY_FRAME_FRD: {
const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
}
break;
}
} else {
resetVerticalVelocityToZero();