@ -10,8 +10,10 @@
@@ -10,8 +10,10 @@
* as published by the Free Software Foundation ; either version 2.1
* of the License , or ( at your option ) any later version .
*/
# include <FastSerial.h>
# include <AP_AHRS.h>
# include <AP_HAL.h>
extern const AP_HAL : : HAL & hal ;
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
@ -28,24 +30,20 @@
@@ -28,24 +30,20 @@
# define SPIN_RATE_LIMIT 20
void
AP_AHRS_MPU6000 : : init ( AP_PeriodicProcess * scheduler )
AP_AHRS_MPU6000 : : init ( )
{
bool timer_running = false ;
// suspend timer so interrupts on spi bus do not interfere with communication to mpu6000
if ( scheduler ! = NULL ) {
timer_running = scheduler - > running ( ) ;
scheduler - > suspend_timer ( ) ;
}
// suspend timer so interrupts on spi bus do not interfere with
// communication to mpu6000
hal . scheduler - > suspend_timer_procs ( ) ;
_mpu6000 - > dmp_init ( ) ;
push_gains_to_dmp ( ) ;
_mpu6000 - > push_gyro_offsets_to_dmp ( ) ;
// restart timer
if ( timer_running ) {
scheduler - > resume_timer ( ) ;
}
hal . scheduler - > resume_timer_procs ( ) ;
} ;
// run a full MPU6000 update round
@ -87,8 +85,9 @@ float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
@@ -87,8 +85,9 @@ float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
}
}
// Function to correct the gyroX and gyroY bias (roll and pitch) using the gravity vector from accelerometers
// We use the internal chip axis definition to make the bias correction because both sensors outputs (gyros and accels)
// Function to correct the gyroX and gyroY bias (roll and pitch) using the
// gravity vector from accelerometers We use the internal chip axis definition
// to make the bias correction because both sensors outputs (gyros and accels)
// are in chip axis definition
void AP_AHRS_MPU6000 : : drift_correction ( float deltat )
{
@ -97,40 +96,52 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
@@ -97,40 +96,52 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
// Get current values for gyros
_accel_vector = _ins - > get_accel ( ) ;
// We take the accelerometer readings and cumulate to average them and obtain the gravity vector
// We take the accelerometer readings and cumulate to average them and
// obtain the gravity vector
_accel_filtered + = _accel_vector ;
_accel_filtered_samples + + ;
_gyro_bias_from_gravity_counter + + ;
// We make the bias calculation and correction at a lower rate (GYRO_BIAS_FROM_GRAVITY_RATE)
// We make the bias calculation and correction at a lower rate
// (GYRO_BIAS_FROM_GRAVITY_RATE)
if ( _gyro_bias_from_gravity_counter = = GYRO_BIAS_FROM_GRAVITY_RATE ) {
_gyro_bias_from_gravity_counter = 0 ;
_accel_filtered / = _accel_filtered_samples ; // average
// Adjust ground reference : Accel Cross Gravity to obtain the error between gravity from accels and gravity from attitude solution
// Adjust ground reference : Accel Cross Gravity to obtain the error
// between gravity from accels and gravity from attitude solution
// errorRollPitch are in Accel LSB units
errorRollPitch [ 0 ] = _accel_filtered . y * _dcm_matrix . c . z + _accel_filtered . z * _dcm_matrix . c . x ;
errorRollPitch [ 1 ] = - _accel_filtered . z * _dcm_matrix . c . y - _accel_filtered . x * _dcm_matrix . c . z ;
errorRollPitch [ 0 ] = _accel_filtered . y * _dcm_matrix . c . z
+ _accel_filtered . z * _dcm_matrix . c . x ;
errorRollPitch [ 1 ] = - _accel_filtered . z * _dcm_matrix . c . y
- _accel_filtered . x * _dcm_matrix . c . z ;
errorRollPitch [ 0 ] * = deltat * 1000 ;
errorRollPitch [ 1 ] * = deltat * 1000 ;
// we limit to maximum gyro drift rate on each axis
float drift_limit = _mpu6000 - > get_gyro_drift_rate ( ) * deltat / _gyro_bias_from_gravity_gain ; //0.65*0.04 / 0.005 = 5.2
errorRollPitch [ 0 ] = constrain ( errorRollPitch [ 0 ] , - drift_limit , drift_limit ) ;
errorRollPitch [ 1 ] = constrain ( errorRollPitch [ 1 ] , - drift_limit , drift_limit ) ;
// 0.65*0.04 / 0.005 = 5.2
float drift_limit = _mpu6000 - > get_gyro_drift_rate ( ) * deltat
/ _gyro_bias_from_gravity_gain ;
# define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
errorRollPitch [ 0 ] = constrain ( errorRollPitch [ 0 ] ,
- drift_limit , drift_limit ) ;
errorRollPitch [ 1 ] = constrain ( errorRollPitch [ 1 ] ,
- drift_limit , drift_limit ) ;
// We correct gyroX and gyroY bias using the error vector
_gyro_bias [ 0 ] + = errorRollPitch [ 0 ] * _gyro_bias_from_gravity_gain ;
_gyro_bias [ 1 ] + = errorRollPitch [ 1 ] * _gyro_bias_from_gravity_gain ;
// TO-DO: fix this. Currently it makes the roll and pitch drift more!
// If bias values are greater than 1 LSB we update the hardware offset registers
// If bias values are greater than 1 LSB we update the hardware offset
// registers
if ( fabs ( _gyro_bias [ 0 ] ) > 1.0 ) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
//_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that we have already corrected on registers...
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that
// we have already corrected on registers...
}
if ( fabs ( _gyro_bias [ 1 ] ) > 1.0 ) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
@ -167,14 +178,16 @@ AP_AHRS_MPU6000::reset(bool recover_eulers)
@@ -167,14 +178,16 @@ AP_AHRS_MPU6000::reset(bool recover_eulers)
}
}
// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
// push offsets down from IMU to INS (required so MPU6000 can perform it's own
// attitude estimation)
void
AP_AHRS_MPU6000 : : push_offsets_to_ins ( )
{
// push down gyro offsets (TO-DO: why are x and y offsets are reversed?!)
_mpu6000 - > push_gyro_offsets_to_dmp ( ) ;
// push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!)
// push down accelerometer offsets
// (TO-DO: why are x and y offsets are reversed?!)
_mpu6000 - > push_accel_offsets_to_dmp ( ) ;
}
@ -220,18 +233,23 @@ AP_AHRS_MPU6000::yaw_error_compass(void)
@@ -220,18 +233,23 @@ AP_AHRS_MPU6000::yaw_error_compass(void)
//
// drift_correction_yaw - yaw drift correction using the compass
// we have no way to update the dmp with it's actual heading from our compass
// instead we use the yaw_corrected variable to hold what we think is the real heading
// we also record what the dmp said it's last heading was in the yaw_last_uncorrected variable so that on the next iteration we can add the change in yaw to our estimate
// we have no way to update the dmp with it's actual heading from our
// compass instead we use the yaw_corrected variable to hold what we think
// is the real heading we also record what the dmp said it's last heading
// was in the yaw_last_uncorrected variable so that on the next iteration we
// can add the change in yaw to our estimate
//
void
AP_AHRS_MPU6000 : : drift_correction_yaw ( void )
{
static float yaw_corrected = HEADING_UNKNOWN ;
static float last_dmp_yaw = HEADING_UNKNOWN ;
float dmp_roll , dmp_pitch , dmp_yaw ; // roll pitch and yaw values from dmp
float yaw_delta ; // change in yaw according to dmp
float yaw_error ; // difference between heading and corrected yaw
// roll pitch and yaw values from dmp
float dmp_roll , dmp_pitch , dmp_yaw ;
// change in yaw according to dmp
float yaw_delta ;
// difference between heading and corrected yaw
float yaw_error ;
static float heading ;
// get uncorrected yaw values from dmp
@ -282,7 +300,8 @@ void
@@ -282,7 +300,8 @@ void
AP_AHRS_MPU6000 : : euler_angles ( void )
{
_dcm_matrix . to_euler ( & roll , & pitch , & yaw ) ;
//quaternion.to_euler(&roll, &pitch, &yaw); // cannot use this because the quaternion is not correct for yaw drift
// cannot use this because the quaternion is not correct for yaw drift
//quaternion.to_euler(&roll, &pitch, &yaw);
roll_sensor = degrees ( roll ) * 100 ;
pitch_sensor = degrees ( pitch ) * 100 ;
Pat Hickey
13 years ago
committed by
Andrew Tridgell