Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: return roll and pitch trim angles

This commit is contained in:
Randy Mackay 2013-01-30 20:47:57 +09:00 committed by rmackay9
parent 54e7bce75e
commit 21de9f5f47
2 changed files with 38 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -311,7 +311,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
// original sketch available at // original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on), 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 samples[6];
Vector3f new_offsets; Vector3f new_offsets;
@ -382,6 +384,10 @@ bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
_accel_offset.set(new_offsets); _accel_offset.set(new_offsets);
_accel_scale.set(new_scaling); _accel_scale.set(new_scaling);
_save_parameters(); _save_parameters();
// calculate the trims as well and pass back to caller
_calculate_trim(samples[0], trim_roll, trim_pitch);
return true; 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__ #endif // __AVR_ATmega1280__

5
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -64,7 +64,9 @@ public:
// perform accelerometer calibration including providing user instructions // perform accelerometer calibration including providing user instructions
// and feedback // and feedback
virtual bool calibrate_accel(void (*flash_leds_cb)(bool on), 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 #endif
/// Perform cold-start initialisation for just the gyros. /// 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_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_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 _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 #endif
// save parameters to eeprom // save parameters to eeprom