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

ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig

Browse Source 
- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
Daniel Agar
2023-09-15 10:02:09 -04:00
parent 845b01a00d
commit 10b54d08fc
14 changed files with 367 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/range_height_control.cpp
+6 -1
View File
@@ -204,6 +204,7 @@ void Ekf::controlRangeHeightFusion()
bool Ekf::isConditionalRangeAidSuitable()
{
#if defined(CONFIG_EKF2_TERRAIN)
if (_control_status.flags.in_air
&& _range_sensor.isHealthy()
&& isTerrainEstimateValid()) {
@@ -212,7 +213,10 @@ bool Ekf::isConditionalRangeAidSuitable()
float range_hagl_max = _params.max_hagl_for_range_aid;
float max_vel_xy = _params.max_vel_for_range_aid;
const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
const float hagl_innov = _aid_src_terrain_range_finder.innovation;
const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
bool is_hagl_stable = (hagl_test_ratio < 1.f);
@@ -234,6 +238,7 @@ bool Ekf::isConditionalRangeAidSuitable()
return is_in_range && is_hagl_stable && is_below_max_speed;
}
#endif // CONFIG_EKF2_TERRAIN
return false;
}