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

InertialSensor: disable vibration checks on APM2

Browse Source 
Also bug fix get_accel_clip_count's instance check
This commit is contained in:
Randy Mackay 2015-06-12 18:23:38 +09:00
parent fe1da458a7
commit 8ceccd778d
2 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -285,8 +285,10 @@ 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_floor_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ),
_accel_vibe_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_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);
@ -296,7 +298,9 @@ AP_InertialSensor::AP_InertialSensor() :
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_error_count[i] = 0; _accel_error_count[i] = 0;
_gyro_error_count[i] = 0; _gyro_error_count[i] = 0;
#if INS_VIBRATION_CHECK
_accel_clip_count[i] = 0; _accel_clip_count[i] = 0;
#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));
@ -643,14 +647,16 @@ AP_InertialSensor::init_gyro()
_save_parameters(); _save_parameters();
} }
#if INS_VIBRATION_CHECK
// accelerometer clipping reporting // accelerometer clipping reporting
uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
{ {
if (instance > get_accel_count()) { if (instance >= get_accel_count()) {
return 0; return 0;
} }
return _accel_clip_count[instance]; return _accel_clip_count[instance];
} }
#endif
// get_gyro_health_all - return true if all gyros are healthy // get_gyro_health_all - return true if all gyros are healthy
bool AP_InertialSensor::get_gyro_health_all(void) const bool AP_InertialSensor::get_gyro_health_all(void) const
@ -1330,6 +1336,7 @@ void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
} }
} }
#if INS_VIBRATION_CHECK
// calculate vibration levels and check for accelerometer clipping (called by a backends) // calculate vibration levels and check for accelerometer clipping (called by a backends)
void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt) void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)
{ {
@ -1365,3 +1372,4 @@ Vector3f AP_InertialSensor::get_vibration_levels() const
vibe.z = safe_sqrt(vibe.z); vibe.z = safe_sqrt(vibe.z);
return vibe; return vibe;
} }
#endif

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -17,9 +17,11 @@
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 #if HAL_CPU_CLASS > HAL_CPU_CLASS_16
#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
#else #else
#define INS_MAX_INSTANCES 1 #define INS_MAX_INSTANCES 1
#define INS_MAX_BACKENDS 1 #define INS_MAX_BACKENDS 1
#define INS_VIBRATION_CHECK 0
#endif #endif
@ -220,6 +222,7 @@ public:
// enable/disable raw gyro/accel logging // enable/disable raw gyro/accel logging
void set_raw_logging(bool enable) { _log_raw_data = enable; } void set_raw_logging(bool enable) { _log_raw_data = enable; }
#if INS_VIBRATION_CHECK
// calculate vibration levels and check for accelerometer clipping (called by a backends) // calculate vibration levels and check for accelerometer clipping (called by a backends)
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);
@ -228,6 +231,7 @@ public:
// 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;
#endif
private: private:
@ -336,10 +340,12 @@ private:
uint32_t _accel_error_count[INS_MAX_INSTANCES]; uint32_t _accel_error_count[INS_MAX_INSTANCES];
uint32_t _gyro_error_count[INS_MAX_INSTANCES]; uint32_t _gyro_error_count[INS_MAX_INSTANCES];
#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;
LowPassFilterVector3f _accel_vibe_filter; LowPassFilterVector3f _accel_vibe_filter;
#endif
DataFlash_Class *_dataflash; DataFlash_Class *_dataflash;
}; };