From 0820ebddb95419b46d5ed58d8b12591b7c1ca455 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 28 Jul 2012 14:17:38 +0900 Subject: [PATCH] AP_AHRS_MPU6000: first draft implementation of class that uses MPU6000's DMP to calculate attitude --- libraries/AP_AHRS/AP_AHRS_MPU6000.cpp | 321 ++++++++++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_MPU6000.h | 108 +++++++++ 2 files changed, 429 insertions(+) create mode 100644 libraries/AP_AHRS/AP_AHRS_MPU6000.cpp create mode 100644 libraries/AP_AHRS/AP_AHRS_MPU6000.h diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp new file mode 100644 index 0000000000..3da3a6bf3b --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -0,0 +1,321 @@ +/* + 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 + +// this is the speed in cm/s above which we first get a yaw lock with +// the GPS +#define GPS_SPEED_MIN 300 + +// this is the speed in cm/s at which we stop using drift correction +// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN +#define GPS_SPEED_RESET 100 + +// the limit (in degrees/second) beyond which we stop integrating +// omega_I. At larger spin rates the DCM PI controller can get 'dizzy' +// which results in false gyro drift. See +// http://gentlenav.googlecode.com/files/fastRotations.pdf +#define SPIN_RATE_LIMIT 20 + +// table of user settable parameters +const AP_Param::GroupInfo AP_AHRS_MPU6000::var_info[] PROGMEM = { + // @Param: AHRS_YAW_P + // @DisplayName: Yaw P + // @Description: This controls the weight the compass has on the overall heading + // @Range: 0 .4 + // @Increment: .01 + AP_GROUPINFO("YAW_P", 0, AP_AHRS_MPU6000, _kp_yaw), + + // @Param: AHRS_RP_P + // @DisplayName: AHRS RP_P + // @Description: This controls how fast the accelerometers correct the attitude + // @Range: 0 .4 + // @Increment: .01 + AP_GROUPINFO("RP_P", 1, AP_AHRS_MPU6000, _kp), + AP_GROUPEND +}; + +void +AP_AHRS_MPU6000::init() +{ + _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; + + // 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(); + + // 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); + + // 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); + } +} + +// 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 ) +{ + float errorRollPitch[2]; + + // 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++; + + _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 + + // 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; + + // 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]; + } + + // 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 + */ +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); + } +} + +// 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 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()); + } + + _mpu6000->dmp_set_sensor_fusion_accel_gain(gain); +} + +// produce a yaw error value. The returned value is proportional +// to sin() of the current heading error in earth frame +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; + + 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); + + // calculate the error term in earth frame + Vector3f error = rb % mag_earth; + + return error.z; +} + +// +// drift_correction_yaw - yaw drift correction using the compass +// +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; + float yaw_deltat; + 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; + } + + // 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) { + yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; + _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); + + // 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 + + // 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 +// navigation control +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 + + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; + yaw_sensor = degrees(yaw) * 100; + + if (yaw_sensor < 0) + yaw_sensor += 36000; +} + +/* reporting of DCM state for MAVLink */ + +// average error_roll_pitch since last call +float AP_AHRS_MPU6000::get_error_rp(void) +{ + // 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; +} diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.h b/libraries/AP_AHRS/AP_AHRS_MPU6000.h new file mode 100644 index 0000000000..f40bfb19cb --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.h @@ -0,0 +1,108 @@ +#ifndef AP_AHRS_MPU6000_H +#define AP_AHRS_MPU6000_H +/* + DCM based AHRS (Attitude Heading Reference System) interface for + ArduPilot + + 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. +*/ + +#define GYRO_BIAS_FROM_GRAVITY_RATE 4 // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz +#define HEADING_UNKNOWN -9999 // Initial value to detect that compass correction is not initialized + +// max rate of gyro drift in gyro_LSB_units/s (16.4LSB = 1deg/s) +static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 degrees/s/s; + +class AP_AHRS_MPU6000 : public AP_AHRS +{ +public: + // Constructors + AP_AHRS_MPU6000(AP_IMU_INS *imu, GPS *&gps, AP_InertialSensor_MPU6000 *mpu6000) : AP_AHRS(imu, gps), _mpu6000(mpu6000) + { + _dcm_matrix.identity(); + + // these are experimentally derived from the simulator + // with large drift levels + _ki = 0.0087; + _ki_yaw = 0.01; + + _kp.set(0.4); + _kp_yaw.set(0.4); + + // dmp related variable initialisation + _gyro_bias_from_gravity_gain = 0.008; + _compass_bias_time = 0; + } + + // initialisation routine to start MPU6000's dmp + void init(); + + // return the smoothed gyro vector corrected for drift + Vector3f get_gyro(void) {return _imu->get_gyro(); } + Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } + + // return the current drift correction integrator value + Vector3f get_gyro_drift(void) {return _omega_I; } + + // Methods + void update(void); + void reset(bool recover_eulers = false); + + // push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation) + void push_offsets_to_ins(); + void push_gains_to_dmp(); + + // status reporting + float get_error_rp(void); + float get_error_yaw(void); + + // settable parameters + AP_Float _kp_yaw; + AP_Float _kp; + AP_Float gps_gain; // not currently supported + + // for holding parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + float _ki; + float _ki_yaw; + bool _have_initial_yaw; + AP_InertialSensor_MPU6000 *_mpu6000; + + // Methods + void drift_correction(float deltat); + + // Compass correction variables. TO-DO: move or replace? + float wrap_PI(float angle_in_radians); // TO-DO: move this to standard AP_AHRS methods + long _compass_bias_time; + + void drift_correction_yaw(void); + float yaw_error_compass(); + void euler_angles(void); + + Vector3f _accel_filtered; + int16_t _accel_filtered_samples; + float _gyro_bias[3]; // bias_tracking + float _gyro_bias_from_gravity_gain; // bias correction algorithm gain + uint8_t _gyro_bias_from_gravity_counter; + + // primary representation of attitude + Matrix3f _dcm_matrix; + + Vector3f _accel_vector; // current accel vector + + Vector3f _omega_I; // Omega Integrator correction + Vector3f _omega_I_sum; + float _omega_I_sum_time; + + // state to support status reporting + float _error_yaw_sum; + uint16_t _error_yaw_count; + float _error_yaw_last; +}; + +#endif // AP_AHRS_MPU6000_H