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

ekf2: update EV height state machine (small piece of EV overhaul)

Browse Source 
- respect new EKF2_EV_CTRL parameter for VPOS usage
  - EV hgt rotate EV position before usage (there's often a small offset in frames)
  - EV hgt reset use proper EV velocity body frame
  - try to keep EV hgt and EV vel state machines consistent
  - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128
This commit is contained in:
Daniel Agar
2022-12-13 13:29:18 -05:00
committed by GitHub
parent 805ffa9d0b
commit 696eeb9a49
12 changed files with 149 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_vel_control.cpp
+4 -4
View File
@@ -38,8 +38,8 @@
#include "ekf.h"
void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset,
bool quality_sufficient, estimator_aid_source3d_s &aid_src)
void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "EV velocity";
@@ -134,8 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_con
continuing_conditions_passing = false;
}
starting_conditions_passing &= continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
if (_control_status.flags.ev_vel) {
aid_src.fusion_enabled = true;