Gurvan PRIEM
/
RaptorControl
An incomplete quadcopter control programme.
MPU6050/GurvIMU.cpp
- Committer:
- Gurvan
- Date:
- 2013-07-17
- Revision:
- 0:9cb9445a11f0
File content as of revision 0:9cb9445a11f0:
#include "GurvIMU.h" #include "MPU6050.h" #include "mbed.h" #define M_PI 3.1415926535897932384626433832795 #define twoKpDef (2.0f * 1.0f) // 2 * proportional gain #define twoKiDef (2.0f * 0.0f) // 2 * integral gain GurvIMU::GurvIMU() { //MPU mpu = MPU6050(0x69); //0x69 = MPU6050 I2C ADDRESS // Variable definitions q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame twoKp = twoKpDef; // 2 * proportional gain (Kp) twoKi = twoKiDef; // 2 * integral gain (Ki) integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki cycle_nb = 0; timer_us.start(); } //Function definitions void GurvIMU::getValues(float * values) { int16_t accgyroval[6]; mpu.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]); for(int i = 0; i<3; i++) values[i] = (float) accgyroval[i]; for(int i = 3; i<6; i++) values[i] = (accgyroval[i]-offset[i]) * (M_PI / 180) / 16.4f; } void GurvIMU::getVerticalAcceleration(float av) { float values[6]; float q[4]; // quaternion float g_x, g_y, g_z; // estimated gravity direction getQ(q); g_x = 2 * (q[1]*q[3] - q[0]*q[2]); g_y = 2 * (q[0]*q[1] + q[2]*q[3]); g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; getValues(values); av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2]; } void GurvIMU::getOffset(void) { int sample_nb = 50; float values[6]; for(int i=0; i<6 ; i++) offset[i] = 0; for(int i=0; i<sample_nb; i++) { getValues(values); for(int j=0; j<6; j++) offset[j]+=values[j]; } for(int j=0; j<6; j++) offset[j]/=sample_nb; } void GurvIMU::AHRS_update(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; dt_us=timer_us.read_us(); sample_freq = 1.0 / ((dt_us) / 1000000.0); timer_us.reset(); // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated and measured direction of gravity halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * (1.0f / sample_freq); // integral error scaled by Ki integralFBy += twoKi * halfey * (1.0f / sample_freq); integralFBz += twoKi * halfez * (1.0f / sample_freq); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * (1.0f / sample_freq)); // pre-multiply common factors gy *= (0.5f * (1.0f / sample_freq)); gz *= (0.5f * (1.0f / sample_freq)); qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; } void GurvIMU::getQ(float * q) { float val[6]; getValues(val); //while(cycle_nb < 1000){ AHRS_update(val[3], val[4], val[5], val[0], val[1], val[2]); //cycle_nb++;} q[0] = q0; q[1] = q1; q[2] = q2; q[3] = q3; } void GurvIMU::getYawPitchRollRad(float * ypr) { float q[4]; // quaternion float g_x, g_y, g_z; // estimated gravity direction getQ(q); g_x = 2 * (q[1]*q[3] - q[0]*q[2]); g_y = 2 * (q[0]*q[1] + q[2]*q[3]); g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); ypr[1] = atan(g_x * invSqrt(g_y*g_y + g_z*g_z)); ypr[2] = atan(g_y * invSqrt(g_x*g_x + g_z*g_z)); } void GurvIMU::init() { mpu.initialize(); mpu.setI2CMasterModeEnabled(0); mpu.setI2CBypassEnabled(0); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); getOffset(); wait(0.005); } float invSqrt(float number) { volatile long i; volatile float x, y; volatile const float f = 1.5F; x = number * 0.5F; y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); y = * ( float * ) &i; y = y * ( f - ( x * y * y ) ); return y; }