fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-22 01:20:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+11 -13
View File
@@ -66,23 +66,21 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
if (_control_status.flags.in_air) { const float dist_var = getRngVar();
const float dist_var = getRngVar(); _rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
_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) { if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
_rng_consistency_check.reset(); _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);
} }
_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 { } else {
// If we are supposed to be using range finder data but have bad range measurements // 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 // and are on the ground, then synthesise a measurement at the expected on ground value
src/modules/ekf2/test/test_EKF_flow.cpp
+1 -1
View File
@@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
_ekf_wrapper.enableFlowFusion(); _ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow(); _sensor_simulator.startFlow();
_sensor_simulator.startRangeFinder(); _sensor_simulator.startRangeFinder();
_sensor_simulator.runSeconds(1.0); _sensor_simulator.runSeconds(2.0);
// THEN: estimated velocity should match simulated velocity // THEN: estimated velocity should match simulated velocity
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
src/modules/ekf2/test/test_EKF_height_fusion.cpp
+1 -1
View File
@@ -250,7 +250,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
_sensor_simulator.stopGps(); _sensor_simulator.stopGps();
_sensor_simulator.runSeconds(10); _sensor_simulator.runSeconds(14);
EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);
_sensor_simulator.stopRangeFinder(); _sensor_simulator.stopRangeFinder();