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

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -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;
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -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;