Rover: integrate automatic roll and pitch trims

APMrover2/setup.pde

@@ -296,13 +296,17 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
 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));
+    }
     return(0);
 }
 #endif