From aaa35bd1ec1f1affa6c25ab604bc5063ab7fbbd4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 May 2015 08:19:18 +1000 Subject: [PATCH] AP_InertialSensor: using atan2f() gives more accurate euler corrections thanks to Jon and Paul! --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 45c3e569b2..3e3420f30d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -435,8 +435,8 @@ AP_InertialSensor::_detect_backends(void) */ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch) { - trim_roll = -asinf(accel_sample.y/GRAVITY_MSS); - trim_pitch = asinf(accel_sample.x/GRAVITY_MSS); + trim_pitch = atan2f(accel_sample.x, pythagorous2(accel_sample.y, accel_sample.z)); + trim_roll = atan2f(-accel_sample.y, -accel_sample.z); if (fabsf(trim_roll) > radians(10) || fabsf(trim_pitch) > radians(10)) { hal.console->println_P(PSTR("trim over maximum of 10 degrees"));