fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 23:30:35 +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/EKF2.cpp
+43 -1
View File
@@ -352,6 +352,8 @@ void EKF2::Run()
_ekf.resetImuBias();
_imu_calibration_count = imu.calibration_count;
SelectImuStatus();
}
} else {
@@ -384,11 +386,13 @@ void EKF2::Run()
if (_device_id_accel != sensor_selection.accel_device_id) {
_ekf.resetAccelBias();
_device_id_accel = sensor_selection.accel_device_id;
SelectImuStatus();
}
if (_device_id_gyro != sensor_selection.gyro_device_id) {
_ekf.resetGyroBias();
_device_id_gyro = sensor_selection.gyro_device_id;
SelectImuStatus();
}
}
}
@@ -397,6 +401,8 @@ void EKF2::Run()
if (imu_updated) {
const hrt_abstime now = imu_sample_new.time_us;
UpdateImuStatus();
// push imu data into estimator
_ekf.setIMUData(imu_sample_new);
PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
@@ -1296,6 +1302,42 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
void EKF2::SelectImuStatus()
{
for (uint8_t imu_instance = 0; imu_instance < MAX_NUM_IMUS; imu_instance++) {
uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_instance};
vehicle_imu_status_s imu_status{};
imu_status_sub.copy(&imu_status);
if (imu_status.accel_device_id == _device_id_accel) {
_vehicle_imu_status_sub.ChangeInstance(imu_instance);
return;
}
}
PX4_WARN("%d - IMU status not found for accel %d, gyro %d", _instance, _device_id_accel, _device_id_gyro);
}
void EKF2::UpdateImuStatus()
{
vehicle_imu_status_s imu_status;
if (_vehicle_imu_status_sub.update(&imu_status)) {
if (imu_status.accel_device_id != _device_id_accel) {
SelectImuStatus();
return;
}
// accel -> delta velocity
_ekf.setDeltaVelocityHighFrequencyVibrationMetric(imu_status.accel_vibration_metric * _ekf.get_dt_imu_avg());
// gyro -> delta angle
_ekf.setDeltaAngleHighFrequencyVibrationMetric(imu_status.gyro_vibration_metric * _ekf.get_dt_imu_avg());
_ekf.setDeltaAngleConingMetric(imu_status.gyro_coning_vibration * _ekf.get_dt_imu_avg());
}
}
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample
@@ -1810,7 +1852,7 @@ int EKF2::task_spawn(int argc, char *argv[])
// allocate EKF2 instances until all found or arming
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
bool ekf2_instance_created[4][4] {}; // IMUs * mags
bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
while ((multi_instances_allocated < multi_instances)
&& (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)