diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 59fb392377..e127707a0e 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -73,6 +73,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_notify,          50,    300),
     SCHED_TASK(one_second_loop,         1,   3000),
     SCHED_TASK(compass_cal_update,     50,    100), 
+    SCHED_TASK(accel_cal_update,       10,    100),
 #if FRSKY_TELEM_ENABLED == ENABLED
     SCHED_TASK(frsky_telemetry_send,    5,    100),
 #endif
diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index c5dc7b6711..b9adc7e663 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -957,6 +957,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_CMD_PREFLIGHT_CALIBRATION:
+                if(hal.util->get_soft_armed()) {
+                    result = MAV_RESULT_FAILED;
+                    break;
+                }
                 if (is_equal(packet.param1,1.0f)) {
                     rover.ins.init_gyro();
                     if (rover.ins.gyro_calibrated_ok_all()) {
@@ -972,21 +976,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     rover.trim_radio();
                     result = MAV_RESULT_ACCEPTED;
                 } else if (is_equal(packet.param5,1.0f)) {
-                    float trim_roll, trim_pitch;
-                    AP_InertialSensor_UserInteract_MAVLink interact(this);
+                    result = MAV_RESULT_ACCEPTED;
                     // start with gyro calibration
                     rover.ins.init_gyro();
                     // reset ahrs gyro bias
                     if (rover.ins.gyro_calibrated_ok_all()) {
                         rover.ahrs.reset_gyro_drift();
-                    }
-                    if(rover.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
-                        // reset ahrs's trim to suggested values from calibration routine
-                        rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                        result = MAV_RESULT_ACCEPTED;
                     } else {
                         result = MAV_RESULT_FAILED;
                     }
+                    rover.ins.acal_init();
+                    rover.ins.get_acal()->start(this);
+
                 } else if (is_equal(packet.param5,2.0f)) {
                     // start with gyro calibration
                     rover.ins.init_gyro();
@@ -1128,7 +1129,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
-
     case MAVLINK_MSG_ID_SET_MODE:
 		{
             handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 3fe5d42026..c6dc8c106d 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -38,6 +38,7 @@
 #include <AP_Compass/AP_Compass.h>     // ArduPilot Mega Magnetometer Library
 #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
+#include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
 #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
 #include <AP_NavEKF/AP_NavEKF.h>
 #include <AP_NavEKF2/AP_NavEKF2.h>
@@ -363,7 +364,6 @@ private:
     // Loiter control
     uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds)
     uint32_t loiter_time;     // How long have we been loitering - The start time in millis
-
     float distance_past_wp; // record the distance we have gone past the wp
 
     // time that rudder/steering arming has been running
@@ -522,7 +522,7 @@ private:
     bool arm_motors(AP_Arming::ArmingMethod method);
     bool motor_active();
     void update_home();
-
+    void accel_cal_update(void);
 public:
     bool print_log_menu(void);
     int8_t dump_log(uint8_t argc, const Menu::arg *argv);
diff --git a/APMrover2/make.inc b/APMrover2/make.inc
index 5d2673a891..8feecd24b9 100644
--- a/APMrover2/make.inc
+++ b/APMrover2/make.inc
@@ -8,6 +8,7 @@ LIBRARIES += AP_Baro
 LIBRARIES += AP_Compass
 LIBRARIES += AP_Math
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_AHRS
 LIBRARIES += AP_NavEKF
 LIBRARIES += AP_NavEKF2
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index a5629561a2..cbeb2ee518 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -35,6 +35,20 @@ void Rover::compass_cal_update() {
     }
 }
 
+// Accel calibration
+
+void Rover::accel_cal_update() {
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+    ins.acal_update();
+    // check if new trim values, and set them    float trim_roll, trim_pitch;
+    float trim_roll,trim_pitch;
+    if(ins.get_new_trim(trim_roll, trim_pitch)) {
+        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+    }
+}
+
 // read the sonars
 void Rover::read_sonars(void)
 {
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index 5b97be253a..b7d9edb03f 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -396,8 +396,7 @@ void Rover::startup_INS_ground(void)
 	ahrs.set_fly_forward(true);
     ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
 
-	ins.init(scheduler.get_loop_rate_hz());
-
+    ins.init(scheduler.get_loop_rate_hz());
     ahrs.reset();
 }
 
diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp
index 0421f00f73..67ffb8dd40 100644
--- a/APMrover2/test.cpp
+++ b/APMrover2/test.cpp
@@ -308,7 +308,8 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
 	//cliSerial->printf("Calibrating.");
 	ahrs.init();
     ahrs.set_fly_forward(true);
-	ins.init(scheduler.get_loop_rate_hz());
+
+    ins.init(scheduler.get_loop_rate_hz());
     ahrs.reset();
 
 	print_hit_enter();
@@ -371,7 +372,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
     ahrs.set_compass(&compass);
 
     // we need the AHRS initialised for this test
-	ins.init(scheduler.get_loop_rate_hz());
+    ins.init(scheduler.get_loop_rate_hz());
     ahrs.reset();
 
 	int counter = 0;