diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 666a5c33b0..2d58c00569 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -77,6 +77,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] PROGMEM = {
     SCHED_TASK(compass_accumulate,      1,    900),
     SCHED_TASK(update_notify,           1,    300),
     SCHED_TASK(one_second_loop,        50,   3000),
+    SCHED_TASK(compass_cal_update,      1,    100), 
 #if FRSKY_TELEM_ENABLED == ENABLED
     SCHED_TASK(frsky_telemetry_send,   10,    100),
 #endif
diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 98f3829288..210a8c0c01 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -589,13 +589,24 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
         break;
 
+    case MSG_MAG_CAL_PROGRESS:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_PROGRESS);
+        rover.compass.send_mag_cal_progress(chan);
+        break;
+
+    case MSG_MAG_CAL_REPORT:
+        CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT);
+        rover.compass.send_mag_cal_report(chan);
+        break;
+
     case MSG_RETRY_DEFERRED:
     case MSG_TERRAIN:
     case MSG_OPTICAL_FLOW:
     case MSG_GIMBAL_REPORT:
     case MSG_RPM:
         break; // just here to prevent a warning
-	}
+
+    }
 
     
     return true;
@@ -820,6 +831,8 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_RANGEFINDER);
         send_message(MSG_SYSTEM_TIME);
         send_message(MSG_BATTERY2);
+        send_message(MSG_MAG_CAL_REPORT);
+        send_message(MSG_MAG_CAL_PROGRESS);
         send_message(MSG_MOUNT_STATUS);
         send_message(MSG_EKF_STATUS_REPORT);
     }
@@ -1036,6 +1049,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 = rover.compass.handle_mag_cal_command(packet);
+            break;
+
         default:
                 break;
             }
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index f9b2d7a0e5..b9493a4126 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -372,6 +372,7 @@ private:
     void update_alt();
     void gcs_failsafe_check(void);
     void compass_accumulate(void);
+    void compass_cal_update(void);
     void update_compass(void);
     void update_logging1(void);
     void update_logging2(void);
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index 6029f1388b..d69e0a3557 100644
--- a/APMrover2/sensors.cpp
+++ b/APMrover2/sensors.cpp
@@ -28,6 +28,13 @@ void Rover::read_receiver_rssi(void)
     receiver_rssi = rssi.read_receiver_rssi_uint8();
 }
 
+//Calibrate compass
+void Rover::compass_cal_update() {
+    if (!hal.util->get_soft_armed()) {
+        compass.compass_cal_update();
+    }
+}
+
 // read the sonars
 void Rover::read_sonars(void)
 {