Copter: integrate automatic roll and pitch trims

ArduCopter/GCS_Mavlink.pde

@@ -1227,9 +1227,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 trim_radio();
             }
             if (packet.param5 == 1) {
+                float trim_roll, trim_pitch;
                 // this blocks
                 AP_InertialSensor_UserInteractStream interact(hal.console);
-                ins.calibrate_accel(flash_leds, &interact);
+                if(ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch)) {
+                    // reset ahrs's trim to suggested values from calibration routine
+                    ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+                }
             }
             result = MAV_RESULT_ACCEPTED;
             break;

ArduCopter/setup.pde

@@ -288,13 +288,18 @@ static void setup_wait_key(void)
 static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
+    float trim_roll, trim_pitch;
+
     cliSerial->println_P(PSTR("Initialising gyros"));
     ahrs.init();
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);
     AP_InertialSensor_UserInteractStream interact(hal.console);
-    ins.calibrate_accel(flash_leds, &interact);
+    if(ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch)) {
+        // reset ahrs's trim to suggested values from calibration routine
+        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+    }
     report_ins();
     return(0);
 }