fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

reset terrain w flow based on current horizontal velocity

Browse Source
This commit is contained in:
Marco Hauswirth 2025-08-08 19:34:19 +02:00 committed by Marco Hauswirth
parent 547582b16b
commit 073013cf85
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -125,11 +125,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
if (_flow_counter == 0) {
_flow_vel_body_lpf.reset(_flow_vel_body);
_flow_rate_compensated_lpf.reset(_flow_rate_compensated);
_flow_counter = 1;
} else {
_flow_vel_body_lpf.update(_flow_vel_body);
_flow_rate_compensated_lpf.update(_flow_rate_compensated);
_flow_counter++;
}
@ -240,8 +242,23 @@ void Ekf::resetTerrainToFlow()
{
ECL_INFO("reset hagl to flow");
// TODO: use the flow data
const float new_terrain = -_gpos.altitude() + _params.ekf2_min_rng;
float new_terrain = -_gpos.altitude() + _params.ekf2_min_rng;
if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
// ||vel_NE|| = ||( R * flow_body * range).xy()||
// range = ||vel_NE|| / ||P * R * flow_body||
constexpr float kProjXY[2][3] = {{1.f, 0.f, 0.f}, {0.f, 1.f, 0.f}};
const matrix::Matrix<float, 2, 3> proj(kProjXY);
const Vector3f flow_body(-_flow_rate_compensated_lpf.getState()(1), _flow_rate_compensated_lpf.getState()(0), 0.f);
const float denom = Vector2f(proj * _R_to_earth * flow_body).norm();
if (denom > 1e-6f) {
const float range = _state.vel.xy().norm() / denom;
new_terrain = -_gpos.altitude() + max(range, _params.ekf2_min_rng);
}
}
const float delta_terrain = new_terrain - _state.terrain;
_state.terrain = new_terrain;
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);

1
src/modules/ekf2/EKF/ekf.h
View File

@ -529,6 +529,7 @@ private:
uint32_t _flow_counter{0}; ///< number of flow samples read for initialization
Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
AlphaFilter<Vector2f> _flow_rate_compensated_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_AIRSPEED)