From 5b50f31b8fc0fb02b61391decdb651a18799350b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Jan 2013 20:48:42 +0900
Subject: [PATCH] Copter: integrate automatic roll and pitch trims
---
ArduCopter/GCS_Mavlink.pde | 5 ++++-
ArduCopter/setup.pde | 6 +++++-
2 files changed, 9 insertions(+), 2 deletions(-)
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 104136a92..8e24b1d24 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1096,8 +1096,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
trim_radio();
}
if (packet.param5 == 1) {
+ float trim_roll, trim_pitch;
// this blocks
- ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key);
+ ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key, 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;
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index 102c6fe3d..e11253f4c 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -284,11 +284,15 @@ 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"));
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate,
delay, flash_leds, &timer_scheduler);
- ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key);
+ ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key, 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);
}
--
GitLab