diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp
index 2471c6a369..0bc1c30dbe 100644
--- a/AntennaTracker/AntennaTracker.cpp
+++ b/AntennaTracker/AntennaTracker.cpp
@@ -49,6 +49,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] PROGMEM = {
     SCHED_TASK(check_usb_mux,           5,    300),
     SCHED_TASK(gcs_retry_deferred,      1,   1000),
     SCHED_TASK(one_second_loop,        50,   3900),
+    SCHED_TASK(compass_cal_update,      1,    100),
 };
 
 /**
diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 5b8cbf5a44..33b42e9a01 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -260,6 +260,15 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(HWSTATUS);
         tracker.send_hwstatus(chan);
         break;
+    case MSG_MAG_CAL_PROGRESS:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS);
+        tracker.compass.send_mag_cal_progress(chan);
+        break;
+
+    case MSG_MAG_CAL_REPORT:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT);
+        tracker.compass.send_mag_cal_report(chan);
+        break;
 
     case MSG_SERVO_OUT:
     case MSG_EXTENDED_STATUS1:
@@ -459,6 +468,8 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
         send_message(MSG_SIMSTATE);
+        send_message(MSG_MAG_CAL_REPORT);
+        send_message(MSG_MAG_CAL_PROGRESS);
     }
 }
 
@@ -697,6 +708,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 break;
             }
 
+            case MAV_CMD_DO_START_MAG_CAL:
+            case MAV_CMD_DO_ACCEPT_MAG_CAL:
+            case MAV_CMD_DO_CANCEL_MAG_CAL:
+                result = tracker.compass.handle_mag_cal_command(packet);
+                break;
+
             default:
                 break;
         }
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index cea0a7ec35..8066fe18de 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -247,6 +247,7 @@ private:
     void update_armed_disarmed();
     void gcs_send_text_fmt(const prog_char_t *fmt, ...);
     void init_capabilities(void);
+    void compass_cal_update();
 
 public:
     void mavlink_snoop(const mavlink_message_t* msg);
diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp
index b6a76d561c..0ac692093e 100644
--- a/AntennaTracker/sensors.cpp
+++ b/AntennaTracker/sensors.cpp
@@ -56,6 +56,14 @@ void Tracker::barometer_accumulate(void)
     barometer.accumulate();
 }
 
+/*
+ calibrate compass
+*/
+void Tracker::compass_cal_update() {
+    if (!hal.util->get_soft_armed()) {
+        compass.compass_cal_update();
+    }
+}
 
 /*
   read the GPS