Took Kris Winer's code and added some functions for the MPU6050
Fork of MPU6050 by
Diff: MPU6050.cpp
- Revision:
- 3:6624faa750c7
- Parent:
- 2:19e22a4eaa18
--- a/MPU6050.cpp Wed Feb 11 20:15:11 2015 +0000 +++ b/MPU6050.cpp Sat Mar 14 15:44:48 2015 +0000 @@ -54,15 +54,19 @@ */ MPU6050::MPU6050() : debugSerial(USBTX, USBRX) { + //i2Cdev.frequency(400000); first_update = 0; last_update = 0; cur_time = 0; - GyroMeasError = PI * (60.0f / 180.0f); + GyroMeasError = PI * (40.0f / 180.0f); beta = sqrt(3.0f / 4.0f) * GyroMeasError; - GyroMeasDrift = PI * (1.0f / 180.0f); + GyroMeasDrift = PI * (2.0f / 180.0f); zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; timer.start(); - + q[0] = 1.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 0.0f; devAddr = MPU6050_DEFAULT_ADDRESS; } @@ -81,6 +85,10 @@ beta = sqrt(3.0f / 4.0f) * GyroMeasError; GyroMeasDrift = PI * (1.0f / 180.0f); zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; + q[0] = 1.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 0.0f; timer.start(); devAddr = address; @@ -3290,9 +3298,10 @@ */ void MPU6050::getAndConvertData(float *ax, float *ay, float *az, float *yaw, float *pitch, float *roll, - float *accel_bias, float *gyro_bias) { + float *accel_bias, float *gyro_bias, + float *gx, float *gy, float *gz) { int16_t *tmp_ax, *tmp_ay, *tmp_az, *tmp_gx, *tmp_gy, *tmp_gz; - float ares = 0, gres = 0, g[3], q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; + float ares = 0, gres = 0, g[3]; tmp_ax = (int16_t *) malloc(sizeof(*tmp_ax)); tmp_ay = (int16_t *) malloc(sizeof(*tmp_ay)); @@ -3324,16 +3333,16 @@ switch (this->getFullScaleGyroRange()) { case 0: + gres = (float) 250/MPU6050_ADC_RANGE; + break; + case 1: gres = (float) 500/MPU6050_ADC_RANGE; break; - case 1: + case 2: gres = (float) 1000/MPU6050_ADC_RANGE; break; - case 2: + case 3: gres = (float) 2000/MPU6050_ADC_RANGE; - break; - case 3: - gres = (float) 4000/MPU6050_ADC_RANGE; } #ifdef useDebugSerial @@ -3367,19 +3376,23 @@ *ay = (float) (*tmp_ay) * ares; *az = (float) (*tmp_az) * ares; - g[0] = (float) (*tmp_gx)*gres; // Get actual rotation values - g[1] = (float) (*tmp_gy)*gres; - g[2] = (float) (*tmp_gz)*gres; + *gx = g[0] = (float) (*tmp_gx)*gres; // Get actual rotation values + *gy = g[1] = (float) (*tmp_gy)*gres; + *gz = g[2] = (float) (*tmp_gz)*gres; #ifdef useDebugSerial //debugSerial.printf("%f %f %f %f %f %f\n\r", *ax, *ay, *az, g[0], g[1], g[2]); #endif - // Function to generate quaternion - this->MadgwickQuaternionUpdate(ax, ay, az, g[0]*PI/180.0f, g[1]*PI/180.0f, g[2]*PI/180.0f, q); +#ifdef useDebugSerial + //debugSerial.printf("Madgwick: %f, %f, %f, %f\n\r", q[0], q[1], q[2], q[3]); +#endif + // Function to generate quaternion + this->MadgwickQuaternionUpdate(ax, ay, az, g[0]*PI/180.0f, g[1]*PI/180.0f, g[2]*PI/180.0f); + #ifdef useDebugSerial - //debugSerial.printf("Madgwick: %d, %d, %d, %d\n\r", q[0], q[1], q[2], q[3]); + //debugSerial.printf("Madgwick1: %f, %f, %f, %f\n\r", q[0], q[1], q[2], q[3]); #endif // Generate yaw, pitch, and roll @@ -3388,9 +3401,9 @@ *roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); // Change to degrees - //pitch *= 180.0f / PI; - //yaw *= 180.0f / PI; - //roll *= 180.0f / PI; + *pitch *= 180.0f / PI; + *yaw *= 180.0f / PI; + *roll *= 180.0f / PI; free(tmp_ax); free(tmp_ay); @@ -3406,9 +3419,8 @@ * gx, gy, gz won't change * q[0], q[1], q[2], q[3] will get populated */ -void MPU6050::MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz, float *q) { - - int cur_time; +void MPU6050::MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz) { + float deltat; float tmp_ax = *ax, tmp_ay = *ay, tmp_az = *az; @@ -3433,14 +3445,9 @@ //debugSerial.printf("MPU6050::Got into Madgwick Quaternion\n\r"); #endif - cur_time = this->timer.read_us(); + cur_time = timer.read_us(); deltat = (float) ((cur_time - last_update)/1000000.0f); // Integration time by time elapsed since last filter update last_update = cur_time; - - if (last_update - first_update > 10000000.0f) { - beta = 0.04; // Decrease filter gain after stabilized - zeta = 0.015; // Increase bias drift gain after stabilized - } #ifdef useDebugSerial //debugSerial.printf("cur_time: %d deltat: %f beta: %f zeta: %f\n\r"); @@ -3495,7 +3502,7 @@ // Compute the quaternion derivative qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz; qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy; - qDot3 = _halfq1 * gy + _halfq2 * gz + _halfq4 * gx; + qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx; qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx; // Compute then integrate estimated quaternion derivative