From 21de9f5f47a791fe0b59db5e32cecb2a5d889372 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 30 Jan 2013 20:47:57 +0900 Subject: [PATCH] AP_InertialSensor: return roll and pitch trim angles --- .../AP_InertialSensor/AP_InertialSensor.cpp | 35 ++++++++++++++++++- .../AP_InertialSensor/AP_InertialSensor.h | 5 ++- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 9473429f17..52bfe9fd3e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -311,7 +311,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) // original sketch available at // http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on), - AP_InertialSensor_UserInteract* interact) + AP_InertialSensor_UserInteract* interact, + float &trim_roll, + float &trim_pitch) { Vector3f samples[6]; Vector3f new_offsets; @@ -382,6 +384,10 @@ bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on), _accel_offset.set(new_offsets); _accel_scale.set(new_scaling); _save_parameters(); + + // calculate the trims as well and pass back to caller + _calculate_trim(samples[0], trim_roll, trim_pitch); + return true; } @@ -541,5 +547,32 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float } } +// _calculate_trim - calculates the x and y trim angles (in radians) given a raw accel sample (i.e. no scaling or offsets applied) taken when the vehicle was level +void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch) +{ + // scale sample and apply offsets + Vector3f accel_scale = _accel_scale.get(); + Vector3f accel_offsets = _accel_offset.get(); + Vector3f scaled_accels_x( accel_sample.x * accel_scale.x - accel_offsets.x, + 0, + accel_sample.z * accel_scale.z - accel_offsets.z ); + Vector3f scaled_accels_y( 0, + accel_sample.y * accel_scale.y - accel_offsets.y, + accel_sample.z * accel_scale.z - accel_offsets.z ); + + // calculate x and y axis angle (i.e. roll and pitch angles) + Vector3f vertical = Vector3f(0,0,-1); + trim_roll = scaled_accels_y.angle(vertical); + trim_pitch = scaled_accels_x.angle(vertical); + + // angle call doesn't return the sign so take care of it here + if( scaled_accels_y.y > 0 ) { + trim_roll = -trim_roll; + } + if( scaled_accels_x.x < 0 ) { + trim_pitch = -trim_pitch; + } +} + #endif // __AVR_ATmega1280__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 73b0dfcc4b..96caf92daa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -64,7 +64,9 @@ public: // perform accelerometer calibration including providing user instructions // and feedback virtual bool calibrate_accel(void (*flash_leds_cb)(bool on), - AP_InertialSensor_UserInteract *interact); + AP_InertialSensor_UserInteract *interact, + float& trim_roll, + float& trim_pitch); #endif /// Perform cold-start initialisation for just the gyros. @@ -144,6 +146,7 @@ protected: virtual void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); virtual void _calibrate_reset_matrices(float dS[6], float JS[6][6]); virtual void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); + virtual void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); #endif // save parameters to eeprom