From 4c40b44563ff64f5918e7339ff3c081d723bdb90 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 2 Feb 2013 10:59:58 +0900
Subject: [PATCH] Copter: limit max ahrs trim to 10degrees
Limit is applied as trims from accelerometer calibration are copied to
the ahrs object.
---
ArduCopter/GCS_Mavlink.pde | 2 ++
ArduCopter/setup.pde | 2 ++
2 files changed, 4 insertions(+)
diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 8e24b1d24..44be364e7 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1100,6 +1100,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// this blocks
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
+ trim_roll = constrain(trim_roll, ToRad(-10.0f), ToRad(10.0f));
+ trim_pitch = constrain(trim_pitch, ToRad(-10.0f), ToRad(10.0f));
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
result = MAV_RESULT_ACCEPTED;
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index e1c414fe1..c8c844fe9 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -292,6 +292,8 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv)
delay, flash_leds, &timer_scheduler);
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
+ trim_roll = constrain(trim_roll, ToRad(-10.0f), ToRad(10.0f));
+ trim_pitch = constrain(trim_pitch, ToRad(-10.0f), ToRad(10.0f));
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
report_ins();
return(0);
--
GitLab