diff --git a/AntennaTracker/GCS_Mavlink.pde b/AntennaTracker/GCS_Mavlink.pde
index 70082d6a1b..06cdc17e33 100644
--- a/AntennaTracker/GCS_Mavlink.pde
+++ b/AntennaTracker/GCS_Mavlink.pde
@@ -519,10 +519,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             
             case MAV_CMD_PREFLIGHT_CALIBRATION:
             {
-                if (packet.param1 == 1 ||
-                    packet.param2 == 1) {
-                    calibrate_ins();
-                } else if (packet.param3 == 1) {
+                if (packet.param1 == 1) {
+                    ins.init_gyro();
+                    if (ins.gyro_calibrated_ok_all()) {
+                        ahrs.reset_gyro_drift();
+                        result = MAV_RESULT_ACCEPTED;
+                    } else {
+                        result = MAV_RESULT_FAILED;
+                    }
+                } 
+                if (packet.param3 == 1) {
                     init_barometer();
                     // zero the altitude difference on next baro update
                     nav_status.need_altitude_calibration = true;
@@ -531,7 +537,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     // Cant trim radio
                 }
 #if !defined( __AVR_ATmega1280__ )
-                if (packet.param5 == 1) {
+                else if (packet.param5 == 1) {
                     float trim_roll, trim_pitch;
                     AP_InertialSensor_UserInteract_MAVLink interact(this);
                     if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
diff --git a/AntennaTracker/system.pde b/AntennaTracker/system.pde
index 2fff56006f..1aa0af5672 100644
--- a/AntennaTracker/system.pde
+++ b/AntennaTracker/system.pde
@@ -99,19 +99,6 @@ static void init_tracker()
     nav_status.need_altitude_calibration = true;
 }
 
-// Level the tracker by calibrating the INS
-// Requires that the tracker be physically 'level' and horizontal
-static void calibrate_ins()
-{
-    gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move tracker"));
-    ahrs.init();
-    ins.init(AP_InertialSensor::COLD_START, ins_sample_rate);
-    ins.init_accel();
-    ahrs.set_trim(Vector3f(0, 0, 0));
-    ahrs.reset();
-    init_barometer();
-}
-
 // updates the status of the notify objects
 // should be called at 50hz
 static void update_notify()