aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: we only need peak hold for negative X for now

Browse Source
This commit is contained in:
Andrew Tridgell 2015-12-31 14:23:04 +11:00
parent 7cde90553d
commit b2745bb545
2 changed files with 15 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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)
{
if (instance != _primary_accel) {
// we only record for primary accel
return;
}
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;
}
// 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;
}
}

15
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -226,10 +226,7 @@ public:
// accel peak hold detector
void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);
Vector3f get_accel_peak_hold_pos() const { return _accel_peak_hold_pos[_primary_accel]; }
Vector3f get_accel_peak_hold_pos(uint8_t instance) const;
Vector3f get_accel_peak_hold_neg() const { return _accel_peak_hold_neg[_primary_accel]; }
Vector3f get_accel_peak_hold_neg(uint8_t instance) const;
float get_accel_peak_hold_neg_x() const { return _peak_hold_state.accel_peak_hold_neg_x; }
//Returns accel calibrator interface object pointer
AP_AccelCal* get_acal() const { return _acal; }
@ -384,11 +381,11 @@ private:
LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
// peak hold detector
Vector3f _accel_peak_hold_pos[INS_MAX_INSTANCES];
Vector3f _accel_peak_hold_neg[INS_MAX_INSTANCES];
Vector3ul _accel_peak_hold_pos_age[INS_MAX_INSTANCES];
Vector3ul _accel_peak_hold_neg_age[INS_MAX_INSTANCES];
// peak hold detector state for primary accel
struct PeakHoldState {
float accel_peak_hold_neg_x;
uint32_t accel_peak_hold_neg_x_age;
} _peak_hold_state;
// threshold for detecting stillness
AP_Float _still_threshold;