diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index bd4d2783a6..f494f71f89 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -496,7 +496,12 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact // run the calibration routine for (uint8_t k=0; kprintf_P(PSTR("Insufficient accel range")); + continue; + } + + bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation); interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"), (unsigned)k, @@ -872,13 +877,62 @@ AP_InertialSensor::_init_gyro() } #if !defined( __AVR_ATmega1280__ ) + +/* + check that the samples used for accel calibration have a sufficient + range on each axis. The sphere fit in _calibrate_accel() can produce + bad offsets and scaling factors if the range of input data is + insufficient. + + We rotate each sample in the check to body frame to cope with 45 + board orientations which could result in smaller ranges. The sample + inputs are in sensor frame + */ +bool AP_InertialSensor::_check_sample_range(const Vector3f accel_sample[6], enum Rotation rotation, + AP_InertialSensor_UserInteract* interact) +{ + // we want at least 12 m/s/s range on all axes. This should be + // very easy to achieve, and guarantees the accels have been + // exposed to a good range of data + const float min_range = 12.0f; + + Vector3f min_sample, max_sample; + + // start with first sample + min_sample = accel_sample[0]; + min_sample.rotate(rotation); + max_sample = min_sample; + + for (uint8_t s=1; s<6; s++) { + Vector3f sample = accel_sample[s]; + sample.rotate(rotation); + for (uint8_t i=0; i<3; i++) { + if (sample[i] < min_sample[i]) { + min_sample[i] = sample[i]; + } + if (sample[i] > max_sample[i]) { + max_sample[i] = sample[i]; + } + } + } + Vector3f range = max_sample - min_sample; + interact->printf_P(PSTR("AccelRange: %.1f %.1f %.1f"), + range.x, range.y, range.z); + bool ok = (range.x >= min_range && + range.y >= min_range && + range.z >= min_range); + return ok; +} + + // _calibrate_model - perform low level accel calibration // accel_sample are accelerometer samples collected in 6 different positions // accel_offsets are output from the calibration routine // accel_scale are output from the calibration routine // returns true if successful -bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], - Vector3f& accel_offsets, Vector3f& accel_scale ) +bool AP_InertialSensor::_calibrate_accel(const Vector3f accel_sample[6], + Vector3f& accel_offsets, Vector3f& accel_scale, + enum Rotation rotation) { int16_t i; int16_t num_iterations = 0; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 8313740f4a..033efc17ac 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -228,7 +228,9 @@ private: // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // _calibrate_accel - perform low level accel calibration - bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); + bool _calibrate_accel(const Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale, enum Rotation r); + 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]); void _calibrate_reset_matrices(float dS[6], float JS[6][6]); void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);