zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: remove check for vibe check 

For all supported boards we have vibration check.
master
Lucas De Marchi 9 years ago committed by Andrew Tridgell
parent
db62e55753
commit
30a50b362f
2 changed files with 0 additions and 19 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 5
      libraries/AP_InertialSensor/AP_InertialSensor.h

14
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -283,14 +283,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2], 0), AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2], 0),
#if INS_VIBRATION_CHECK
// @Param: STILL_THRESH // @Param: STILL_THRESH
// @DisplayName: Stillness threshold for detecting if we are moving // @DisplayName: Stillness threshold for detecting if we are moving
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5 // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
// @Range: 0.05 to 50 // @Range: 0.05 to 50
// @User: Advanced // @User: Advanced
AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH), AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
#endif
// @Param: GYR_CAL // @Param: GYR_CAL
// @DisplayName: Gyro Calibration scheme // @DisplayName: Gyro Calibration scheme
@ -336,19 +334,15 @@ AP_InertialSensor::AP_InertialSensor() :
_accel_error_count[i] = 0; _accel_error_count[i] = 0;
_gyro_error_count[i] = 0; _gyro_error_count[i] = 0;
_gyro_cal_ok[i] = true; _gyro_cal_ok[i] = true;
#if INS_VIBRATION_CHECK
_accel_clip_count[i] = 0; _accel_clip_count[i] = 0;
#endif
_accel_max_abs_offsets[i] = 3.5f; _accel_max_abs_offsets[i] = 3.5f;
_accel_sample_rates[i] = 0; _accel_sample_rates[i] = 0;
} }
#if INS_VIBRATION_CHECK
for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) { 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_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); _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));
} }
@ -738,7 +732,6 @@ 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
{ {
@ -747,7 +740,6 @@ uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
} }
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
@ -1547,7 +1539,6 @@ AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id)
return backend->get_auxiliary_bus(); return backend->get_auxiliary_bus();
} }
#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)
{ {
@ -1584,17 +1575,12 @@ Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
} }
return vibe; return vibe;
} }
#endif
// check for vibration movement. Return true if all axis show nearly zero movement // check for vibration movement. Return true if all axis show nearly zero movement
bool AP_InertialSensor::is_still() bool AP_InertialSensor::is_still()
{ {
#if INS_VIBRATION_CHECK
Vector3f vibe = get_vibration_levels(); Vector3f vibe = get_vibration_levels();
return (vibe.x < _still_threshold) && return (vibe.x < _still_threshold) &&
(vibe.y < _still_threshold) && (vibe.y < _still_threshold) &&
(vibe.z < _still_threshold); (vibe.z < _still_threshold);
#else
return false;
#endif
} }

5
libraries/AP_InertialSensor/AP_InertialSensor.h
Unescape View File

@ -16,7 +16,6 @@
*/ */
#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_INSTANCES 2 #define INS_VIBRATION_CHECK_INSTANCES 2
#include <stdint.h> #include <stdint.h>
@ -202,7 +201,6 @@ 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);
@ -212,7 +210,6 @@ 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
// check for vibration movement. True when all axis show nearly zero movement // check for vibration movement. True when all axis show nearly zero movement
bool is_still(); bool is_still();
@ -353,7 +350,6 @@ 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[INS_VIBRATION_CHECK_INSTANCES]; LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
@ -361,7 +357,6 @@ private:
// threshold for detecting stillness // threshold for detecting stillness
AP_Float _still_threshold; AP_Float _still_threshold;
#endif
/* /*
state for HIL support state for HIL support

Write Preview
Loading…
Cancel
Save