AP_AccelCal: basic sanity check on fit parameters

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 {

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;