diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 38c21435a8..dc89b7bea5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -296,6 +296,19 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
 
+    // @Param: TRIM_OPTION
+    // @DisplayName: ACC TRIM Option
+    // @Description: Choose what is the truth for accel trim calculation
+    // @User: Advanced
+    // @Values: 0:accel cal Level is Truth, 1:body aligned accel is truth
+    AP_GROUPINFO("TRIM_OPTION", 25, AP_InertialSensor, _trim_option, 0),
+
+    // @Param: ACC_BODY_ALIGNED
+    // @DisplayName: ACC body aligned
+    // @Description: The body aligned accel to be used for trim calculation
+    // @User: Advanced
+    // @Value: Accelerometer ID    
+    AP_GROUPINFO("ACC_BODYFIX", 26, AP_InertialSensor, _acc_body_aligned, 2),
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -551,191 +564,6 @@ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& tri
     return true;
 }
 
-// calibrate_accel - perform accelerometer calibration including providing user
-// instructions and feedback Gauss-Newton accel calibration routines borrowed
-// from Rolfe Schmidt blog post describing the method:
-// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
-// original sketch available at
-// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
-bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
-                                        float &trim_roll,
-                                        float &trim_pitch)
-{
-    uint8_t num_accels = MIN(get_accel_count(), INS_MAX_INSTANCES);
-    Vector3f samples[INS_MAX_INSTANCES][6];
-    Vector3f new_offsets[INS_MAX_INSTANCES];
-    Vector3f new_scaling[INS_MAX_INSTANCES];
-    Vector3f orig_offset[INS_MAX_INSTANCES];
-    Vector3f orig_scale[INS_MAX_INSTANCES];
-    uint8_t num_ok = 0;
-
-    // exit immediately if calibration is already in progress
-    if (_calibrating) {
-        return false;
-    }
-
-    _calibrating = true;
-
-    /*
-      we do the accel calibration with no board rotation. This avoids
-      having to rotate readings during the calibration
-    */
-    enum Rotation saved_orientation = _board_orientation;
-    _board_orientation = ROTATION_NONE;
-
-    for (uint8_t k=0; k<num_accels; k++) {
-        // backup original offsets and scaling
-        orig_offset[k] = _accel_offset[k].get();
-        orig_scale[k]  = _accel_scale[k].get();
-
-        // clear accelerometer offsets and scaling
-        _accel_offset[k] = Vector3f(0,0,0);
-        _accel_scale[k] = Vector3f(1,1,1);
-    }
-
-    memset(samples, 0, sizeof(samples));
-
-    // capture data from 6 positions
-    for (uint8_t i=0; i<6; i++) {
-        const char *msg;
-
-        // display message to user
-        switch ( i ) {
-            case 0:
-                msg = "level";
-                break;
-            case 1:
-                msg = "on its LEFT side";
-                break;
-            case 2:
-                msg = "on its RIGHT side";
-                break;
-            case 3:
-                msg = "nose DOWN";
-                break;
-            case 4:
-                msg = "nose UP";
-                break;
-            default:    // default added to avoid compiler warning
-            case 5:
-                msg = "on its BACK";
-                break;
-        }
-        interact->printf(
-                "Place vehicle %s and press any key.\n", msg);
-
-        // wait for user input
-        if (!interact->blocking_read()) {
-            //No need to use interact->printf for an error, blocking_read does this when it fails
-            goto failed;
-        }
-
-        const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
-
-        // wait 100ms for ins filter to rise
-        for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
-            wait_for_sample();
-            update();
-            hal.scheduler->delay(update_dt_milliseconds);
-        }
-
-        uint32_t num_samples = 0;
-        while (num_samples < 400/update_dt_milliseconds) {
-            wait_for_sample();
-            // read samples from ins
-            update();
-            // capture sample
-            for (uint8_t k=0; k<num_accels; k++) {
-                Vector3f samp;
-                if(get_delta_velocity(k,samp) && _delta_velocity_dt[k] > 0) {
-                    samp /= _delta_velocity_dt[k];
-                } else {
-                    samp = get_accel(k);
-                }
-                samples[k][i] += samp;
-                if (!get_accel_health(k)) {
-                    interact->printf("accel[%u] not healthy", (unsigned)k);
-                    goto failed;
-                }
-            }
-            hal.scheduler->delay(update_dt_milliseconds);
-            num_samples++;
-        }
-        for (uint8_t k=0; k<num_accels; k++) {
-            samples[k][i] /= num_samples;
-        }
-    }
-
-    // run the calibration routine
-    for (uint8_t k=0; k<num_accels; k++) {
-        if (!_check_sample_range(samples[k], saved_orientation, interact)) {
-            interact->printf("Insufficient accel range");
-            continue;
-        }
-
-        bool success = _calibrate_accel(samples[k],
-                                        new_offsets[k],
-                                        new_scaling[k],
-                                        _accel_max_abs_offsets[k],
-                                        saved_orientation);
-
-        interact->printf("Offsets[%u]: %.2f %.2f %.2f\n",
-                            (unsigned)k,
-                           (double)new_offsets[k].x, (double)new_offsets[k].y, (double)new_offsets[k].z);
-        interact->printf("Scaling[%u]: %.2f %.2f %.2f\n",
-                           (unsigned)k,
-                           (double)new_scaling[k].x, (double)new_scaling[k].y, (double)new_scaling[k].z);
-        if (success) num_ok++;
-    }
-
-    if (num_ok == num_accels) {
-        interact->printf("Calibration successful\n");
-
-        for (uint8_t k=0; k<num_accels; k++) {
-            // set and save calibration
-            _accel_offset[k].set(new_offsets[k]);
-            _accel_scale[k].set(new_scaling[k]);
-        }
-        for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
-            // clear unused accelerometer's scaling and offsets
-            _accel_offset[k] = Vector3f(0,0,0);
-            _accel_scale[k] = Vector3f(0,0,0);
-        }
-        _save_parameters();
-
-        /*
-          calculate the trims as well from primary accels 
-          We use the original board rotation for this sample
-        */
-        Vector3f level_sample = samples[0][0];
-        level_sample.x *= new_scaling[0].x;
-        level_sample.y *= new_scaling[0].y;
-        level_sample.z *= new_scaling[0].z;
-        level_sample -= new_offsets[0];
-        level_sample.rotate(saved_orientation);
-
-        if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
-            goto failed;
-        }
-
-        _board_orientation = saved_orientation;
-
-        _calibrating = false;
-        return true;
-    }
-
-failed:
-    interact->printf("Calibration FAILED\n");
-    // restore original scaling and offsets
-    for (uint8_t k=0; k<num_accels; k++) {
-        _accel_offset[k].set(orig_offset[k]);
-        _accel_scale[k].set(orig_scale[k]);
-    }
-    _board_orientation = saved_orientation;
-    _calibrating = false;
-    return false;
-}
-
 void
 AP_InertialSensor::init_gyro()
 {
@@ -1042,205 +870,6 @@ AP_InertialSensor::_init_gyro()
     AP_Notify::flags.initialising = false;
 }
 
-/*
-  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("AccelRange: %.1f %.1f %.1f",
-            (double)range.x, (double)range.y, (double)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(const Vector3f accel_sample[6],
-                                         Vector3f& accel_offsets,
-                                         Vector3f& accel_scale,
-                                         float max_abs_offsets,
-                                         enum Rotation rotation)
-{
-    int16_t i;
-    int16_t num_iterations = 0;
-    float eps = 0.000000001f;
-    float change = 100.0f;
-    float data[3];
-    float beta[6];
-    float delta[6];
-    float ds[6];
-    float JS[6][6];
-    bool success = true;
-
-    // reset
-    beta[0] = beta[1] = beta[2] = 0;
-    beta[3] = beta[4] = beta[5] = 1.0f/GRAVITY_MSS;
-    
-    while( num_iterations < 20 && change > eps ) {
-        num_iterations++;
-
-        _calibrate_reset_matrices(ds, JS);
-
-        for( i=0; i<6; i++ ) {
-            data[0] = accel_sample[i].x;
-            data[1] = accel_sample[i].y;
-            data[2] = accel_sample[i].z;
-            _calibrate_update_matrices(ds, JS, beta, data);
-        }
-
-        _calibrate_find_delta(ds, JS, delta);
-
-        change =    delta[0]*delta[0] +
-                    delta[0]*delta[0] +
-                    delta[1]*delta[1] +
-                    delta[2]*delta[2] +
-                    delta[3]*delta[3] / (beta[3]*beta[3]) +
-                    delta[4]*delta[4] / (beta[4]*beta[4]) +
-                    delta[5]*delta[5] / (beta[5]*beta[5]);
-
-        for( i=0; i<6; i++ ) {
-            beta[i] -= delta[i];
-        }
-    }
-
-    // copy results out
-    accel_scale.x = beta[3] * GRAVITY_MSS;
-    accel_scale.y = beta[4] * GRAVITY_MSS;
-    accel_scale.z = beta[5] * GRAVITY_MSS;
-    accel_offsets.x = beta[0] * accel_scale.x;
-    accel_offsets.y = beta[1] * accel_scale.y;
-    accel_offsets.z = beta[2] * accel_scale.z;
-
-    // sanity check scale
-    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;
-    }
-
-    if (accel_offsets.is_nan() ||
-        fabsf(accel_offsets.x) > max_abs_offsets ||
-        fabsf(accel_offsets.y) > max_abs_offsets ||
-        fabsf(accel_offsets.z) > max_abs_offsets) {
-        success = false;
-    }
-
-    // return success or failure
-    return success;
-}
-
-void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6],
-                                    float beta[6], float data[3])
-{
-    int16_t j, k;
-    float dx, b;
-    float residual = 1.0f;
-    float jacobian[6];
-    
-    for( j=0; j<3; j++ ) {
-        b = beta[3+j];
-        dx = (float)data[j] - beta[j];
-        residual -= b*b*dx*dx;
-        jacobian[j] = 2.0f*b*b*dx;
-        jacobian[3+j] = -2.0f*b*dx*dx;
-    }
-    
-    for( j=0; j<6; j++ ) {
-        dS[j] += jacobian[j]*residual;
-        for( k=0; k<6; k++ ) {
-            JS[j][k] += jacobian[j]*jacobian[k];
-        }
-    }
-}
-
-
-// _calibrate_reset_matrices - clears matrices
-void AP_InertialSensor::_calibrate_reset_matrices(float dS[6], float JS[6][6])
-{
-    int16_t j,k;
-    for( j=0; j<6; j++ ) {
-        dS[j] = 0.0f;
-        for( k=0; k<6; k++ ) {
-            JS[j][k] = 0.0f;
-        }
-    }
-}
-
-void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float delta[6])
-{
-    //Solve 6-d matrix equation JS*x = dS
-    //first put in upper triangular form
-    int16_t i,j,k;
-    float mu;
-
-    //make upper triangular
-    for( i=0; i<6; i++ ) {
-        //eliminate all nonzero entries below JS[i][i]
-        for( j=i+1; j<6; j++ ) {
-            mu = JS[i][j]/JS[i][i];
-            if( !is_zero(mu) ) {
-                dS[j] -= mu*dS[i];
-                for( k=j; k<6; k++ ) {
-                    JS[k][j] -= mu*JS[k][i];
-                }
-            }
-        }
-    }
-
-    //back-substitute
-    for( i=5; i>=0; i-- ) {
-        dS[i] /= JS[i][i];
-        JS[i][i] = 1.0f;
-        
-        for( j=0; j<i; j++ ) {
-            mu = JS[i][j];
-            dS[j] -= mu*dS[i];
-            JS[i][j] = 0.0f;
-        }
-    }
-
-    for( i=0; i<6; i++ ) {
-        delta[i] = dS[i];
-    }
-}
-
 // save parameters to eeprom
 void AP_InertialSensor::_save_parameters()
 {
@@ -1607,3 +1236,141 @@ bool AP_InertialSensor::is_still()
            (vibe.y < _still_threshold) &&
            (vibe.z < _still_threshold);
 }
+
+// initialise and register accel calibrator
+// called during the startup of accel cal
+void AP_InertialSensor::acal_init()
+{
+    _acal = new AP_AccelCal;
+    _acal->register_client(this);
+    _accel_calibrator = new AccelCalibrator[INS_MAX_INSTANCES];
+}
+
+// update accel calibrator
+void AP_InertialSensor::acal_update() 
+{
+    if(_acal == NULL) {
+        return;
+    }
+
+    _acal->update();
+
+    if (hal.util->get_soft_armed() && _acal->get_status() != ACCEL_CAL_NOT_STARTED) {
+        _acal->cancel();
+    }
+}
+
+/*
+    set and save accelerometer bias along with trim calculation
+*/
+void AP_InertialSensor::_acal_save_calibrations()
+{
+    Vector3f bias, gain;
+    for (uint8_t i=0; i<_accel_count; i++) {
+        if (_accel_calibrator[i].get_status() == ACCEL_CAL_SUCCESS) {
+            _accel_calibrator[i].get_calibration(bias, gain);
+            _accel_offset[i].set_and_save(bias);
+            _accel_scale[i].set_and_save(gain);
+        } else {
+            _accel_offset[i].set_and_save(Vector3f(0,0,0));
+            _accel_scale[i].set_and_save(Vector3f(0,0,0));
+        }
+    }
+
+    Vector3f aligned_sample;
+    Vector3f misaligned_sample;
+    switch(_trim_option) {
+
+        case 0:
+            // The first level step of accel cal will be taken as gnd truth,
+            // i.e. trim will be set as per the output of primary accel from the level step
+            get_primary_accel_cal_sample_avg(0,aligned_sample);
+            _trim_pitch = atan2f(aligned_sample.x, pythagorous2(aligned_sample.y, aligned_sample.z));
+            _trim_roll = atan2f(-aligned_sample.y, -aligned_sample.z);
+            _new_trim = true;
+            break;
+        case 1:
+            // Reference accel is truth, in this scenario there is a reference accel
+            // as mentioned in ACC_BODY_ALIGNED
+            if (get_primary_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
+                // determine trim from aligned sample vs misaligned sample
+                Vector3f cross = (misaligned_sample%aligned_sample);
+                float dot = (misaligned_sample*aligned_sample);
+                Quaternion q(sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
+                q.normalize();
+                _trim_roll = q.get_euler_roll();
+                _trim_pitch = q.get_euler_pitch();
+                _new_trim = true;
+            }
+            break;
+        default:
+            _new_trim = false;
+            //noop
+    }
+
+    if (fabsf(_trim_roll) > radians(10) || 
+        fabsf(_trim_pitch) > radians(10)) {
+        hal.console->print("ERR: Trim over maximum of 10 degrees!!");
+        _new_trim = false;  //we have either got faulty level during acal or highly misaligned accelerometers 
+    }
+}
+
+void AP_InertialSensor::_acal_event_failure()
+{
+    for (uint8_t i=0; i<_accel_count; i++) {
+        _accel_offset[i].set_and_save(Vector3f(0,0,0));
+        _accel_scale[i].set_and_save(Vector3f(0,0,0));
+    }
+}
+
+/*
+    Returns true if new valid trim values are available and passes them to reference vars    
+*/
+bool AP_InertialSensor::get_new_trim(float& trim_roll, float &trim_pitch)
+{
+    if (_new_trim) {
+        trim_roll = _trim_roll;
+        trim_pitch = _trim_pitch;
+        _new_trim = false;
+        return true;
+    }
+    return false;
+}
+
+/*    
+    Returns body fixed accelerometer level data averaged during accel calibration's first step
+*/
+bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const
+{
+    if (_accel_count <= _acc_body_aligned || _accel_calibrator[2].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[2].get_num_samples_collected()) {
+        return false;
+    }
+    _accel_calibrator[_acc_body_aligned].get_sample_corrected(sample_num, ret);
+    ret.rotate(_board_orientation);
+    return true;
+}
+
+/*    
+    Returns Primary accelerometer level data averaged during accel calibration's first step
+*/
+bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const
+{
+    uint8_t count = 0;
+    Vector3f avg = Vector3f(0,0,0);
+    for(uint8_t i=0; i<MIN(_accel_count,2); i++) {
+        if (_accel_calibrator[i].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[i].get_num_samples_collected()) {
+            continue;
+        }
+        Vector3f sample;
+        _accel_calibrator[i].get_sample_corrected(sample_num, sample);
+        avg += sample;
+        count++;
+    }
+    if(count == 0) {
+        return false;
+    }
+    avg /= count;
+    ret = avg;
+    ret.rotate(_board_orientation);
+    return true;
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 32d0c87495..433d90dffd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -21,6 +21,7 @@
 #include <stdint.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_AccelCal/AP_AccelCal.h>
 #include "AP_InertialSensor_UserInteract.h"
 #include <Filter/LowPassFilter.h>
 #include <Filter/LowPassFilter2p.h>
@@ -41,12 +42,13 @@ class DataFlash_Class;
  * blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
  * original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
  */
-class AP_InertialSensor
+class AP_InertialSensor : AP_AccelCal_Client
 {
     friend class AP_InertialSensor_Backend;
 
 public:
     AP_InertialSensor();
+
     static AP_InertialSensor *get_instance();
 
     enum Gyro_Calibration_Timing {
@@ -69,11 +71,6 @@ public:
     uint8_t register_gyro(uint16_t raw_sample_rate_hz);
     uint8_t register_accel(uint16_t raw_sample_rate_hz);
 
-    // perform accelerometer calibration including providing user instructions
-    // and feedback
-    bool calibrate_accel(AP_InertialSensor_UserInteract *interact,
-                         float& trim_roll,
-                         float& trim_pitch);
     bool calibrate_trim(float &trim_roll, float &trim_pitch);
 
     /// calibrating - returns true if the gyros or accels are currently being calibrated
@@ -226,6 +223,24 @@ public:
 
     void detect_backends(void);
 
+    //Returns accel calibrator interface object pointer
+    AP_AccelCal* get_acal() const { return _acal; }
+
+    // Returns body fixed accelerometer level data averaged during accel calibration's first step
+    bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const;
+
+    // Returns primary accelerometer level data averaged during accel calibration's first step
+    bool get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;
+
+    // Returns newly calculated trim values if calculated
+    bool get_new_trim(float& trim_roll, float &trim_pitch);
+
+    // initialise and register accel calibrator
+    // called during the startup of accel cal
+    void acal_init();
+
+    // update accel calibrator
+    void acal_update();
 private:
 
     // load backend drivers
@@ -240,17 +255,6 @@ private:
     // blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
     // 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,
-                          float 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]);
-    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]);
     bool _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
 
     // save parameters to eeprom
@@ -380,9 +384,27 @@ private:
         float delta_time;
     } _hil {};
 
+    // Trim options
+    AP_Int8 _acc_body_aligned;
+    AP_Int8 _trim_option;
+
     DataFlash_Class *_dataflash;
 
     static AP_InertialSensor *_s_instance;
+    AP_AccelCal* _acal;
+
+    AccelCalibrator *_accel_calibrator;
+
+    //save accelerometer bias and scale factors
+    void _acal_save_calibrations();
+    void _acal_event_failure();
+
+    // Returns AccelCalibrator objects pointer for specified acceleromter
+    AccelCalibrator* _acal_get_calibrator(uint8_t i) { return i<get_accel_count()?&(_accel_calibrator[i]):NULL; }
+
+    float _trim_pitch;
+    float _trim_roll;
+    bool _new_trim;
 };
 
 #include "AP_InertialSensor_Backend.h"
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index 299ec24bd5..e2b7808219 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -130,6 +130,25 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
     _imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
     _imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
     _imu._delta_velocity_valid[instance] = true;
+
+
+    if (_imu._accel_calibrator[instance].get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
+        Vector3f cal_sample = _imu._delta_velocity[instance];
+
+        //remove rotation
+        cal_sample.rotate_inverse(_imu._board_orientation);
+
+        // remove scale factors
+        const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+        cal_sample.x /= accel_scale.x;
+        cal_sample.y /= accel_scale.y;
+        cal_sample.z /= accel_scale.z;
+
+        //remove offsets
+        cal_sample += _imu._accel_offset[instance].get() * _imu._delta_velocity_dt[instance] ;
+
+        _imu._accel_calibrator[instance].new_sample(cal_sample, _imu._delta_velocity_dt[instance]);
+    }
 }
 
 void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
index c283ac150f..015bc8d6be 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
+++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp
@@ -14,7 +14,6 @@ AP_InertialSensor ins;
 
 static void display_offsets_and_scaling();
 static void run_test();
-static void run_calibration();
 
 void setup(void)
 {
@@ -34,7 +33,6 @@ void loop(void)
     hal.console->println();
     hal.console->println(
     "Menu:\r\n"
-    "    c calibrate accelerometers\r\n"
     "    d) display offsets and scaling\r\n"
     "    l) level (capture offsets from level)\r\n"
     "    t) test\r\n"
@@ -49,11 +47,6 @@ void loop(void)
     while( hal.console->available() ) {
         user_input = hal.console->read();
 
-        if( user_input == 'c' || user_input == 'C' ) {
-            run_calibration();
-            display_offsets_and_scaling();
-        }
-
         if( user_input == 'd' || user_input == 'D' ) {
             display_offsets_and_scaling();
         }
@@ -68,19 +61,6 @@ void loop(void)
     }
 }
 
-static void run_calibration()
-{
-    float roll_trim, pitch_trim;
-    // clear off any other characters (like line feeds,etc)
-    while( hal.console->available() ) {
-        hal.console->read();
-    }
-
-
-    AP_InertialSensor_UserInteractStream interact(hal.console);
-    ins.calibrate_accel(&interact, roll_trim, pitch_trim);
-}
-
 static void display_offsets_and_scaling()
 {
     Vector3f accel_offsets = ins.get_accel_offsets();
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/make.inc b/libraries/AP_InertialSensor/examples/INS_generic/make.inc
index f954c1c34f..717ba41234 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/make.inc
+++ b/libraries/AP_InertialSensor/examples/INS_generic/make.inc
@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Declination
 LIBRARIES += AP_GPS
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_Math
 LIBRARIES += AP_Mission
 LIBRARIES += AP_NavEKF
@@ -22,3 +23,4 @@ LIBRARIES += DataFlash
 LIBRARIES += Filter
 LIBRARIES += GCS_MAVLink
 LIBRARIES += StorageManager
+LIBRARIES += AP_OpticalFlow
diff --git a/libraries/AP_InertialSensor/examples/VibTest/make.inc b/libraries/AP_InertialSensor/examples/VibTest/make.inc
index f954c1c34f..43ba9c239d 100644
--- a/libraries/AP_InertialSensor/examples/VibTest/make.inc
+++ b/libraries/AP_InertialSensor/examples/VibTest/make.inc
@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Declination
 LIBRARIES += AP_GPS
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_Math
 LIBRARIES += AP_Mission
 LIBRARIES += AP_NavEKF