fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 23:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: clean up hagl vs terrain naming

Browse Source 
Terrain is the state: terrain vertical position
Hagl (height above ground level) is the vertical distance between the
vertical position and the terrain vertical position
This commit is contained in:
bresch
2024-06-20 14:42:33 +02:00
committed by Silvan Fuhrer
parent 68980b59e2
commit 9001c23926
9 changed files with 36 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+3 -3
View File
@@ -133,7 +133,7 @@ void Ekf::startFlowFusion()
fuseOptFlow(_hagl_sensor_status.flags.flow);
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
resetHaglFlow();
resetTerrainToFlow();
} else {
ECL_INFO("optical flow fusion failed to start");
@@ -146,7 +146,7 @@ void Ekf::startFlowFusion()
resetFlowFusion();
} else if (_hagl_sensor_status.flags.flow) {
resetHaglFlow();
resetTerrainToFlow();
}
}
@@ -172,7 +172,7 @@ void Ekf::resetFlowFusion()
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
}
void Ekf::resetHaglFlow()
void Ekf::resetTerrainToFlow()
{
ECL_INFO("reset hagl to flow");
// TODO: use the flow data
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+8 -14
View File
@@ -39,7 +39,7 @@
#include "ekf.h"
#include "ekf_derivation/generated/compute_hagl_innov_var.h"
void Ekf::controlRangeHeightFusion()
void Ekf::controlRangeHaglFusion()
{
static constexpr const char *HGT_SRC_NAME = "RNG";
@@ -97,7 +97,7 @@ void Ekf::controlRangeHeightFusion()
if (rng_data_ready && _range_sensor.getSampleAddress()) {
updateRangeHeight(aid_src);
updateRangeHagl(aid_src);
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
@@ -136,7 +136,7 @@ void Ekf::controlRangeHeightFusion()
stopRngTerrFusion();
if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
resetHaglRng(aid_src);
resetTerrainToRng(aid_src);
}
} else if (do_range_aid) {
@@ -160,7 +160,7 @@ void Ekf::controlRangeHeightFusion()
_control_status.flags.rng_hgt = true;
if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
resetHaglRng(aid_src);
resetTerrainToRng(aid_src);
}
}
}
@@ -193,7 +193,7 @@ void Ekf::controlRangeHeightFusion()
stopRngTerrFusion();
} else {
resetHaglRng(aid_src);
resetTerrainToRng(aid_src);
}
}
@@ -213,7 +213,7 @@ void Ekf::controlRangeHeightFusion()
} else {
if (aid_src.innovation_rejected) {
resetHaglRng(aid_src);
resetTerrainToRng(aid_src);
}
_hagl_sensor_status.flags.range_finder = true;
@@ -230,7 +230,7 @@ void Ekf::controlRangeHeightFusion()
}
}
void Ekf::updateRangeHeight(estimator_aid_source1d_s &aid_src)
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
{
aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
aid_src.innovation = getHagl() - aid_src.observation;
@@ -266,7 +266,7 @@ float Ekf::getRngVar() const
0.f);
}
void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src)
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
{
_state.terrain = _state.pos(2) + aid_src.observation;
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
@@ -277,8 +277,6 @@ void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src)
bool Ekf::isConditionalRangeAidSuitable()
{
#if defined(CONFIG_EKF2_TERRAIN)
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
float range_hagl_max = _params.max_hagl_for_range_aid;
@@ -303,10 +301,6 @@ bool Ekf::isConditionalRangeAidSuitable()
}
return is_in_range && is_hagl_stable && is_below_max_speed;
#endif // CONFIG_EKF2_TERRAIN
return false;
}
void Ekf::stopRngHgtFusion()
src/modules/ekf2/EKF/control.cpp
+4
View File
@@ -144,6 +144,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL
//
#if defined(CONFIG_EKF2_TERRAIN)
controlTerrainFakeFusion();
#endif // CONFIG_EKF2_TERRAIN
controlZeroInnovationHeadingUpdate();
src/modules/ekf2/EKF/covariance.cpp
+5
View File
@@ -99,6 +99,11 @@ void Ekf::initialiseCovariance()
#if defined(CONFIG_EKF2_WIND)
resetWindCov();
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_TERRAIN)
// use the ground clearance value as our uncertainty
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
#endif // CONFIG_EKF2_TERRAIN
}
void Ekf::predictCovariance(const imuSample &imu_delayed)
src/modules/ekf2/EKF/ekf.cpp
+5 -9
View File
@@ -71,6 +71,11 @@ void Ekf::reset()
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel.setZero();
#endif // CONFIG_EKF2_WIND
//
#if defined(CONFIG_EKF2_TERRAIN)
// assume a ground clearance
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER)
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -160,10 +165,6 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes(imu_sample_delayed);
#if defined(CONFIG_EKF2_TERRAIN)
runTerrainEstimator(imu_sample_delayed);
#endif // CONFIG_EKF2_TERRAIN
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
return true;
@@ -199,11 +200,6 @@ bool Ekf::initialiseFilter()
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
#if defined(CONFIG_EKF2_TERRAIN)
// Initialise the terrain state
initHagl();
#endif // CONFIG_EKF2_TERRAIN
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
src/modules/ekf2/EKF/ekf.h
+6 -11
View File
@@ -807,32 +807,27 @@ private:
bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
#if defined(CONFIG_EKF2_TERRAIN)
// terrain vertical position estimator
void initHagl();
void runTerrainEstimator(const imuSample &imu_delayed);
void initTerrain();
float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
void controlHaglFakeFusion();
void controlTerrainFakeFusion();
# if defined(CONFIG_EKF2_RANGE_FINDER)
// update the terrain vertical position estimate using a height above ground measurement from the range finder
bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
void updateRangeHeight(estimator_aid_source1d_s &aid_src);
void resetHaglRng(estimator_aid_source1d_s &aid_src);
void updateRangeHagl(estimator_aid_source1d_s &aid_src);
void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
float getRngVar() const;
# endif // CONFIG_EKF2_RANGE_FINDER
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
// update the terrain vertical position estimate using an optical flow measurement
void resetHaglFlow();
void resetTerrainToFlow();
# endif // CONFIG_EKF2_OPTICAL_FLOW
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER)
// range height
void controlRangeHeightFusion();
void controlRangeHaglFusion();
bool isConditionalRangeAidSuitable();
void stopRngHgtFusion();
void stopRngTerrFusion();
src/modules/ekf2/EKF/height_control.cpp
+1 -1
View File
@@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlRangeHeightFusion();
controlRangeHaglFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
checkHeightSensorRefFallback();
src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp
+4 -9
View File
@@ -42,7 +42,7 @@
#include <mathlib/mathlib.h>
void Ekf::initHagl()
void Ekf::initTerrain()
{
// assume a ground clearance
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
@@ -51,7 +51,7 @@ void Ekf::initHagl()
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
}
void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
void Ekf::controlTerrainFakeFusion()
{
// If we are on ground, store the local position and time to use as a reference
if (!_control_status.flags.in_air) {
@@ -62,14 +62,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
// because when using optical flow measurements, it is safer to start with a small distance to ground
// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
initHagl();
initTerrain();
}
controlHaglFakeFusion();
}
void Ekf::controlHaglFakeFusion()
{
if (!_control_status.flags.in_air
&& !_hagl_sensor_status.flags.range_finder
&& !_hagl_sensor_status.flags.flow) {
@@ -85,7 +80,7 @@ void Ekf::controlHaglFakeFusion()
#endif // CONFIG_EKF2_OPTICAL_FLOW
if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
initHagl();
initTerrain();
}
}
}
src/modules/ekf2/Kconfig
-1
View File
@@ -110,7 +110,6 @@ depends on MODULES_EKF2
bool "optical flow fusion support"
default y
select EKF2_TERRAIN
depends on EKF2_RANGE_FINDER
---help---
EKF2 optical flow fusion support.