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

vehicleIMU: compute and log accel variance

This commit is contained in:
bresch 2021-04-22 15:39:42 +02:00 committed by Daniel Agar
parent f02786d112
commit bf2fb70d67
3 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_imu_status.msg
View File

@ -20,6 +20,7 @@ float32 gyro_coning_vibration # Level of coning vibration in the IMU delta ang
float32[3] mean_accel # average accelerometer readings since last publication
float32[3] mean_gyro # average gyroscope readings since last publication
float32[3] var_accel # average accelerometer variance since last publication
float32 temperature_accel
float32 temperature_gyro

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

@ -491,6 +491,7 @@ bool VehicleIMU::Publish()
const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
UpdateAccelVibrationMetrics(acceleration);
UpdateAccelSquaredErrorSum(acceleration);
const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
// vehicle_imu_status
@ -507,6 +508,12 @@ bool VehicleIMU::Publish()
_status.temperature_accel = _accel_temperature / _accel_sum_count;
_accel_sum.zero();
_accel_temperature = 0;
// variance accel
const Vector3f variance_accel{_accel_squared_error_sum / _accel_sum_count};
variance_accel.copyTo(_status.var_accel);
_accel_squared_error_sum.zero();
_accel_sum_count = 0;
// mean gyro
@ -595,6 +602,13 @@ void VehicleIMU::UpdateIntegratorConfiguration()
}
}
void VehicleIMU::UpdateAccelSquaredErrorSum(const Vector3f &acceleration)
{
// Compute the squared error using the average from last publication for efficiency purposes
const Vector3f error{acceleration - Vector3f(_status.mean_accel)};
_accel_squared_error_sum += error.emult(error);
}
void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration)
{
// Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)

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

@ -83,6 +83,7 @@ private:
void UpdateIntegratorConfiguration();
void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
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)};
@ -118,6 +119,7 @@ private:
unsigned _accel_last_generation{0};
unsigned _gyro_last_generation{0};
matrix::Vector3f _accel_squared_error_sum{};
matrix::Vector3f _accel_sum{};
matrix::Vector3f _gyro_sum{};
int _accel_sum_count{0};