diff --git a/libraries/AP_AccelCal/AccelCalibrator.cpp b/libraries/AP_AccelCal/AccelCalibrator.cpp index d424aab219..309dc1c91e 100644 --- a/libraries/AP_AccelCal/AccelCalibrator.cpp +++ b/libraries/AP_AccelCal/AccelCalibrator.cpp @@ -135,7 +135,7 @@ void AccelCalibrator::new_sample(const Vector3f delta_velocity, float dt) { if (_samples_collected >= _conf_num_samples) { run_fit(MAX_ITERATIONS, _fitness); - if (_fitness < _conf_tolerance) { + if (_fitness < _conf_tolerance && accept_result()) { set_status(ACCEL_CAL_SUCCESS); } else { set_status(ACCEL_CAL_FAILED); @@ -146,6 +146,20 @@ void AccelCalibrator::new_sample(const Vector3f delta_velocity, float dt) { } } +// determines if the result is acceptable +bool AccelCalibrator::accept_result() const { + if (fabsf(_param_struct.offset.x) > GRAVITY_MSS || + fabsf(_param_struct.offset.y) > GRAVITY_MSS || + fabsf(_param_struct.offset.z) > GRAVITY_MSS || + _param_struct.diag.x < 0.8f || _param_struct.diag.x > 1.2f || + _param_struct.diag.y < 0.8f || _param_struct.diag.y > 1.2f || + _param_struct.diag.z < 0.8f || _param_struct.diag.z > 1.2f) { + return false; + } else { + return true; + } +} + // interface for LSq estimator to read sample buffer sent after conversion from delta velocity // to averaged acc over time bool AccelCalibrator::get_sample(uint8_t i, Vector3f& s) const { diff --git a/libraries/AP_AccelCal/AccelCalibrator.h b/libraries/AP_AccelCal/AccelCalibrator.h index 4f031758f8..7e0e9ce29e 100644 --- a/libraries/AP_AccelCal/AccelCalibrator.h +++ b/libraries/AP_AccelCal/AccelCalibrator.h @@ -120,6 +120,9 @@ private: // sets status of calibrator and takes appropriate actions void set_status(enum accel_cal_status_t); + // determines if the result is acceptable + bool accept_result() const; + // returns number of paramters are required for selected Fit type uint8_t get_num_params() const;