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

move vehicle at rest detection ekf2 -> land_detector

Browse Source 
- move vehicle at reset detection ekf2 -> land_detector
 - ekf_unit: reduce init period
   - Fake fusion is when at rest is quite strong and makes the variance reduce rapidly. Reduce the amount of time we wait before checking if the variances are still large enough.
 - ekf_unit: reduce minimum vel/pos variance required after init
   - Fake pos fusion has a low observation noise, making the vel/pos variances reduce quickly.

Co-authored-by:: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2022-02-01 17:50:19 -05:00
committed by GitHub
parent e387f302f9
commit f3e2a197ad
19 changed files with 408 additions and 441 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
-22
View File
@@ -75,9 +75,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_newest_high_rate_imu_sample = imu_sample;
// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
_imu_updated = _imu_down_sampler.update(imu_sample);
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
@@ -96,25 +93,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
}
}
bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
{
// detect if the vehicle is not moving when on ground
if (!_control_status.flags.in_air) {
if (((_vibe_metrics(1) * 4.0e4f > _params.is_moving_scaler) || (_vibe_metrics(2) * 2.1e2f > _params.is_moving_scaler))
&& ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
_time_last_move_detect_us = imu.time_us;
}
return ((imu.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
} else {
_time_last_move_detect_us = imu.time_us;
return false;
}
}
void EstimatorInterface::setMagData(const magSample &mag_sample)
{
if (!_initialised) {