diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index aee7764c72..e3d58c6496 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -1,15 +1,15 @@ /* - AP_AHRS_MPU6000.cpp - - AHRS system using MPU6000's internal calculations - - Adapted for the general ArduPilot AHRS interface by Andrew Tridgell - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public License - as published by the Free Software Foundation; either version 2.1 - of the License, or (at your option) any later version. -*/ + * AP_AHRS_MPU6000.cpp + * + * AHRS system using MPU6000's internal calculations + * + * Adapted for the general ArduPilot AHRS interface by Andrew Tridgell + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation; either version 2.1 + * of the License, or (at your option) any later version. + */ #include #include @@ -30,46 +30,46 @@ void AP_AHRS_MPU6000::init() { - _mpu6000->dmp_init(); - push_gains_to_dmp(); - push_offsets_to_ins(); + _mpu6000->dmp_init(); + push_gains_to_dmp(); + push_offsets_to_ins(); }; // run a full MPU6000 update round void AP_AHRS_MPU6000::update(void) { - float delta_t; + float delta_t; - // tell the IMU to grab some data. is this necessary? - _imu->update(); + // tell the IMU to grab some data. is this necessary? + _imu->update(); - // ask the IMU how much time this sensor reading represents - delta_t = _imu->get_delta_time(); + // ask the IMU how much time this sensor reading represents + delta_t = _imu->get_delta_time(); - // convert the quaternions into a DCM matrix - _mpu6000->quaternion.rotation_matrix(_dcm_matrix); - - // we run the gyro bias correction using gravity vector algorithm - drift_correction(delta_t); + // convert the quaternions into a DCM matrix + _mpu6000->quaternion.rotation_matrix(_dcm_matrix); - // Calculate pitch, roll, yaw for stabilization and navigation - euler_angles(); + // we run the gyro bias correction using gravity vector algorithm + drift_correction(delta_t); + + // Calculate pitch, roll, yaw for stabilization and navigation + euler_angles(); } // wrap_PI - ensure an angle (expressed in radians) is between -PI and PI // TO-DO: should remove and replace with more standard functions float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians) { - if( angle_in_radians > M_PI ){ - return(angle_in_radians - 2*M_PI); - } - else if( angle_in_radians < -M_PI ){ - return(angle_in_radians + 2*M_PI); - } - else{ - return(angle_in_radians); - } + if( angle_in_radians > M_PI ) { + return(angle_in_radians - 2*M_PI); + } + else if( angle_in_radians < -M_PI ) { + return(angle_in_radians + 2*M_PI); + } + else{ + return(angle_in_radians); + } } // Function to correct the gyroX and gyroY bias (roll and pitch) using the gravity vector from accelerometers @@ -77,111 +77,111 @@ float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians) // are in chip axis definition void AP_AHRS_MPU6000::drift_correction( float deltat ) { - float errorRollPitch[2]; + float errorRollPitch[2]; - // Get current values for gyros - _accel_vector = _imu->get_accel(); + // Get current values for gyros + _accel_vector = _imu->get_accel(); - // We take the accelerometer readings and cumulate to average them and obtain the gravity vector - _accel_filtered += _accel_vector; - _accel_filtered_samples++; + // 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) - if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ){ - _gyro_bias_from_gravity_counter = 0; + _gyro_bias_from_gravity_counter++; + // 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 + _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 - // 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; + // 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] *= deltat * 1000; - errorRollPitch[1] *= deltat * 1000; + 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); + // 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); - // 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( 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... - } - if (fabs(_gyro_bias[1])>1.0){ - //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0); - //_gyro_bias[1] -= (int)_gyro_bias[1]; - } + // 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; - // Reset the accelerometer variables - _accel_filtered.x = 0; - _accel_filtered.y = 0; - _accel_filtered.z = 0; - _accel_filtered_samples=0; - } + // 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( 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... + } + if (fabs(_gyro_bias[1])>1.0) { + //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0); + //_gyro_bias[1] -= (int)_gyro_bias[1]; + } - // correct the yaw - drift_correction_yaw(); + // Reset the accelerometer variables + _accel_filtered.x = 0; + _accel_filtered.y = 0; + _accel_filtered.z = 0; + _accel_filtered_samples=0; + } + + // correct the yaw + drift_correction_yaw(); } /* - reset the DCM matrix and omega. Used on ground start, and on - extreme errors in the matrix + * reset the DCM matrix and omega. Used on ground start, and on + * extreme errors in the matrix */ void AP_AHRS_MPU6000::reset(bool recover_eulers) { - // if the caller wants us to try to recover to the current - // attitude then calculate the dcm matrix from the current - // roll/pitch/yaw values - if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) { - _dcm_matrix.from_euler(roll, pitch, yaw); - } else { - // otherwise make it flat - _dcm_matrix.from_euler(0, 0, 0); - } + // if the caller wants us to try to recover to the current + // attitude then calculate the dcm matrix from the current + // roll/pitch/yaw values + if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) { + _dcm_matrix.from_euler(roll, pitch, yaw); + } else { + // otherwise make it flat + _dcm_matrix.from_euler(0, 0, 0); + } } // 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->set_gyro_offsets_scaled(((AP_IMU_INS*)_imu)->gy(),((AP_IMU_INS*)_imu)->gx(),((AP_IMU_INS*)_imu)->gz()); - ((AP_IMU_INS*)_imu)->gx(0); - ((AP_IMU_INS*)_imu)->gy(0); - ((AP_IMU_INS*)_imu)->gz(0); + // push down gyro offsets (TO-DO: why are x and y offsets are reversed?!) + _mpu6000->set_gyro_offsets_scaled(((AP_IMU_INS*)_imu)->gy(),((AP_IMU_INS*)_imu)->gx(),((AP_IMU_INS*)_imu)->gz()); + ((AP_IMU_INS*)_imu)->gx(0); + ((AP_IMU_INS*)_imu)->gy(0); + ((AP_IMU_INS*)_imu)->gz(0); - // push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!) - _mpu6000->set_accel_offsets_scaled(((AP_IMU_INS*)_imu)->ay(), ((AP_IMU_INS*)_imu)->ax(), ((AP_IMU_INS*)_imu)->az()); - //((AP_IMU_INS*)_imu)->ax(0); - //((AP_IMU_INS*)_imu)->ay(0); - //((AP_IMU_INS*)_imu)->az(0); + // push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!) + _mpu6000->set_accel_offsets_scaled(((AP_IMU_INS*)_imu)->ay(), ((AP_IMU_INS*)_imu)->ax(), ((AP_IMU_INS*)_imu)->az()); + //((AP_IMU_INS*)_imu)->ax(0); + //((AP_IMU_INS*)_imu)->ay(0); + //((AP_IMU_INS*)_imu)->az(0); } void AP_AHRS_MPU6000::push_gains_to_dmp() { - uint8_t gain; - if( _kp.get() >= 1.0 ) { - gain = 0xFF; - }else if( _kp.get() <= 0.0 ) { - gain = 0x00; - }else{ - gain = (uint8_t)((float)0xFF * _kp.get()); - } + uint8_t gain; + if( _kp.get() >= 1.0 ) { + gain = 0xFF; + }else if( _kp.get() <= 0.0 ) { + gain = 0x00; + }else{ + gain = (uint8_t)((float)0xFF * _kp.get()); + } - _mpu6000->dmp_set_sensor_fusion_accel_gain(gain); + _mpu6000->dmp_set_sensor_fusion_accel_gain(gain); } // produce a yaw error value. The returned value is proportional @@ -189,24 +189,24 @@ AP_AHRS_MPU6000::push_gains_to_dmp() float AP_AHRS_MPU6000::yaw_error_compass(void) { - Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); - // get the mag vector in the earth frame - Vector3f rb = _dcm_matrix * mag; + Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); + // get the mag vector in the earth frame + Vector3f rb = _dcm_matrix * mag; - rb.normalize(); - if (rb.is_inf()) { - // not a valid vector - return 0.0; - } + rb.normalize(); + if (rb.is_inf()) { + // not a valid vector + return 0.0; + } - // get the earths magnetic field (only X and Y components needed) - Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), - sin(_compass->get_declination()), 0); + // get the earths magnetic field (only X and Y components needed) + Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), + sin(_compass->get_declination()), 0); - // calculate the error term in earth frame - Vector3f error = rb % mag_earth; + // calculate the error term in earth frame + Vector3f error = rb % mag_earth; - return error.z; + return error.z; } // @@ -215,57 +215,57 @@ AP_AHRS_MPU6000::yaw_error_compass(void) void AP_AHRS_MPU6000::drift_correction_yaw(void) { - static float yaw_last_uncorrected = HEADING_UNKNOWN; - static float yaw_corrected = HEADING_UNKNOWN; - float yaw_delta = 0; - bool new_value = false; - float yaw_error; - static float heading; + static float yaw_last_uncorrected = HEADING_UNKNOWN; + static float yaw_corrected = HEADING_UNKNOWN; + float yaw_delta = 0; + bool new_value = false; + float yaw_error; + static float heading; - // we assume the DCM matrix is completely uncorrect for yaw - // retrieve how much heading has changed according to non-corrected dcm - if( yaw_last_uncorrected != HEADING_UNKNOWN ) { - yaw_delta = wrap_PI(yaw - yaw_last_uncorrected); // the change in heading according to the gyros only since the last interation - yaw_last_uncorrected = yaw; - } + // we assume the DCM matrix is completely uncorrect for yaw + // retrieve how much heading has changed according to non-corrected dcm + if( yaw_last_uncorrected != HEADING_UNKNOWN ) { + yaw_delta = wrap_PI(yaw - yaw_last_uncorrected); // the change in heading according to the gyros only since the last interation + yaw_last_uncorrected = yaw; + } - // initialise yaw_corrected (if necessary) - if( yaw_corrected != HEADING_UNKNOWN ) { - yaw_corrected = yaw; - }else{ - yaw_corrected = wrap_PI(yaw_corrected + yaw_delta); // best guess of current yaw is previous iterations corrected yaw + yaw change from gyro - _dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw - } + // initialise yaw_corrected (if necessary) + if( yaw_corrected != HEADING_UNKNOWN ) { + yaw_corrected = yaw; + }else{ + yaw_corrected = wrap_PI(yaw_corrected + yaw_delta); // best guess of current yaw is previous iterations corrected yaw + yaw change from gyro + _dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw + } - // if we have new compass data - if( _compass && _compass->use_for_yaw() ) { - if (_compass->last_update != _compass_last_update) { - _compass_last_update = _compass->last_update; - heading = _compass->calculate_heading(_dcm_matrix); - if( !_have_initial_yaw ) { - yaw_corrected = heading; - _have_initial_yaw = true; - _dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw - } - new_value = true; - } - } + // if we have new compass data + if( _compass && _compass->use_for_yaw() ) { + if (_compass->last_update != _compass_last_update) { + _compass_last_update = _compass->last_update; + heading = _compass->calculate_heading(_dcm_matrix); + if( !_have_initial_yaw ) { + yaw_corrected = heading; + _have_initial_yaw = true; + _dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw + } + new_value = true; + } + } - // perform the yaw correction - if( new_value ){ - yaw_error = yaw_error_compass(); - // the yaw error is a vector in earth frame - //Vector3f error = Vector3f(0,0, yaw_error); + // perform the yaw correction + if( new_value ) { + yaw_error = yaw_error_compass(); + // the yaw error is a vector in earth frame + //Vector3f error = Vector3f(0,0, yaw_error); - // convert the error vector to body frame - //error = _dcm_matrix.mul_transpose(error); + // convert the error vector to body frame + //error = _dcm_matrix.mul_transpose(error); - // Update the differential component to reduce the error (itīs like a P control) - yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get()); // probably completely wrong + // Update the differential component to reduce the error (itīs like a P control) + yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get()); // probably completely wrong - // rebuild the dcm matrix yet again - _dcm_matrix.from_euler(roll, pitch, yaw_corrected); - } + // rebuild the dcm matrix yet again + _dcm_matrix.from_euler(roll, pitch, yaw_corrected); + } } // calculate the euler angles which will be used for high level @@ -273,15 +273,15 @@ AP_AHRS_MPU6000::drift_correction_yaw(void) 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 + _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 - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; - yaw_sensor = degrees(yaw) * 100; + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; + yaw_sensor = degrees(yaw) * 100; - if (yaw_sensor < 0) - yaw_sensor += 36000; + if (yaw_sensor < 0) + yaw_sensor += 36000; } /* reporting of DCM state for MAVLink */ @@ -289,13 +289,13 @@ AP_AHRS_MPU6000::euler_angles(void) // average error_roll_pitch since last call float AP_AHRS_MPU6000::get_error_rp(void) { - // not yet supported with DMP - return 0.0; + // not yet supported with DMP + return 0.0; } // average error_yaw since last call float AP_AHRS_MPU6000::get_error_yaw(void) { - // not yet supported with DMP - return 0.0; + // not yet supported with DMP + return 0.0; }