9param magcalib

ArduCopter/ArduCopter.pde

@@ -760,7 +760,8 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { run_nav_updates,       8,     80 },
     { update_thr_average,   40,     10 },
     { three_hz_loop,       133,      9 },
-    { compass_accumulate,    8,     42 },
+    { compass_accumulate,    4,     42 },
+    { compass_cal_update,    4,     40 },
     { barometer_accumulate,  8,     25 },
 #if FRAME_CONFIG == HELI_FRAME
     { check_dynamic_flight,  8,     10 },

ArduCopter/GCS_Mavlink.pde

@@ -636,6 +636,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_BATTERY2:
         break; // just here to prevent a warning
+
+    case MSG_MAG_CAL_PROGRESS:
+        compass.send_mag_cal_progress(chan);
+        break;
+
+    case MSG_MAG_CAL_REPORT:
+        compass.send_mag_cal_report(chan);
+        break;
     }
 
     return true;
@@ -854,6 +862,8 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_MOUNT_STATUS);
         send_message(MSG_OPTICAL_FLOW);
         send_message(MSG_GIMBAL_REPORT);
+        send_message(MSG_MAG_CAL_REPORT);
+        send_message(MSG_MAG_CAL_PROGRESS);
     }
 }
 
@@ -1305,6 +1315,70 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+        case MAV_CMD_DO_START_MAG_CAL: {
+            result = MAV_RESULT_ACCEPTED;
+            if(motors.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;
+
+            if (mag_mask == 0) { // 0 means all
+                compass.start_calibration_all(retry, autosave, delay);
+                break;
+            }
+
+            if(!compass.start_calibration_mask(mag_mask, retry, autosave, delay)) {
+                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(!compass.accept_calibration_all()) {
+                    result = MAV_RESULT_FAILED;
+                }
+                break;
+            }
+
+            if(!compass.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
+                compass.cancel_calibration_all();
+                break;
+            }
+
+            compass.cancel_calibration_mask(mag_mask);
+            break;
+        }
+
         default:
             result = MAV_RESULT_UNSUPPORTED;
             break;

ArduCopter/compass_cal.pde

@@ -0,0 +1,24 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define COMPASS_CAL_STICK_DELAY 2.0f // 2 seconds
+#define COMPASS_CAL_DELAY 5.0f
+
+static void compass_cal_update() {
+    compass.compass_cal_update();
+
+    if (compass.is_calibrating()) {
+        return;
+    }
+
+    bool stick_gesture_detected = !motors.armed() && g.rc_4.control_in > 4000 && g.rc_3.control_in > 900;
+    static uint32_t stick_gesture_begin = 0;
+    uint32_t tnow = millis();
+
+    if(!stick_gesture_detected) {
+        stick_gesture_begin = tnow;
+    } else {
+        if(tnow-stick_gesture_begin > 1000*COMPASS_CAL_STICK_DELAY) {
+            compass.start_calibration_all(true,true,COMPASS_CAL_DELAY);
+        }
+    }
+}

ArduCopter/motors.pde

@@ -584,6 +584,13 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs)
     start_logging();
 #endif
 
+    if(compass.is_calibrating()) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Compass calibration running"));
+        }
+        return false;
+    }
+
     // always check if the current mode allows arming
     if (!mode_allows_arming(control_mode, arming_from_gcs)) {
         if (display_failure) {

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -99,26 +99,16 @@ bool AP_Compass_PX4::read(void)
 
     for (uint8_t i=0; i<_num_instances; i++) {
         // avoid division by zero if we haven't received any mag reports
-        if (_count[i] == 0) continue;
+        if (_count[i] == 0) {
+            continue;
+        }
 
+        // take average of field measurements since last read()
         _sum[i] /= _count[i];
-        _sum[i] *= 1000;
-
-        // apply default board orientation for this compass type. This is
-        // a noop on most boards
-        _sum[i].rotate(MAG_BOARD_ORIENTATION);
 
-        if (_external[i]) {
-            // add user selectable orientation
-            _sum[i].rotate((enum Rotation)_orientation[i].get());
-        } else {
-            // add in board orientation from AHRS
-            _sum[i].rotate(_board_orientation);
-        }
-
+        // publish to _field
         _field[i] = _sum[i];
-        apply_corrections(_field[i],i);
-
+
         _sum[i].zero();
         _count[i] = 0;
     }
@@ -132,12 +122,35 @@ void AP_Compass_PX4::accumulate(void)
 {
     struct mag_report mag_report;
     for (uint8_t i=0; i<_num_instances; i++) {
+        bool received_sample = false;
+        // get latest report
         while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
                mag_report.timestamp != _last_timestamp[i]) {
-            _sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z);
-            _count[i]++;
             _last_timestamp[i] = mag_report.timestamp;
+            received_sample = true;
+        }
+
+        if(!received_sample) continue;
+
+        _raw_field[i] = Vector3f(mag_report.x, mag_report.y, mag_report.z);
+        _raw_field[i] *= 1000;
+
+        // apply default board orientation for this compass type. This is
+        // a noop on most boards
+        _raw_field[i].rotate(MAG_BOARD_ORIENTATION);
+
+        if (_external[i]) {
+            // add user selectable orientation
+            _raw_field[i].rotate((enum Rotation)_orientation[i].get());
+        } else {
+            // add in board orientation from AHRS
+            _raw_field[i].rotate(_board_orientation);
         }
+
+        apply_corrections(_raw_field[i],i);
+
+        _sum[i] += _raw_field[i];
+        _count[i]++;
     }
 }
 

libraries/AP_Compass/AP_Compass_PX4.h

@@ -22,12 +22,18 @@ class AP_Compass_PX4 : public Compass
     // return the primary compass
     uint8_t get_primary(void) const;
 
+    bool supports_raw_field() const { return true; }
+    void get_raw_field(uint8_t i, Vector3f &field) const { field = _raw_field[i]; }
+    uint32_t last_raw_update_us(uint8_t i) const { return _last_timestamp[i]; }
+
 private:
     uint8_t _num_instances;
     int _mag_fd[COMPASS_MAX_INSTANCES];
     Vector3f _sum[COMPASS_MAX_INSTANCES];
     uint32_t _count[COMPASS_MAX_INSTANCES];
     uint64_t _last_timestamp[COMPASS_MAX_INSTANCES];
+
+    Vector3f _raw_field[COMPASS_MAX_INSTANCES];
 };
 
 #endif // AP_Compass_PX4_H