diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 702d9cd0b0..b598799f45 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -70,6 +70,11 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f _imu._accel_healthy[instance] = true; } +void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, + const Vector3f &accel) +{ +} + void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance, float max_offset) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 3219dfdf1c..60eb49f695 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -88,6 +88,12 @@ protected: // rotate accel vector, scale, offset and publish void _publish_accel(uint8_t instance, const Vector3f &accel); + // this should be called every time a new accel raw sample is available - + // be it published or not + // the sample is raw in the sense that it's not filtered yet, but it must + // be rotated and corrected (_rotate_and_correct_accel) + void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel); + // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index 4dbe25995b..fbebe8f05e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -225,6 +225,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) // Adjust for chip scaling to get m/s/s accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; _rotate_and_correct_accel(_accel_instance, accel); + _notify_new_accel_raw_sample(_accel_instance, accel); _accel_filtered = _accel_filter.apply(accel); _have_accel_sample = true; _last_accel_timestamp = now; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 0a2bb8924b..e5cd33f5e1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -345,6 +345,7 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) // Adjust for chip scaling to get m/s/s accel *= ADXL345_ACCELEROMETER_SCALE_M_S; _rotate_and_correct_accel(_accel_instance, accel); + _notify_new_accel_raw_sample(_accel_instance, accel); _data[_data_idx].accel_filtered = _accel_filter.apply(accel); _have_accel_sample = true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 6d7c784e98..ac54392033 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -741,6 +741,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z); accel_data *= _accel_scale; _rotate_and_correct_accel(_accel_instance, accel_data); + _notify_new_accel_raw_sample(_accel_instance, accel_data); _accel_filtered = _accel_filter.apply(accel_data); _accel_sample_available = true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 94b94b5834..2b8b4cb4ab 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -763,6 +763,8 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) _rotate_and_correct_accel(_accel_instance, accel); _rotate_and_correct_gyro(_gyro_instance, gyro); + _notify_new_accel_raw_sample(_accel_instance, accel); + #if MPU6000_FAST_SAMPLING _accel_filtered = _accel_filter.apply(accel); _gyro_filtered = _gyro_filter.apply(gyro); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index b73ec3a6fa..6f952b6829 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -1092,6 +1092,7 @@ void AP_InertialSensor_MPU9150::_accumulate(void) accel = Vector3f(accel_x, accel_y, accel_z); accel *= MPU9150_ACCEL_SCALE_2G; _rotate_and_correct_accel(_accel_instance, accel); + _notify_new_accel_raw_sample(_accel_instance, accel); _accel_filtered = _accel_filter.apply(accel); gyro = Vector3f(gyro_x, gyro_y, gyro_z); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 613c851e3d..73558857d6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -391,6 +391,7 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() accel *= MPU9250_ACCEL_SCALE_1G; accel.rotate(_default_rotation); _rotate_and_correct_accel(_accel_instance, accel); + _notify_new_accel_raw_sample(_accel_instance, accel); gyro = Vector3f(int16_val(rx.v, 5), int16_val(rx.v, 4), diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 2f15333060..3767b52bf4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -259,6 +259,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep // apply corrections _rotate_and_correct_accel(frontend_instance, accel); + _notify_new_accel_raw_sample(frontend_instance, accel); // apply filter for control path _accel_in[i] = _accel_filter[i].apply(accel);