Plane: wire up accel calibrator for plane

ArduPlane/ArduPlane.cpp

@@ -60,6 +60,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(update_notify,          50,    300),
     SCHED_TASK(read_rangefinder,       50,    500),
     SCHED_TASK(compass_cal_update,     50,    100),
+    SCHED_TASK(accel_cal_update,       10,    100),
 #if OPTFLOW == ENABLED
     SCHED_TASK(update_optical_flow,    50,    500),
 #endif

ArduPlane/GCS_Mavlink.cpp

@@ -1219,6 +1219,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_PREFLIGHT_CALIBRATION:
+            if(hal.util->get_soft_armed()) {
+                result = MAV_RESULT_FAILED;
+                break;
+            }
             plane.in_calibration = true;
             if (is_equal(packet.param1,1.0f)) {
                 plane.ins.init_gyro();
@@ -1238,21 +1242,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 plane.trim_radio();
                 result = MAV_RESULT_ACCEPTED;
             } 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
                 plane.ins.init_gyro();
                 // reset ahrs gyro bias
                 if (plane.ins.gyro_calibrated_ok_all()) {
                     plane.ahrs.reset_gyro_drift();
-                }
-                if(plane.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
-                    // reset ahrs's trim to suggested values from calibration routine
-                    plane.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-                    result = MAV_RESULT_ACCEPTED;
                 } else {
                     result = MAV_RESULT_FAILED;
                 }
+                plane.ins.acal_init();
+                plane.ins.get_acal()->start(this);
+
             } else if (is_equal(packet.param5,2.0f)) {
                 // start with gyro calibration
                 plane.ins.init_gyro();
@@ -1521,7 +1522,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-
     case MAVLINK_MSG_ID_SET_MODE:
     {
         handle_set_mode(msg, FUNCTOR_BIND(&plane, &Plane::mavlink_set_mode, bool, uint8_t));

ArduPlane/Plane.h

@@ -47,6 +47,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 <RC_Channel/RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
@@ -702,6 +703,7 @@ private:
     // time that rudder arming has been running
     uint32_t rudder_arm_timer;
 
+
     void demo_servos(uint8_t i);
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
@@ -982,11 +984,11 @@ private:
     void init_capabilities(void);
     void dataflash_periodic(void);
     uint16_t throttle_min(void) const;
-    
     void do_parachute(const AP_Mission::Mission_Command& cmd);
     void parachute_check();
     void parachute_release();
     bool parachute_manual_release();
+    void accel_cal_update(void);
 
 public:
     void mavlink_delay_cb();

ArduPlane/make.inc

@@ -8,6 +8,7 @@ LIBRARIES += AP_Compass
 LIBRARIES += AP_Math
 LIBRARIES += AP_ADC
 LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AccelCal
 LIBRARIES += AP_AHRS
 LIBRARIES += RC_Channel
 LIBRARIES += AP_RangeFinder

ArduPlane/sensors.cpp

@@ -65,6 +65,20 @@ void Plane::compass_cal_update() {
     }
 }
 
+/*
+    Accel calibration
+*/
+void Plane::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));
+    }
+}
+
 /*
   ask airspeed sensor for a new value
  */