Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: using atan2f() gives more accurate euler corrections

Browse Source 
thanks to Jon and Paul!
This commit is contained in:
Andrew Tridgell 2015-05-16 08:19:18 +10:00
parent 9ceee3cea7
commit aaa35bd1ec
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -435,8 +435,8 @@ AP_InertialSensor::_detect_backends(void)
*/ */
bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch) 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 = atan2f(accel_sample.x, pythagorous2(accel_sample.y, accel_sample.z));
trim_pitch = asinf(accel_sample.x/GRAVITY_MSS); trim_roll = atan2f(-accel_sample.y, -accel_sample.z);
if (fabsf(trim_roll) > radians(10) || if (fabsf(trim_roll) > radians(10) ||
fabsf(trim_pitch) > radians(10)) { fabsf(trim_pitch) > radians(10)) {
hal.console->println_P(PSTR("trim over maximum of 10 degrees")); hal.console->println_P(PSTR("trim over maximum of 10 degrees"));