mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
InertialSensor: add vibration monitoring of 2nd IMU
This commit is contained in:
parent
85027e1997
commit
affbd67c43
@ -285,10 +285,6 @@ AP_InertialSensor::AP_InertialSensor() :
|
||||
_hil_mode(false),
|
||||
_calibrating(false),
|
||||
_log_raw_data(false),
|
||||
#if INS_VIBRATION_CHECK
|
||||
_accel_vibe_floor_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ),
|
||||
_accel_vibe_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ),
|
||||
#endif
|
||||
_dataflash(NULL)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
@ -304,6 +300,12 @@ AP_InertialSensor::AP_InertialSensor() :
|
||||
|
||||
_accel_max_abs_offsets[i] = 3.5f;
|
||||
}
|
||||
#if INS_VIBRATION_CHECK
|
||||
for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
|
||||
_accel_vibe_floor_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
|
||||
_accel_vibe_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ);
|
||||
}
|
||||
#endif
|
||||
memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
|
||||
memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
|
||||
}
|
||||
@ -1426,29 +1428,30 @@ void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vect
|
||||
_accel_clip_count[instance]++;
|
||||
}
|
||||
|
||||
// calculate vibration on primary accel only
|
||||
if (instance != _primary_accel) {
|
||||
return;
|
||||
}
|
||||
|
||||
// filter accel a 5hz
|
||||
Vector3f accel_filt = _accel_vibe_floor_filter.apply(accel, dt);
|
||||
// calculate vibration levels
|
||||
if (instance < INS_VIBRATION_CHECK_INSTANCES) {
|
||||
// filter accel at 5hz
|
||||
Vector3f accel_filt = _accel_vibe_floor_filter[instance].apply(accel, dt);
|
||||
|
||||
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
|
||||
Vector3f accel_diff = (accel - accel_filt);
|
||||
accel_diff.x *= accel_diff.x;
|
||||
accel_diff.y *= accel_diff.y;
|
||||
accel_diff.z *= accel_diff.z;
|
||||
_accel_vibe_filter.apply(accel_diff, dt);
|
||||
_accel_vibe_filter[instance].apply(accel_diff, dt);
|
||||
}
|
||||
}
|
||||
|
||||
// retrieve latest calculated vibration levels
|
||||
Vector3f AP_InertialSensor::get_vibration_levels() const
|
||||
Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
|
||||
{
|
||||
Vector3f vibe = _accel_vibe_filter.get();
|
||||
Vector3f vibe;
|
||||
if (instance < INS_VIBRATION_CHECK_INSTANCES) {
|
||||
vibe = _accel_vibe_filter[instance].get();
|
||||
vibe.x = safe_sqrt(vibe.x);
|
||||
vibe.y = safe_sqrt(vibe.y);
|
||||
vibe.z = safe_sqrt(vibe.z);
|
||||
}
|
||||
return vibe;
|
||||
}
|
||||
#endif
|
||||
|
@ -18,6 +18,7 @@
|
||||
#define INS_MAX_INSTANCES 3
|
||||
#define INS_MAX_BACKENDS 6
|
||||
#define INS_VIBRATION_CHECK 1
|
||||
#define INS_VIBRATION_CHECK_INSTANCES 2
|
||||
#else
|
||||
#define INS_MAX_INSTANCES 1
|
||||
#define INS_MAX_BACKENDS 1
|
||||
@ -214,7 +215,8 @@ public:
|
||||
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
|
||||
|
||||
// retrieve latest calculated vibration levels
|
||||
Vector3f get_vibration_levels() const;
|
||||
Vector3f get_vibration_levels() const { return get_vibration_levels(_primary_accel); }
|
||||
Vector3f get_vibration_levels(uint8_t instance) const;
|
||||
|
||||
// retrieve and clear accelerometer clipping count
|
||||
uint32_t get_accel_clip_count(uint8_t instance) const;
|
||||
@ -346,8 +348,8 @@ private:
|
||||
#if INS_VIBRATION_CHECK
|
||||
// vibration and clipping
|
||||
uint32_t _accel_clip_count[INS_MAX_INSTANCES];
|
||||
LowPassFilterVector3f _accel_vibe_floor_filter;
|
||||
LowPassFilterVector3f _accel_vibe_filter;
|
||||
LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
|
||||
LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
|
||||
#endif
|
||||
|
||||
/*
|
||||
|
Loading…
Reference in New Issue
Block a user