dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_imu: vibration metrics use acceleration and angular velocity 

- this avoids any jitter in the integration timespan from impacting the vibration metrics
 - vehicle_imu_status vibration metrics are not consumed by anything
(yet), so changing the scaling of the metric shouldn't matter
This commit is contained in:
Daniel Agar 2021-07-22 09:44:21 -04:00
parent e01b631465
commit f2ae8ae814
2 changed files with 22 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -423,15 +423,16 @@ bool VehicleIMU::Publish()
// delta angle: apply offsets, scale, and board rotation // delta angle: apply offsets, scale, and board rotation
_gyro_calibration.SensorCorrectionsUpdate(); _gyro_calibration.SensorCorrectionsUpdate();
const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt; const float gyro_dt_inv = 1.e6f / imu.delta_angle_dt;
const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle * gyro_dt_inv)};
UpdateGyroVibrationMetrics(angular_velocity);
const Vector3f delta_angle_corrected{angular_velocity / gyro_dt_inv};
// delta velocity: apply offsets, scale, and board rotation // delta velocity: apply offsets, scale, and board rotation
_accel_calibration.SensorCorrectionsUpdate(); _accel_calibration.SensorCorrectionsUpdate();
const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt; const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
UpdateAccelVibrationMetrics(acceleration);
UpdateAccelVibrationMetrics(delta_velocity_corrected); const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
UpdateGyroVibrationMetrics(delta_angle_corrected);
// vehicle_imu_status // vehicle_imu_status
// publish before vehicle_imu so that error counts are available synchronously if needed // publish before vehicle_imu so that error counts are available synchronously if needed
@ -535,26 +536,26 @@ void VehicleIMU::UpdateIntegratorConfiguration()
} }
} }
void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity) void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration)
{ {
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)
const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev; _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric
_status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + 0.01f * delta_velocity_diff.norm(); + 0.01f * Vector3f(acceleration - _acceleration_prev).norm();
_delta_velocity_prev = delta_velocity; _acceleration_prev = acceleration;
} }
void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity)
{ {
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev)
const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev; _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric
_status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * delta_angle_diff.norm(); + 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm();
// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) // Gyro delta angle coning metric = filtered length of (angular_velocity x angular_velocity_prev)
const Vector3f coning_metric = delta_angle % _delta_angle_prev; const Vector3f coning_metric{angular_velocity % _angular_velocity_prev};
_status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm(); _status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm();
_delta_angle_prev = delta_angle; _angular_velocity_prev = angular_velocity;
} }
void VehicleIMU::PrintStatus() void VehicleIMU::PrintStatus()

8
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -81,8 +81,8 @@ private:
bool UpdateGyro(); bool UpdateGyro();
void UpdateIntegratorConfiguration(); void UpdateIntegratorConfiguration();
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
@ -125,8 +125,8 @@ private:
float _accel_temperature{0}; float _accel_temperature{0};
float _gyro_temperature{0}; float _gyro_temperature{0};
matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement matrix::Vector3f _acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics
matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
vehicle_imu_status_s _status{}; vehicle_imu_status_s _status{};