Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add pos/neg peak detector 

new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout:
    Vector3f get_accel_peak_hold_pos()
    Vector3f get_accel_peak_hold_neg()

This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
This commit is contained in:
Tom Pittenger 2015-12-29 09:18:07 -08:00 committed by Andrew Tridgell
parent b052959c61
commit 3aaf2b1d2b
3 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1216,6 +1216,40 @@ 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;
}
// 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;
}
}
// retrieve latest calculated vibration levels
Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
{

14
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -9,6 +9,7 @@
#define AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS (15.5f*GRAVITY_MSS) // accelerometer values over 15.5G are recorded as a clipping error
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz
#define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout
/**
maximum number of INS instances available on this platform. If more
@ -223,6 +224,13 @@ public:
void detect_backends(void);
// 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;
//Returns accel calibrator interface object pointer
AP_AccelCal* get_acal() const { return _acal; }
@ -376,6 +384,12 @@ 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];
// threshold for detecting stillness
AP_Float _still_threshold;

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -174,6 +174,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_imu._accel_filter[instance].reset();
}
_imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = true;
DataFlash_Class *dataflash = get_dataflash();