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

ekf2: remove redundant IMU vibration metrics

Browse Source 
- consume IMU vibration metrics from vehicle_imu_status
This commit is contained in:
Daniel Agar
2021-07-30 13:34:53 -04:00
committed by Lorenz Meier
parent a397004bf8
commit fb4ac0f08c
4 changed files with 55 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
-18
View File
@@ -63,7 +63,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_newest_high_rate_imu_sample = imu_sample;
// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
computeVibrationMetric(imu_sample);
_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
_imu_updated = _imu_down_sampler.update(imu_sample);
@@ -84,23 +83,6 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
}
}
void EstimatorInterface::computeVibrationMetric(const imuSample &imu)
{
// calculate a metric which indicates the amount of coning vibration
Vector3f temp = imu.delta_ang % _delta_ang_prev;
_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
// calculate a metric which indicates the amount of high frequency gyro vibration
temp = imu.delta_ang - _delta_ang_prev;
_delta_ang_prev = imu.delta_ang;
_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
// calculate a metric which indicates the amount of high frequency accelerometer vibration
temp = imu.delta_vel - _delta_vel_prev;
_delta_vel_prev = imu.delta_vel;
_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
}
bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
{
// detect if the vehicle is not moving when on ground