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: fixed AHRS_TRIM calculation again

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-16 07:18:09 +10:00
parent dd7c42be67
commit b564ba0868
2 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -428,6 +428,26 @@ AP_InertialSensor::_detect_backends(void)
_product_id.set(_backends[0]->product_id()); _product_id.set(_backends[0]->product_id());
} }
/*
_calculate_trim - calculates the x and y trim angles. The
accel_sample must be correctly scaled, offset and oriented for the
board
*/
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);
if (fabsf(trim_roll) > radians(10) ||
fabsf(trim_pitch) > radians(10)) {
hal.console->println_P(PSTR("trim over maximum of 10 degrees"));
return false;
}
hal.console->printf_P(PSTR("Trim OK: roll=%.2f pitch=%.2f\n"),
degrees(trim_roll),
degrees(trim_pitch));
return true;
}
#if !defined( __AVR_ATmega1280__ ) #if !defined( __AVR_ATmega1280__ )
// calibrate_accel - perform accelerometer calibration including providing user // calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed // instructions and feedback Gauss-Newton accel calibration routines borrowed
@ -585,11 +605,12 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
level_sample.x *= new_scaling[0].x; level_sample.x *= new_scaling[0].x;
level_sample.y *= new_scaling[0].y; level_sample.y *= new_scaling[0].y;
level_sample.z *= new_scaling[0].z; level_sample.z *= new_scaling[0].z;
level_sample += new_offsets[0]; level_sample -= new_offsets[0];
level_sample.rotate(saved_orientation); level_sample.rotate(saved_orientation);
trim_pitch = atan2(level_sample.x,pythagorous2(level_sample.y,level_sample.z)); if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
trim_roll = atan2(-level_sample.y,-level_sample.z); goto failed;
}
_board_orientation = saved_orientation; _board_orientation = saved_orientation;

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -236,6 +236,7 @@ private:
void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
void _calibrate_reset_matrices(float dS[6], float JS[6][6]); void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
bool _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
#endif #endif
// save parameters to eeprom // save parameters to eeprom