Brenton Yiu
Took Kris Winer's code and added some functions for the MPU6050
Fork of
MPU6050
by 
Simon Garfieldsg
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