Tracker: wire up accel cal for tracker

AntennaTracker/AntennaTracker.cpp

@@ -47,6 +47,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
     SCHED_TASK(gcs_retry_deferred,     50,   1000),
     SCHED_TASK(one_second_loop,         1,   3900),
     SCHED_TASK(compass_cal_update,     50,    100),
+    SCHED_TASK(accel_cal_update,       10,    100)
 };
 
 /**

AntennaTracker/GCS_Mavlink.cpp

@@ -631,18 +631,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 if (is_equal(packet.param4,1.0f)) {
                     // Cant trim radio
                 } else if (is_equal(packet.param5,1.0f)) {
-                    float trim_roll, trim_pitch;
-                    AP_InertialSensor_UserInteract_MAVLink interact(this);
+                    result = MAV_RESULT_ACCEPTED;
                     // start with gyro calibration
                     tracker.ins.init_gyro();
                     // reset ahrs gyro bias
                     if (tracker.ins.gyro_calibrated_ok_all()) {
                         tracker.ahrs.reset_gyro_drift();
+                    } else {
+                        result = MAV_RESULT_FAILED;
                     }
-                    if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
-                        // reset ahrs's trim to suggested values from calibration routine
-                        tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                    }
+                    // start accel cal
+                    tracker.ins.acal_init();
+                    tracker.ins.get_acal()->start(this);
                 } else if (is_equal(packet.param5,2.0f)) {
                     // start with gyro calibration
                     tracker.ins.init_gyro();

AntennaTracker/Tracker.h

@@ -40,6 +40,7 @@
 #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_ADC/AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
 #include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
+#include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
 #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
 #include <Filter/Filter.h>                     // Filter library
 #include <AP_Buffer/AP_Buffer.h>      // APM FIFO Buffer
@@ -221,6 +222,7 @@ private:
     void update_ahrs();
     void update_compass(void);
     void compass_accumulate(void);
+    void accel_cal_update(void);
     void barometer_accumulate(void);
     void update_GPS(void);
     void init_servos();

AntennaTracker/make.inc

@@ -6,6 +6,7 @@ LIBRARIES += AP_Baro
 LIBRARIES += AP_Compass
 LIBRARIES += AP_Math
 LIBRARIES += AP_ADC
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_InertialSensor
 LIBRARIES += AP_AHRS
 LIBRARIES += Filter

AntennaTracker/sensors.cpp

@@ -71,6 +71,20 @@ void Tracker::compass_cal_update() {
     }
 }
 
+/*
+    Accel calibration
+*/
+void Tracker::accel_cal_update() {
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+    ins.acal_update();
+    float trim_roll, trim_pitch;
+    if(ins.get_new_trim(trim_roll, trim_pitch)) {
+        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+    }
+}
+
 /*
   read the GPS
  */