From 0e18b5eaad7ef7e035874d54a52777af96927ced Mon Sep 17 00:00:00 2001
From: Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
Date: Wed, 21 Oct 2015 14:27:35 -0700
Subject: [PATCH] Tracker: wire up accel cal for tracker

---
 AntennaTracker/AntennaTracker.cpp |  1 +
 AntennaTracker/GCS_Mavlink.cpp    | 12 ++++++------
 AntennaTracker/Tracker.h          |  2 ++
 AntennaTracker/make.inc           |  1 +
 AntennaTracker/sensors.cpp        | 14 ++++++++++++++
 5 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp
index 5e10930bc6..793f0c8122 100644
--- a/AntennaTracker/AntennaTracker.cpp
+++ b/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)
 };
 
 /**
diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 7bd6fde1a4..9afa77903a 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/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();
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index b4540a9303..fd15c670a5 100644
--- a/AntennaTracker/Tracker.h
+++ b/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();
diff --git a/AntennaTracker/make.inc b/AntennaTracker/make.inc
index 7912b28849..8436c32658 100644
--- a/AntennaTracker/make.inc
+++ b/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
diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp
index 4f88c8a242..d0956208f6 100644
--- a/AntennaTracker/sensors.cpp
+++ b/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
  */