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

reduce tilt to 10deg, fix tilt check

Browse Source
This commit is contained in:
Jacob Dahl 2025-07-17 15:18:40 -08:00
parent ac0b769feb
commit ed4d93b9e6
2 changed files with 15 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
View File

@ -42,22 +42,19 @@
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
{
// Check if rangefinder is available/enabled
if (!_range_buffer) {
// ECL_INFO("no buff");
return;
}
// TODO: why isn't this being done anywhere?
_aid_src_rng_hgt.fused = false;
// Pop rangefinder measurement from buffer of samples into active sample
sensor::rangeSample sample = {};
if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
if (_range_sensor.timedOut(imu_sample.time_us)) {
// Disable fusion if it's currently enabled
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
stopRangeAltitudeFusion("sensor timed out");
stopRangeTerrainFusion("sensor timed out");
}
// ECL_INFO("timed out1");
stopRangeAltitudeFusion("sensor timed out");
stopRangeTerrainFusion("sensor timed out");
}
return;
}
@ -66,16 +63,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setSample(sample);
// TODO: move setting params to init function
// Set all of the parameters
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
float cosine_max_tilt = 0.866f; // 30 degrees
float cosine_max_tilt = 0.9659; // 10 degrees
_range_sensor.setCosMaxTilt(cosine_max_tilt);
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
// Update sensor to earth rotation
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
// TODO: refactor into validity_checks()
// Gate sample consumption on these checks
bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
@ -86,29 +80,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// If quality, tilt, or value are outside of bounds -- throw away measurement
if (!quality_ok || !tilt_ok || !range_ok) {
if (_range_sensor.timedOut(imu_sample.time_us)) {
// Disable fusion if it's currently enabled
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
const char* reason = "sensor data invalid";
stopRangeAltitudeFusion(reason);
stopRangeTerrainFusion(reason);
}
ECL_INFO("timed out2");
}
if (!quality_ok) {
ECL_INFO("!quality_ok");
}
if (!tilt_ok) {
ECL_INFO("!tilt_ok");
}
if (!range_ok) {
ECL_INFO("!range_ok");
} // commander takeoff
stopRangeAltitudeFusion("pre checks failed");
stopRangeTerrainFusion("pre checks failed");
return;
}
// TODO: refactor into apply_body_offset()
// Correct the range data for position offset relative to the IMU
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
@ -118,7 +94,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// Provide sample from buffer to object
_range_sensor.setSample(sample);
// TODO: refactor into consintency_filter_update() that runs consistency status
// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
@ -308,7 +283,7 @@ bool Ekf::rangeAidConditionsPassed()
_rng_aid_conditions_valid = false;
// if (!is_hagl_stable) {
// ECL_INFO("!is_hagl_stable");
// ECL_INFO("!is_hagl_stable");
// }
// if (is_horizontal_aiding_active && !is_below_max_speed) {
if (!is_below_max_speed) {
@ -367,12 +342,12 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter

8
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -132,16 +132,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const
void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
{
// start the consistency check after 1s
// if (_t_since_first_sample < _t_to_init) {
// _t_since_first_sample += dt;
// return;
// }
// TODO: magic test ratio
// if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
if (fabsf(_test_ratio) > 1.f) {
// printf("KinematicState::INCONSISTENT\n");
_t_since_first_sample = 0.f;
_state = KinematicState::INCONSISTENT;
return;