AP_Compass: added handling of compass calibration mavlink messages

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -242,3 +242,82 @@ Compass::get_cal_mask() const
     }
     return cal_mask;
 }
+
+
+/*
+  handle an incoming MAG_CAL command
+ */
+uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
+{
+    uint8_t result = MAV_RESULT_FAILED;
+
+    switch (packet.command) {
+    case MAV_CMD_DO_START_MAG_CAL: {
+        result = MAV_RESULT_ACCEPTED;
+        if (hal.util->get_soft_armed() || packet.param1 < 0 || packet.param1 > 255) {
+            result = MAV_RESULT_FAILED;
+            break;
+        }
+
+        uint8_t mag_mask = packet.param1;
+        bool retry = packet.param2;
+        bool autosave = packet.param3;
+        float delay = packet.param4;
+        bool autoreboot = packet.param5;
+
+        if (mag_mask == 0) { // 0 means all
+            if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
+                result = MAV_RESULT_FAILED;
+            }
+        } else {
+            if (!start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
+                result = MAV_RESULT_FAILED;
+            }
+        }
+        
+        break;
+    }
+
+    case MAV_CMD_DO_ACCEPT_MAG_CAL: {
+        result = MAV_RESULT_ACCEPTED;
+        if(packet.param1 < 0 || packet.param1 > 255) {
+            result = MAV_RESULT_FAILED;
+            break;
+        }
+        
+        uint8_t mag_mask = packet.param1;
+        
+        if (mag_mask == 0) { // 0 means all
+            if(!accept_calibration_all()) {
+                result = MAV_RESULT_FAILED;
+            }
+            break;
+        }
+        
+        if(!accept_calibration_mask(mag_mask)) {
+            result = MAV_RESULT_FAILED;
+        }
+        break;
+    }
+        
+    case MAV_CMD_DO_CANCEL_MAG_CAL: {
+        result = MAV_RESULT_ACCEPTED;
+        if(packet.param1 < 0 || packet.param1 > 255) {
+            result = MAV_RESULT_FAILED;
+            break;
+        }
+        
+        uint8_t mag_mask = packet.param1;
+        
+        if (mag_mask == 0) { // 0 means all
+            cancel_calibration_all();
+            break;
+        }
+        
+        cancel_calibration_mask(mag_mask);
+        break;
+    }
+    }
+    
+    return result;
+}

libraries/AP_Compass/Compass.h

@@ -152,6 +152,11 @@ public:
     uint8_t get_cal_mask() const;
     bool is_calibrating() const;
 
+    /*
+      handle an incoming MAG_CAL command
+    */
+    uint8_t handle_mag_cal_command(const mavlink_command_long_t &packet);
+
     void send_mag_cal_progress(mavlink_channel_t chan);
     void send_mag_cal_report(mavlink_channel_t chan);