diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f03bcd3fa3..1dfbd65086 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -301,6 +301,8 @@ AP_InertialSensor::AP_InertialSensor() : #if INS_VIBRATION_CHECK _accel_clip_count[i] = 0; #endif + + _accel_max_abs_offsets[i] = Vector3f(3.5f, 3.5f, 3.5f); } memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid)); memset(_delta_angle_valid,0,sizeof(_delta_angle_valid)); @@ -578,7 +580,11 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact continue; } - bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation); + bool success = _calibrate_accel(samples[k], + new_offsets[k], + new_scaling[k], + _accel_max_abs_offsets[k], + saved_orientation); interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"), (unsigned)k, @@ -977,7 +983,9 @@ bool AP_InertialSensor::_check_sample_range(const Vector3f accel_sample[6], enum // accel_scale are output from the calibration routine // returns true if successful bool AP_InertialSensor::_calibrate_accel(const Vector3f accel_sample[6], - Vector3f& accel_offsets, Vector3f& accel_scale, + Vector3f& accel_offsets, + Vector3f& accel_scale, + Vector3f& max_abs_offsets, enum Rotation rotation) { int16_t i; @@ -1034,8 +1042,11 @@ bool AP_InertialSensor::_calibrate_accel(const Vector3f accel_sample[6], if( accel_scale.is_nan() || fabsf(accel_scale.x-1.0f) > 0.1f || fabsf(accel_scale.y-1.0f) > 0.1f || fabsf(accel_scale.z-1.0f) > 0.1f ) { success = false; } - // sanity check offsets (3.5 is roughly 3/10th of a G, 5.0 is roughly half a G) - if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 3.5f || fabsf(accel_offsets.y) > 3.5f || fabsf(accel_offsets.z) > 3.5f ) { + + if (accel_offsets.is_nan() || + fabsf(accel_offsets.x) > max_abs_offsets.x || + fabsf(accel_offsets.y) > max_abs_offsets.y || + fabsf(accel_offsets.z) > max_abs_offsets.z) { success = false; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index cd6ce813d7..0d3731da66 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -248,7 +248,11 @@ private: // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // _calibrate_accel - perform low level accel calibration - bool _calibrate_accel(const Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale, enum Rotation r); + bool _calibrate_accel(const Vector3f accel_sample[6], + Vector3f& accel_offsets, + Vector3f& accel_scale, + Vector3f& max_abs_offsets, + enum Rotation rotation); bool _check_sample_range(const Vector3f accel_sample[6], enum Rotation rotation, AP_InertialSensor_UserInteract* interact); void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); @@ -292,6 +296,9 @@ private: AP_Vector3f _accel_offset[INS_MAX_INSTANCES]; AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]; + // accelerometer max absolute offsets to be used for calibration + Vector3f _accel_max_abs_offsets[INS_MAX_INSTANCES]; + // temperatures for an instance if available float _temperature[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 807544b6ba..1d66890515 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -78,6 +78,12 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f } } +void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance, + const Vector3f offset) +{ + _imu._accel_max_abs_offsets[instance] = offset; +} + // set accelerometer error_count void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 4a22654009..9d1a6b2d9b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -73,6 +73,9 @@ protected: // rotate accel vector, scale, offset and publish void _publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct = true); + // set accelerometer max absolute offset for calibration + void _set_accel_max_abs_offset(uint8_t instance, const Vector3f offset); + // publish a temperature value void _publish_temperature(uint8_t instance, float temperature);