|
|
|
@ -1219,34 +1219,17 @@ void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vect
@@ -1219,34 +1219,17 @@ void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vect
|
|
|
|
|
// peak hold detector for slower mechanisms to detect spikes
|
|
|
|
|
void AP_InertialSensor::set_accel_peak_hold(uint8_t instance, const Vector3f &accel) |
|
|
|
|
{ |
|
|
|
|
uint32_t now = AP_HAL::millis(); |
|
|
|
|
|
|
|
|
|
// positive x,y,z peak hold detector
|
|
|
|
|
if (accel.x > _accel_peak_hold_pos[instance].x || _accel_peak_hold_pos_age[instance].x <= now) { |
|
|
|
|
_accel_peak_hold_pos[instance].x = accel.x; |
|
|
|
|
_accel_peak_hold_pos_age[instance].x = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
} |
|
|
|
|
if (accel.y > _accel_peak_hold_pos[instance].y || _accel_peak_hold_pos_age[instance].y <= now) { |
|
|
|
|
_accel_peak_hold_pos[instance].y = accel.y; |
|
|
|
|
_accel_peak_hold_pos_age[instance].y = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
} |
|
|
|
|
if (accel.z > _accel_peak_hold_pos[instance].z || _accel_peak_hold_pos_age[instance].z <= now) { |
|
|
|
|
_accel_peak_hold_pos[instance].z = accel.z; |
|
|
|
|
_accel_peak_hold_pos_age[instance].z = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
if (instance != _primary_accel) { |
|
|
|
|
// we only record for primary accel
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
uint32_t now = AP_HAL::millis(); |
|
|
|
|
|
|
|
|
|
// negative x,y,z peak(min) hold detector
|
|
|
|
|
if (accel.x < _accel_peak_hold_neg[instance].x || _accel_peak_hold_neg_age[instance].x <= now) { |
|
|
|
|
_accel_peak_hold_neg[instance].x = accel.x; |
|
|
|
|
_accel_peak_hold_neg_age[instance].x = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
} |
|
|
|
|
if (accel.y < _accel_peak_hold_neg[instance].y || _accel_peak_hold_neg_age[instance].y <= now) { |
|
|
|
|
_accel_peak_hold_neg[instance].y = accel.y; |
|
|
|
|
_accel_peak_hold_neg_age[instance].y = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
} |
|
|
|
|
if (accel.z < _accel_peak_hold_neg[instance].z || _accel_peak_hold_neg_age[instance].z <= now) { |
|
|
|
|
_accel_peak_hold_neg[instance].z = accel.z; |
|
|
|
|
_accel_peak_hold_neg_age[instance].z = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
// negative x peak(min) hold detector
|
|
|
|
|
if (accel.x < _peak_hold_state.accel_peak_hold_neg_x || |
|
|
|
|
_peak_hold_state.accel_peak_hold_neg_x_age <= now) { |
|
|
|
|
_peak_hold_state.accel_peak_hold_neg_x = accel.x; |
|
|
|
|
_peak_hold_state.accel_peak_hold_neg_x_age = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|