diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 21881e6b11..eb7d99a331 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -63,6 +63,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
     SCHED_TASK(barometer_accumulate,    1,    900),
     SCHED_TASK(update_notify,           1,    300),
     SCHED_TASK(read_rangefinder,        1,    500),
+    SCHED_TASK(compass_cal_update,      1,    100),
 #if OPTFLOW == ENABLED
     SCHED_TASK(update_optical_flow,     1,    500),
 #endif
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 524665d0ff..9d58ca7dcd 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -806,6 +806,16 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
         mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
         break;
+
+    case MSG_MAG_CAL_PROGRESS:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS);
+        plane.compass.send_mag_cal_progress(chan);
+        break;
+
+    case MSG_MAG_CAL_REPORT:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT);
+        plane.compass.send_mag_cal_report(chan);
+        break;
     }
     return true;
 }
@@ -1037,6 +1047,8 @@ GCS_MAVLINK::data_stream_send(void)
 #if AP_TERRAIN_AVAILABLE
         send_message(MSG_TERRAIN);
 #endif
+        send_message(MSG_MAG_CAL_REPORT);
+        send_message(MSG_MAG_CAL_PROGRESS);
         send_message(MSG_BATTERY2);
         send_message(MSG_MOUNT_STATUS);
         send_message(MSG_OPTICAL_FLOW);
@@ -1395,6 +1407,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             plane.autotune_enable(!is_zero(packet.param1));
             break;
 
+        case MAV_CMD_DO_START_MAG_CAL:
+        case MAV_CMD_DO_ACCEPT_MAG_CAL:
+        case MAV_CMD_DO_CANCEL_MAG_CAL:
+            result = plane.compass.handle_mag_cal_command(packet);
+            break;
+
         default:
             break;
         }
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index b0ca76e267..d88fb62e83 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -874,6 +874,7 @@ private:
     void update_alt(void);
     void obc_fs_check(void);
     void compass_accumulate(void);
+    void compass_cal_update();
     void barometer_accumulate(void);
     void update_optical_flow(void);
     void one_second_loop(void);
diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp
index faa294856d..a5d757d3b4 100644
--- a/ArduPlane/sensors.cpp
+++ b/ArduPlane/sensors.cpp
@@ -33,6 +33,15 @@ void Plane::read_rangefinder(void)
 #endif
 }
 
+/*
+  calibrate compass
+*/
+void Plane::compass_cal_update() {
+    if (!hal.util->get_soft_armed()) {
+        compass.compass_cal_update();
+    }
+}
+
 /*
   ask airspeed sensor for a new value
  */