AntennaTracker: implement on-board compass calibration for Antenna tracker

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),
 };
 
 /**

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;
         }

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);

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