Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

InertialSensor: add vibration monitoring of 2nd IMU

Browse Source
This commit is contained in:
Randy Mackay 2015-07-30 16:58:06 +09:00
parent 85027e1997
commit affbd67c43
2 changed files with 30 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -285,10 +285,6 @@ AP_InertialSensor::AP_InertialSensor() :
_hil_mode(false), _hil_mode(false),
_calibrating(false), _calibrating(false),
_log_raw_data(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) _dataflash(NULL)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -304,6 +300,12 @@ AP_InertialSensor::AP_InertialSensor() :
_accel_max_abs_offsets[i] = 3.5f; _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_velocity_valid,0,sizeof(_delta_velocity_valid));
memset(_delta_angle_valid,0,sizeof(_delta_angle_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]++; _accel_clip_count[instance]++;
} }
// calculate vibration on primary accel only // calculate vibration levels
if (instance != _primary_accel) { if (instance < INS_VIBRATION_CHECK_INSTANCES) {
return; // filter accel at 5hz
} Vector3f accel_filt = _accel_vibe_floor_filter[instance].apply(accel, dt);
// filter accel a 5hz
Vector3f accel_filt = _accel_vibe_floor_filter.apply(accel, dt);
// calc difference from this sample and 5hz filtered value, square and filter at 2hz // calc difference from this sample and 5hz filtered value, square and filter at 2hz
Vector3f accel_diff = (accel - accel_filt); Vector3f accel_diff = (accel - accel_filt);
accel_diff.x *= accel_diff.x; accel_diff.x *= accel_diff.x;
accel_diff.y *= accel_diff.y; accel_diff.y *= accel_diff.y;
accel_diff.z *= accel_diff.z; 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 // 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.x = safe_sqrt(vibe.x);
vibe.y = safe_sqrt(vibe.y); vibe.y = safe_sqrt(vibe.y);
vibe.z = safe_sqrt(vibe.z); vibe.z = safe_sqrt(vibe.z);
}
return vibe; return vibe;
} }
#endif #endif

8
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -18,6 +18,7 @@
#define INS_MAX_INSTANCES 3 #define INS_MAX_INSTANCES 3
#define INS_MAX_BACKENDS 6 #define INS_MAX_BACKENDS 6
#define INS_VIBRATION_CHECK 1 #define INS_VIBRATION_CHECK 1
#define INS_VIBRATION_CHECK_INSTANCES 2
#else #else
#define INS_MAX_INSTANCES 1 #define INS_MAX_INSTANCES 1
#define INS_MAX_BACKENDS 1 #define INS_MAX_BACKENDS 1
@ -214,7 +215,8 @@ public:
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt); void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
// retrieve latest calculated vibration levels // 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 // retrieve and clear accelerometer clipping count
uint32_t get_accel_clip_count(uint8_t instance) const; uint32_t get_accel_clip_count(uint8_t instance) const;
@ -346,8 +348,8 @@ private:
#if INS_VIBRATION_CHECK #if INS_VIBRATION_CHECK
// vibration and clipping // vibration and clipping
uint32_t _accel_clip_count[INS_MAX_INSTANCES]; uint32_t _accel_clip_count[INS_MAX_INSTANCES];
LowPassFilterVector3f _accel_vibe_floor_filter; LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
LowPassFilterVector3f _accel_vibe_filter; LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
#endif #endif
/* /*