fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:23: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-08-08 06:26:03 -07: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
src/modules/ekf2/EKF/estimator_interface.h
+3 -2
View File
@@ -81,6 +81,9 @@ public:
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
*/
const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
void setDeltaAngleConingMetric(float delta_angle_coning_metric) { _vibe_metrics(0) = delta_angle_coning_metric; }
void setDeltaAngleHighFrequencyVibrationMetric(float delta_angle_vibration_metric) { _vibe_metrics(1) = delta_angle_vibration_metric; }
void setDeltaVelocityHighFrequencyVibrationMetric(float delta_velocity_vibration_metric) { _vibe_metrics(2) = delta_velocity_vibration_metric; }
void setMagData(const magSample &mag_sample);
@@ -421,8 +424,6 @@ private:
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
// IMU vibration and movement monitoring
Vector3f _delta_ang_prev{}; // delta angle from the previous IMU measurement
Vector3f _delta_vel_prev{}; // delta velocity from the previous IMU measurement
Vector3f _vibe_metrics{}; // IMU vibration metrics
// [0] Level of coning vibration in the IMU delta angles (rad^2)
// [1] high frequency vibration level in the IMU delta angle data (rad)