diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 104136a92b5731923dc1ff5b78565e4c784d9eee..8e24b1d2478b15cce832ce20e629c25c32ae99b4 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1096,8 +1096,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 trim_radio();
             }
             if (packet.param5 == 1) {
+                float trim_roll, trim_pitch;
                 // this blocks
-                ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key);
+                ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key, trim_roll, trim_pitch);
+                // reset ahrs's trim to suggested values from calibration routine
+                ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
             }
             result = MAV_RESULT_ACCEPTED;
             break;
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 102c6fe3d353801893407f44ccc846c1dda4d175..e11253f4cdf5fbf7f8fb33545f23471bee37bb71 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -284,11 +284,15 @@ static void setup_wait_key(void)
 static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
+    float trim_roll, trim_pitch;
+
     cliSerial->println_P(PSTR("Initialising gyros"));
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              delay, flash_leds, &timer_scheduler);
-    ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key);
+    ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key, trim_roll, trim_pitch);
+    // reset ahrs's trim to suggested values from calibration routine
+    ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
     report_ins();
     return(0);
 }