Took Kris Winer's code and added some functions for the MPU6050
Fork of MPU6050 by
Diff: MPU6050.cpp
- Revision:
- 2:19e22a4eaa18
- Parent:
- 0:662207e34fba
- Child:
- 3:6624faa750c7
--- a/MPU6050.cpp Wed May 08 00:34:55 2013 +0000 +++ b/MPU6050.cpp Wed Feb 11 20:15:11 2015 +0000 @@ -54,6 +54,15 @@ */ MPU6050::MPU6050() : debugSerial(USBTX, USBRX) { + first_update = 0; + last_update = 0; + cur_time = 0; + GyroMeasError = PI * (60.0f / 180.0f); + beta = sqrt(3.0f / 4.0f) * GyroMeasError; + GyroMeasDrift = PI * (1.0f / 180.0f); + zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; + timer.start(); + devAddr = MPU6050_DEFAULT_ADDRESS; } @@ -65,6 +74,15 @@ */ MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX) { + first_update = 0; + last_update = 0; + cur_time = 0; + GyroMeasError = PI * (60.0f / 180.0f); + beta = sqrt(3.0f / 4.0f) * GyroMeasError; + GyroMeasDrift = PI * (1.0f / 180.0f); + zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; + timer.start(); + devAddr = address; } @@ -79,17 +97,13 @@ { #ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize start\n"); + debugSerial.printf("MPU6050::initialize start\n\r"); #endif setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); setFullScaleAccelRange(MPU6050_ACCEL_FS_2); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! - -#ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize end\n"); -#endif } /** Verify the I2C connection. @@ -99,13 +113,238 @@ bool MPU6050::testConnection() { #ifdef useDebugSerial - debugSerial.printf("MPU6050::testConnection start\n"); + debugSerial.printf("MPU6050::testConnection start\n\r"); #endif uint8_t deviceId = getDeviceID(); #ifdef useDebugSerial - debugSerial.printf("DeviceId = %d\n",deviceId); + debugSerial.printf("DeviceId = 0x%x\n\r",deviceId); #endif - return deviceId == 0x34; + return deviceId == MPU6050_ADDRESS_AD0_LOW; +} + +/* Check if we need to calibrate the device + * Check the factory trim values and see + * if they are +/- 14 or less. The integer + * that is returned is a bitmap of the sensors + * Bit 5: Z-Gyro Fail + * 4: Y-Gyro Fail + * 3: X-Gyro Fail + * 2: Z-Accel Fail + * 1: Y-Accel Fail + * 0: X-Accel Fail + * If any of the sensors fail the self test + * then there will be a 1 at that bit + * + * The destination argument is used to give back + * the values of the self test. + */ +int MPU6050::selfTest(float* destination) { + float accel_var[3], gyro_var[3]; + long gyro_st[3], accel_st[3], gyro[3], accel[3]; + unsigned char accel_result, gyro_result; + + uint8_t result; + + // Get data without self test + get_st_biases(gyro, accel, 0); + get_st_biases(gyro_st, accel_st, 1); + + // Get the results + accel_result = accel_self_test(accel, accel_st, accel_var); + gyro_result = gyro_self_test(gyro, gyro_st, gyro_var); + + result = gyro_result << 3 | accel_result; + + destination[5] = gyro_var[0]; // X-GYRO + destination[4] = gyro_var[1]; // Y-GYRO + destination[3] = gyro_var[2]; // Z-GYRO + destination[2] = accel_var[0]; // X-ACCL + destination[1] = accel_var[1]; // Y-ACCL + destination[0] = accel_var[2]; // Z-ACCL + + return result; +} + +/* This function gets gyro and accel data + * If hw_test then it will get the self-test values + */ +void MPU6050::get_st_biases(long *gyro, long *accel, unsigned char hw_test) +{ + unsigned char data[12]; + unsigned char packet_count, i; + unsigned short fifo_count; + + // Set the power management registers + data[0] = 0x01; // Use x-gyro reference + data[1] = 0x00; + i2Cdev.writeByte(devAddr, MPU6050_RA_PWR_MGMT_1, data[0]); + i2Cdev.writeByte(devAddr, MPU6050_RA_PWR_MGMT_2, data[1]); + wait(0.2); // Let the PLL lock + + data[0] = 0x00; + // Disable interrupts + i2Cdev.writeByte(devAddr, MPU6050_RA_INT_ENABLE, data[0]); + // Disable fifo + i2Cdev.writeByte(devAddr, MPU6050_RA_FIFO_EN, data[0]); + // -----Set clock to 8 MHz internal? why? + i2Cdev.writeByte(devAddr, MPU6050_RA_PWR_MGMT_1, data[0]); + // Disable i2c master + i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_MST_CTRL, data[0]); + // Disable the stuff in user control + i2Cdev.writeByte(devAddr, MPU6050_RA_USER_CTRL, data[0]); + + // reset fifo and dmp + data[0] = MPU6050_USERCTRL_DMP_RESET_BIT | MPU6050_USERCTRL_FIFO_RESET_BIT; + i2Cdev.writeByte(devAddr, MPU6050_RA_USER_CTRL, data[0]); + wait(0.015); + + // Set lpf + i2Cdev.writeByte(devAddr, MPU6050_RA_CONFIG, MPU6050_DLPF_BW_188); + + // Set rate_div + i2Cdev.writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, 0x00); + + // Configure the Accelerometer and Gyroscope registers + if (hw_test) { + // Configure the accelerometer and gyroscope for self-test + i2Cdev.writeByte(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_SELF_TEST_BYTE); + i2Cdev.writeByte(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_SELF_TEST_BYTE); + wait(.2); + } + else { + // Configure the accelerometer and gyroscope, not for self-test + i2Cdev.writeByte(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_BYTE); + i2Cdev.writeByte(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_BYTE); + } + + // Fill FIFO for 50 ms, and get the count + i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 0x1); + i2Cdev.writeByte(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ENABLE_ALL_FIFO); + wait(.05); + i2Cdev.writeByte(devAddr, MPU6050_RA_FIFO_EN, MPU6050_DISABLE_ALL_FIFO); + i2Cdev.readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, data); + + // Get the number of packets + // A packet is 12 bytes: xAcc, yAcc, zAcc, xGyro, yGyro, zGyro + // Each has two bytes for high and low + fifo_count = (data[0] << 8) | data[1]; // Fix the data count high, low + packet_count = fifo_count / 12; // Get the number of packets (12 bytes per packet) + gyro[0] = gyro[1] = gyro[2] = 0; + accel[0] = accel[1] = accel[2] = 0; + + for (i = 0; i < packet_count; i++) { + short accel_cur[3], gyro_cur[3]; + + // Get a packet of data + i2Cdev.readBytes(devAddr, MPU6050_RA_FIFO_R_W, 12, data); + + accel_cur[0] = ((short) data[0] << 8) | data[1]; + accel_cur[1] = ((short) data[2] << 8) | data[3]; + accel_cur[2] = ((short) data[4] << 8) | data[5]; + + accel[0] += (long) accel_cur[0]; + accel[1] += (long) accel_cur[1]; + accel[2] += (long) accel_cur[2]; + + gyro_cur[0] = (((short) data[6] << 8) | data[7]); + gyro_cur[1] = (((short) data[8] << 8) | data[9]); + gyro_cur[2] = (((short) data[10] << 8) | data[11]); + + gyro[0] += (long) gyro_cur[0]; + gyro[1] += (long) gyro_cur[1]; + gyro[2] += (long) gyro_cur[2]; + } + + // Normalize the packets + gyro[0] = (long) (((long long) gyro[0] << 16) / MPU6050_GCONFIG_SENS / packet_count); + gyro[1] = (long) (((long long) gyro[1] << 16) / MPU6050_GCONFIG_SENS / packet_count); + gyro[2] = (long) (((long long) gyro[2] << 16) / MPU6050_GCONFIG_SENS / packet_count); + accel[0] = (long) (((long long) accel[0] << 16) / MPU6050_ACONFIG_SENS / packet_count); + accel[1] = (long) (((long long) accel[1] << 16) / MPU6050_ACONFIG_SENS / packet_count); + accel[2] = (long) (((long long) accel[2] << 16) / MPU6050_ACONFIG_SENS / packet_count); + + // Don't remove gravity + if (accel[2] > 0L) + accel[2] -= 65536L; + else + accel[2] += 65536L; +} + +/* Test the accelerometer biases + */ +int MPU6050::accel_self_test(long *bias_regular, long *bias_st, float *st_shift_var) +{ + int j, result = 0; + float st_shift[3], st_shift_cust; + + get_accel_prod_shift(st_shift); + + for (j = 0; j < 3; j++) { + st_shift_cust = labs(bias_regular[j] - bias_st[j]) / 65536.f; + if (st_shift[j]) { + st_shift_var[j] = st_shift_cust / st_shift[j] - 1.f; + if (abs(st_shift_var[j]) > MPU6050_ACONFIG_MAX_VAR) + result |= 1 << j; + } + else if ((st_shift_cust < MPU6050_ACONFIG_MIN_G) || + (st_shift_cust > MPU6050_ACONFIG_MAX_G)) + result |= 1 << j; + } + + return result; +} + +/* Get the values of the self test registers + * and calculate the factory trims from the + * values of these registers + */ +void MPU6050::get_accel_prod_shift(float *st_shift) +{ + unsigned char tmp[4], shift_code[3], i; + + // Get the self-test values + i2Cdev.readBytes(devAddr, MPU6050_RA_SELF_TEST_X, 4, tmp); + + shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); + shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); + shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); + + for (i = 0; i < 3; i++) { + if (shift_code[i] == 0) + st_shift[i] = 0.f; + else + st_shift[i] = (4096.0f*0.34f)*(pow((0.92f/0.34f), ((shift_code[0] - 1.0f)/30.0f))); + } +} + +int MPU6050::gyro_self_test(long *bias_regular, long *bias_st, float *st_shift_var) +{ + int j, result = 0; + unsigned char tmp[3]; + float st_shift, st_shift_cust; + + i2Cdev.readBytes(devAddr, MPU6050_RA_SELF_TEST_X, 3, tmp); + + tmp[0] &= 0x1F; + tmp[1] &= 0x1F; + tmp[2] &= 0x1F; + + for (j = 0; j < 3; j++) { + st_shift_cust = labs(bias_regular[j] - bias_st[j]) / 65536.f; + if (tmp[j]) { + st_shift = 3275.f / MPU6050_GCONFIG_SENS; + while (--tmp[j]) + st_shift *= 1.046f; + st_shift_var[j] = st_shift_cust / st_shift - 1.f; + if (fabs(st_shift_var[j]) > MPU6050_GCONFIG_MAX_VAR) + result |= 1 << j; + } + else if ((st_shift_cust < MPU6050_GCONFIG_MIN_DPS) || + (st_shift_cust < MPU6050_GCONFIG_MAX_DPS)) + result |= 1 << j; + } + + return result; } // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) @@ -170,6 +409,27 @@ i2Cdev.writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate); } +uint32_t MPU6050::getSampRate() +{ + uint8_t samp_div; + uint8_t dlpf_mode; + uint32_t samp_rate; + + // Get the sample rate divider and dlpf mode + i2Cdev.readByte(devAddr, MPU6050_RA_SMPLRT_DIV,buffer); + samp_div = buffer[0]; + + i2Cdev.readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer); + dlpf_mode = buffer[0]; + + if (dlpf_mode == 0 || dlpf_mode == 7) + samp_rate = 800000 / (1+samp_div); + else + samp_rate = 100000 / (1+samp_div); + + return samp_rate; +} + // CONFIG register /** Get external FSYNC configuration. @@ -261,6 +521,60 @@ // GYRO_CONFIG register +/** Get self-test enabled setting for gyroscope X axis. + * @return Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +bool MPU6050::getGyroXSelfTest() +{ + i2Cdev.readBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_XA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for gyroscope X axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +void MPU6050::setGyroXSelfTest(bool enabled) +{ + i2Cdev.writeBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_XA_ST_BIT, enabled); +} + +/** Get self-test enabled setting for gyroscope Y axis. + * @return Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +bool MPU6050::getGyroYSelfTest() +{ + i2Cdev.readBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_YA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for gyroscope Y axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +void MPU6050::setGyroYSelfTest(bool enabled) +{ + i2Cdev.writeBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_YA_ST_BIT, enabled); +} + +/** Get self-test enabled setting for gyroscope Z axis. + * @return Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +bool MPU6050::getGyroZSelfTest() +{ + i2Cdev.readBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_ZA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for gyroscope Y axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_GYRO_CONFIG + */ +void MPU6050::setGyroZSelfTest(bool enabled) +{ + i2Cdev.writeBit(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_ZA_ST_BIT, enabled); +} + /** Get full-scale gyroscope range. * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, * as described in the table below. @@ -1898,13 +2212,24 @@ */ void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + +#ifdef useDebugSerial + //debugSerial.printf("MPU6050::getMotion6 start\n\r"); +#endif + i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; *az = (((int16_t)buffer[4]) << 8) | buffer[5]; *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; + +#ifdef useDebugSerial + //debugSerial.printf("%d, %d, %d, %d, %d, %d\n\r", *ax, *ay, *az, *gx, *gy, *gz); +#endif + } /** Get 3-axis accelerometer readings. * These registers store the most recent accelerometer measurements. @@ -2558,6 +2883,15 @@ // PWR_MGMT_1 register +/** Get the PWR_MGMT_1 register contents. + * @see MPU6050_RA_PWR_MGMT_1 + */ +uint8_t MPU6050::getPwrMgmt1Reg() +{ + i2Cdev.readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_BIT, MPU6050_PWR1_LENGTH, buffer); + return buffer[0]; +} + /** Trigger a full device reset. * A small delay of ~50ms may be desirable after triggering a reset. * @see MPU6050_RA_PWR_MGMT_1 @@ -2949,6 +3283,236 @@ // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== +/* Gets the sensor data and + * uses functions to convert + * it into acceleration and + * yaw, pitch, and roll data + */ +void MPU6050::getAndConvertData(float *ax, float *ay, float *az, + float *yaw, float *pitch, float *roll, + float *accel_bias, float *gyro_bias) { + 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}; + + tmp_ax = (int16_t *) malloc(sizeof(*tmp_ax)); + tmp_ay = (int16_t *) malloc(sizeof(*tmp_ay)); + tmp_az = (int16_t *) malloc(sizeof(*tmp_az)); + tmp_gx = (int16_t *) malloc(sizeof(*tmp_gx)); + tmp_gy = (int16_t *) malloc(sizeof(*tmp_gy)); + tmp_gz = (int16_t *) malloc(sizeof(*tmp_gz)); + +#ifdef useDebugSerial + //debugSerial.printf("MPU6050::getAndConvertData start\n\r"); +#endif + + // Get the 6 values + this->getMotion6(tmp_ax, tmp_ay, tmp_az, tmp_gx, tmp_gy, tmp_gz); + + switch (this->getFullScaleAccelRange()) { + case 0: + ares = (float) 2/MPU6050_ADC_RANGE; + break; + case 1: + ares = (float) 4/MPU6050_ADC_RANGE; + break; + case 2: + ares = (float) 8/MPU6050_ADC_RANGE; + break; + case 3: + ares = (float) 16/MPU6050_ADC_RANGE; + } + + switch (this->getFullScaleGyroRange()) { + case 0: + gres = (float) 500/MPU6050_ADC_RANGE; + break; + case 1: + gres = (float) 1000/MPU6050_ADC_RANGE; + break; + case 2: + gres = (float) 2000/MPU6050_ADC_RANGE; + break; + case 3: + gres = (float) 4000/MPU6050_ADC_RANGE; + } + +#ifdef useDebugSerial + //debugSerial.printf("ares: %.10f gres: %.10f\n\r", ares, gres); + //debugSerial.printf("accel_bias[0]: %f\n\r", accel_bias[0]); +#endif + +#ifdef useDebugSerial + //debugSerial.printf("MPU6050::Got back from Motion6\n\r"); +#endif + +#ifdef useDebugSerial + //debugSerial.printf("%d %d %d %d %d %d\n\r", *tmp_ax, *tmp_ay, *tmp_az, *tmp_gx, *tmp_gy, *tmp_gz); +#endif + + // Add in the bias + *tmp_ax = *tmp_ax + accel_bias[0]; + *tmp_ay = *tmp_ay + accel_bias[1]; + *tmp_az = *tmp_az + accel_bias[2]; + + *tmp_gx = *tmp_gx + gyro_bias[0]; + *tmp_gy = *tmp_gy + gyro_bias[1]; + *tmp_gz = *tmp_gz + gyro_bias[2]; + +#ifdef useDebugSerial + //debugSerial.printf("%d %d %d %d %d %d\n\r", *tmp_ax, *tmp_ay, *tmp_az, *tmp_gx, *tmp_gy, *tmp_gz); +#endif + + // Generate the sensor values + *ax = (float) (*tmp_ax) * ares; // Get actual g values + *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; + +#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: %d, %d, %d, %d\n\r", q[0], q[1], q[2], q[3]); +#endif + + // Generate yaw, pitch, and roll + *yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); + *pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + *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; + + free(tmp_ax); + free(tmp_ay); + free(tmp_az); + free(tmp_gx); + free(tmp_gy); + free(tmp_gz); + +} + +/* Compute the quaternion + * ax, ay, and az will get normalized + * 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; + float deltat; + float tmp_ax = *ax, tmp_ay = *ay, tmp_az = *az; + + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // Short name local variable for readability + float norm; // Vector norm + float f1, f2, f3; // Objective function elements + float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // Objective function Jacobian elements + float qDot1, qDot2, qDot3, qDot4; + float hatDot1, hatDot2, hatDot3, hatDot4; + float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error + + float _halfq1 = 0.5f * q1; + float _halfq2 = 0.5f * q2; + float _halfq3 = 0.5f * q3; + float _halfq4 = 0.5f * q4; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + +#ifdef useDebugSerial + //debugSerial.printf("MPU6050::Got into Madgwick Quaternion\n\r"); +#endif + + cur_time = this->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"); +#endif + + // Normalize the accelerometer measurements + norm = sqrt((tmp_ax) * (tmp_ax) + (tmp_ay) * (tmp_ay) + (tmp_az) * (tmp_az)); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + tmp_ax *= norm; + tmp_ay *= norm; + tmp_az *= norm; + + // Compute the objective function and Jacobian + f1 = _2q2 * q4 - _2q1 * q3 - tmp_ax; + f2 = _2q1 * q2 + _2q3 * q4 - tmp_ay; + f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - tmp_az; + J_11or24 = _2q3; + J_12or23 = _2q4; + J_13or22 = _2q1; + J_14or21 = _2q2; + J_32 = 2.0f * J_14or21; + J_33 = 2.0f * J_11or24; + + // Compute the gradient (matrix multiplication) + hatDot1 = J_14or21 * f2 - J_11or24 * f1; + hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3; + hatDot3 = J_12or23 * f2 - J_33 * f3 - J_13or22 * f1; + hatDot4 = J_14or21 * f1 + J_11or24 * f2; + + // Normalize the gradient + norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 *hatDot4); + hatDot1 /= norm; + hatDot2 /= norm; + hatDot3 /= norm; + hatDot4 /= norm; + + // Compute the estimated Gyroscope biases + gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3; + gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2; + gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1; + + // Compute and remove gyroscope biases + gbiasx += gerrx * deltat * zeta; + gbiasy += gerry * deltat * zeta; + gbiasz += gerrz * deltat * zeta; + + // gx -= gbiasx; + // gy -= gbiasy; + // gz -= gbiasz; + + // 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; + qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx; + + // Compute then integrate estimated quaternion derivative + q1 += (qDot1 - (beta * hatDot1)) * deltat; + q2 += (qDot2 - (beta * hatDot2)) * deltat; + q3 += (qDot3 - (beta * hatDot3)) * deltat; + q4 += (qDot4 - (beta * hatDot4)) * deltat; + + // Normalize the quaternion + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + // XG_OFFS_TC register uint8_t MPU6050::getOTPBankValid() @@ -3250,8 +3814,8 @@ setMemoryBank(bank); setMemoryStartAddress(address); uint8_t chunkSize; - uint8_t *verifyBuffer; - uint8_t *progBuffer; + uint8_t *verifyBuffer = NULL; + uint8_t *progBuffer = NULL; uint16_t i; uint8_t j; if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); @@ -3286,19 +3850,19 @@ Serial.print(bank, DEC); Serial.print(", address "); Serial.print(address, DEC); - Serial.print("!\nExpected:"); + Serial.print("!\n\rExpected:"); for (j = 0; j < chunkSize; j++) { Serial.print(" 0x"); if (progBuffer[j] < 16) Serial.print("0"); Serial.print(progBuffer[j], HEX); } - Serial.print("\nReceived:"); + Serial.print("\n\rReceived:"); for (uint8_t j = 0; j < chunkSize; j++) { Serial.print(" 0x"); if (verifyBuffer[i + j] < 16) Serial.print("0"); Serial.print(verifyBuffer[i + j], HEX); } - Serial.print("\n");*/ + Serial.print("\n\r");*/ free(verifyBuffer); if (useProgMem) free(progBuffer); return false; // uh oh. @@ -3328,7 +3892,7 @@ } bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { - uint8_t *progBuffer, success, special; + uint8_t *progBuffer = NULL, success, special; uint16_t i, j; if (useProgMem) { progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary