fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:54:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

also run range consistency check while landed

Browse Source
This commit is contained in:
Marco Hauswirth 2025-03-07 17:22:16 +01:00
parent c98a7ef8ae
commit a97cc72791
3 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -66,23 +66,21 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
if (_control_status.flags.in_air) {
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
_rng_consistency_check.reset();
}
_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
dist_var, imu_sample.time_us);
if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
_rng_consistency_check.reset();
}
_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
dist_var, imu_sample.time_us);
} else {
// If we are supposed to be using range finder data but have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value

2
src/modules/ekf2/test/test_EKF_flow.cpp
View File

@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
_sensor_simulator.startRangeFinder();
_sensor_simulator.runSeconds(1.0);
_sensor_simulator.runSeconds(2.0);
// THEN: estimated velocity should match simulated velocity
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());

2
src/modules/ekf2/test/test_EKF_height_fusion.cpp
View File

@ -250,7 +250,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
_sensor_simulator.stopGps();
_sensor_simulator.runSeconds(10);
_sensor_simulator.runSeconds(14);
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);
_sensor_simulator.stopRangeFinder();