Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

update ecl and add param for innovation consistency checks for range aid fusion (#7585)

Browse Source
This commit is contained in:
ChristophTobler
2017-07-19 10:25:12 +02:00
committed by Paul Riseborough
parent 1dbac79c1b
commit 03e11c4d18
3 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 18e81eba31...9b6e1ab2a7
src/modules/ekf2/ekf2_main.cpp
+5 -1
View File
@@ -256,6 +256,8 @@ private:
_rng_aid; // enables use of a range finder even if primary height source is not range finder (EKF2_HGT_MODE != 2)
control::BlockParamExtFloat _rng_aid_hor_vel_max; // maximum allowed horizontal velocity for range aid
control::BlockParamExtFloat _rng_aid_height_max; // maximum allowed absolute altitude (AGL) for range aid
control::BlockParamExtFloat
_rng_aid_innov_gate; // gate size used for innovation consistency checks for range aid fusion
// vision estimate fusion
control::BlockParamExtFloat _ev_pos_noise; // default position observation noise for exernal vision measurements (m)
@@ -388,6 +390,7 @@ Ekf2::Ekf2():
_rng_aid(this, "EKF2_RNG_AID", false, _params->range_aid),
_rng_aid_hor_vel_max(this, "EKF2_RNG_A_VMAX", false, _params->max_vel_for_range_aid),
_rng_aid_height_max(this, "EKF2_RNG_A_HMAX", false, _params->max_hagl_for_range_aid),
_rng_aid_innov_gate(this, "EKF2_RNG_A_IGATE", false, _params->range_aid_innov_gate),
_ev_pos_noise(this, "EKF2_EVP_NOISE", false, _default_ev_pos_noise),
_ev_ang_noise(this, "EKF2_EVA_NOISE", false, _default_ev_ang_noise),
_ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate),
@@ -933,7 +936,8 @@ void Ekf2::run()
lpos.yaw = euler.psi();
float terrain_vpos;
lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
_ekf.get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = now; // Time when new bottom surface found
src/modules/ekf2/ekf2_params.c
+13
View File
@@ -1043,3 +1043,16 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f);
* @max 10.0
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f);
/**
* Gate size used for innovation consistency checks for range aid fusion
*
* A lower value means HAGL needs to be more stable in order to use range finder for height estimation
* in range aid mode
*
* @group EKF2
* @unit SD
* @min 0.1
* @max 5.0
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);