Library for MPU6050 motion sensor with Madgwick filter.
Dependents: ScilabArduino MPU6050_Hello
Diff: MPU6050.cpp
- Revision:
- 0:9c2bb0f94c31
diff -r 000000000000 -r 9c2bb0f94c31 MPU6050.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.cpp Mon Jan 18 19:44:43 2021 +0000 @@ -0,0 +1,826 @@ +#include "MPU6050.h" + +/** + * @brief Creates an MPU6050 object + * @note Default address is 0x68 + * Default acceleration range 2G + * Default gyroscope range is 250 dps (degrees per second) + * I2C_SDA and I2C_SCL pins are used for I2C connection by default + * Interrupt pin is not connected (NC) by default + * @param + * @retval + */ +MPU6050::MPU6050 +( + uint8_t addr /*= 0x68*/, + AccelScale accelScale /*= AFS_2G*/, + GyroScale gyroScale /*= GFS_250DPS*/, + PinName sdaPin /*= I2C_SDA*/, + PinName sclPin /*= I2C_SCL*/, + PinName interruptInPin /*= NC*/ +) : + _addr(addr << 1), + _accelScale(accelScale), + _gyroScale(gyroScale), + _i2c(sdaPin, sclPin), + _interruptIn(interruptInPin), + _accelRes(0), + _gyroRes(0), + _beta(sqrt(3.0f / 4.0f) * _gyroError), + _zeta(sqrt(3.0f / 4.0f) * _gyroDrift) +{ + _i2c.frequency(400000); // 400 kHz +} + +/** + * @brief + * @note + * @param + * @retval + */ +void MPU6050::rise(Callback<void (void)> func) +{ + _interruptIn.rise(func); +} + +/** + * @brief + * @note + * @param + * @retval + */ +template<typename T> +void MPU6050::rise(T* tptr, void (T:: *mptr) (void)) +{ + if ((mptr != NULL) && (tptr != NULL)) + _interruptIn.rise(tptr, &T::mptr); +} + +/** + * @brief + * @note + * @param + * @retval + */ +void MPU6050::fall(Callback<void (void)> func) +{ + _interruptIn.fall(func); +} + +/** + * @brief + * @note + * @param + * @retval + */ +template<typename T> +void MPU6050::fall(T* tptr, void (T:: *mptr) (void)) +{ + if ((mptr != NULL) && (tptr != NULL)) + _interruptIn.fall(tptr, &T::mptr); +} + +/** + * @brief Writes a byte to register + * @note + * @param + * @retval + */ +void MPU6050::writeReg(uint8_t reg, uint8_t byte) +{ + char data[2]; + + data[0] = reg; + data[1] = byte; + _i2c.write(_addr, data, sizeof(data), false); +} + +/** + * @brief Reads byte from a register + * @note + * @param + * @retval + */ +uint8_t MPU6050::readReg(uint8_t reg) +{ + char ret; // `ret` will store the register data + + _i2c.write(_addr, (const char*) ®, 1, 1); // no stop + _i2c.read(_addr, &ret, 1, 0); + return ret; +} + +/** + * @brief Read multiple bytes from a register + * @note + * @param + * @retval + */ +void MPU6050::readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest) +{ + _i2c.write(_addr, (const char*) ®, 1, 1); // no stop + _i2c.read(_addr, (char*)dest, len, 0); +} + +/** + * @brief Initializes the MPU6050 + * @note + * @param + * @retval + */ +bool MPU6050::init() +{ + uint8_t c; + + writeReg(PWR_MGMT_1, 0x00); // Select internal OSC as clock source, clear sleep mode bit (6), enable all sensors + + // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(100ms); +#else + wait_ms(100); +#endif + // + + // get stable time source + writeReg(PWR_MGMT_1, 0x01); // Select PLL with x-axis gyroscope reference as clock source, bits 2:0 = 001 + + // Configure Gyro and Accelerometer + // DLPF_CFG = bits 2:0 = 0x03 (0b010) + // Sets the sample rate to 1 kHz for Low Pass Filter (DLPF) for both the accelerometers and gyros + // disables FSYNC and sets accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; + // Maximum delay is 4.9 ms (= 204.08 Hz) which is just over a 200 Hz maximum rate + writeReg(CONFIG, 0x03); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + // Where gyroscope output rate = 1kHz when the DLPF is disabled (DLPF_CFG = 0 or 7) + // Use a 200 Hz rate; the same rate set in CONFIG above: 200 = 1000/(1 + 4) + writeReg(SMPLRT_DIV, 0x04); + + // Set gyroscope full scale range + c = readReg(GYRO_CONFIG); // Read the GYRO_CONFIG register + c &= 0b11100000; // Clear self-test bits [7:5] + + // Range selectors FS_SEL is 0 - 3, so left-shift 2-bit value into positions 4:3 + c |= (_gyroScale << 3); // Set FS_SEL bits [4:3] + writeReg(GYRO_CONFIG, c); // Set the full scale range for the gyro + + // Update gyro scale according to the selected gyro full scale range. + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Set DPS/(ADC tick) based on that 2-bit value: + switch (_gyroScale) { + case GFS_250DPS: + _gyroRes = 250.0 / 32768.0; + break; + + case GFS_500DPS: + _gyroRes = 500.0 / 32768.0; + break; + + case GFS_1000DPS: + _gyroRes = 1000.0 / 32768.0; + break; + + case GFS_2000DPS: + _gyroRes = 2000.0 / 32768.0; + break; + } + + // Set accelerometer configuration + c = readReg(ACCEL_CONFIG); // Read the ACCEL_CONFIG register + c &= 0b11100000; // Clear self-test bits [7:5] + + // Range selectors AFS_SEL is 0 - 3, so left-shift 2-bit value into positions 4:3 + c |= (_accelScale << 3); + writeReg(ACCEL_CONFIG, c); // Set full scale range for the accelerometer + + // Update acceleration scle to the selected acceleration scale range. + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + switch (_accelScale) { + case AFS_2G: + _accelRes = 2.0 / 32768.0; + break; + + case AFS_4G: + _accelRes = 4.0 / 32768.0; + break; + + case AFS_8G: + _accelRes = 8.0 / 32768.0; + break; + + case AFS_16G: + _accelRes = 16.0 / 32768.0; + break; + } + + // Configure Interrupts and Bypass Enable + // When bit [7] (INT_LEVEL) is equal to 0, the logic level for the INT pin is active high. + // When bit [6] (INT_OPEN) is equal to 0, the INT pin is configured as push-pull. + // When bit [5] (LATCH_INT_EN) is equal to 1, the INT pin is held high until the interrupt is cleared. + // When bit [4] (INT_RD_CLEAR) is equal to 0, interrupt status bits are cleared only by reading INT_STATUS + // When bit [3] (FSYNC_INT_LEVEL) is equal to 0, the logic level for the FSYNC pin + // (when used as an interrupt to the host processor) is active high. + // When bit [2] (FSYNC_INT_EN) is equal to 0, this bit disables the FSYNC pin from causing + // an interrupt to the host processor. + // When bit [1] (I2C_BYPASS_EN) is equal to 1 and bit [5] (I2C_MST_EN) in the USER_CTRL register) is equal to 0, + // the host application processor will be able to directly access the auxiliary I2C bus of the MPU-60X0. + writeReg(INT_PIN_CFG, 0b00100010); + + // Clear bit [5] in the USER_CTRL register + c = readReg(USER_CTRL); + c &= ~(1 << I2C_MST_EN); + writeReg(USER_CTRL, c); + + // Enable data ready interrupt (bit [0]) + writeReg(INT_ENABLE, 0x01); + + return true; +} + +/** + * @brief Indicates whether data is available after an interrupt + * @note The DATA_RDY_INT bit clears to 0 after the INT_STATUS register has been read. + * @param + * @retval + */ +bool MPU6050::dataReady() +{ + return(readReg(INT_STATUS) & (1 << DATA_RDY_INT)); +} + +/** + * @brief Reads acceleration ADC (analog to digital converter) + * @note + * @param + * @retval Acceleration ADC array of three elements + */ +int16_t* MPU6050::accelADC() +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + + readRegBytes(ACCEL_XOUT_H, 6, rawData); // Read the six raw data registers into data array + _accelAdc[0] = int16_t(rawData[0]) << 8 | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value + _accelAdc[1] = int16_t(rawData[2]) << 8 | rawData[3]; + _accelAdc[2] = int16_t(rawData[4]) << 8 | rawData[5]; + + return _accelAdc; +} + +/** + * @brief Reads gyro ADC (analog to digital converter) + * @note + * @param + * @retval Gyro ADC array of three elements + */ +int16_t* MPU6050::gyroADC() +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + + readRegBytes(GYRO_XOUT_H, 6, rawData); // Read the six raw data registers sequentially into data array + _gyroAdc[0] = int16_t(rawData[0]) << 8 | rawData[1]; // Turn the MSB and LSB into a signed 16-bit value + _gyroAdc[1] = int16_t(rawData[2]) << 8 | rawData[3]; + _gyroAdc[2] = int16_t(rawData[4]) << 8 | rawData[5]; + + return _gyroAdc; +} + +/** + * @brief Reads temperature ADC (analog to digital converter) + * @note + * @param + * @retval Temperature ADC value + */ +int16_t MPU6050::tempADC() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + + readRegBytes(TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return int16_t(rawData[0]) << 8 | rawData[1]; // Turn the MSB and LSB into a 16-bit value +} + +/** + * @brief + * @note + * @param + * @retval + */ +float MPU6050::temp() +{ + return(tempADC() / 340. + 36.53); // Temperature in degrees Celsius +} + +/** + * @brief Configures the motion detection control for low power accelerometer mode + * @note The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly + * Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration + * above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a + * threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out + * consideration for these threshold evaluations; otherwise, the flags would be set all the time! + * @param + * @retval + */ +void MPU6050::lowPowerAccelOnly() +{ + uint8_t c = readReg(PWR_MGMT_1); + writeReg(PWR_MGMT_1, c &~0x30); // Clear sleep and cycle bits [5:6] + writeReg(PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running + c = readReg(PWR_MGMT_2); + writeReg(PWR_MGMT_2, c &~0x38); // Clear standby XA, YA, and ZA bits [3:5] + writeReg(PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running + c = readReg(ACCEL_CONFIG); + writeReg(ACCEL_CONFIG, c &~0x07); // Clear high-pass filter bits [2:0] + + // Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold + writeReg(ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter + c = readReg(CONFIG); + writeReg(CONFIG, c &~0x07); // Clear low-pass filter bits [2:0] + writeReg(CONFIG, c | 0x00); // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate + c = readReg(INT_ENABLE); + writeReg(INT_ENABLE, c &~0xFF); // Clear all interrupts + writeReg(INT_ENABLE, 0x40); // Enable motion threshold (bits 5) interrupt only + + // Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold + // for at least the counter duration + writeReg(MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg + writeReg(MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate + + // Add delay for accumulation of samples +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(100ms); +#else + wait_ms(100); +#endif + c = readReg(ACCEL_CONFIG); + writeReg(ACCEL_CONFIG, c &~0x07); // Clear high-pass filter bits [2:0] + writeReg(ACCEL_CONFIG, c | 0x07); // Set ACCEL_HPF to 7; hold the initial accleration value as a referance + c = readReg(PWR_MGMT_2); + writeReg(PWR_MGMT_2, c &~0xC7); // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7] + writeReg(PWR_MGMT_2, c | 0x47); // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2]) + c = readReg(PWR_MGMT_1); + writeReg(PWR_MGMT_1, c &~0x20); // Clear sleep and cycle bit 5 + writeReg(PWR_MGMT_1, c | 0x20); // Set cycle bit 5 to begin low power accelerometer motion interrupts +} + +/** + * @brief Resets the MPU6050 + * @note + * @param + * @retval + */ +void MPU6050::reset() +{ + // Reset the device + + writeReg(PWR_MGMT_1, 0b10000000); // Write a one to bit 7 (reset bit); +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(100ms); +#else + wait_ms(100); +#endif +} + +/** + * @brief Calibrates the MPU6050 + * @note Function which accumulates gyro and accelerometer data after device initialization. It calculates the average + * of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. + * @param + * @retval + */ +void MPU6050::calibrate() +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t packetCount, fifoCount; + int32_t gyroBias[3] = { 0, 0, 0 }; + int32_t accelBias[3] = { 0, 0, 0 }; + + // Reset the device, reset all registers, clear gyro and accelerometer bias registers + + reset(); + + // Get stable time source: set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 + writeReg(PWR_MGMT_1, 0x01); + writeReg(PWR_MGMT_2, 0x00); +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(200ms); +#else + wait_ms(200); +#endif + // + + // Configure the device for bias calculation + writeReg(INT_ENABLE, 0x00); // Disable all interrupts + writeReg(FIFO_EN, 0x00); // Disable FIFO + writeReg(PWR_MGMT_1, 0x00); // Turn on internal clock source + writeReg(I2C_MST_CTRL, 0x00); // Disable I2C master + writeReg(USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeReg(USER_CTRL, 0x0C); // Reset FIFO and DMP +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(200ms); +#else + wait_ms(200); +#endif + // Configure MPU6050 gyro and accelerometer for bias calculation + + writeReg(CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeReg(SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeReg(GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeReg(ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + uint16_t gyroSensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelSensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeReg(USER_CTRL, 0x40); // Enable FIFO + writeReg(FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050) + + // accumulate 80 samples in 80 milliseconds = 960 bytes +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(80ms); +#else + wait_ms(80); +#endif + // + + // At end of sample accumulation, turn off FIFO sensor read + writeReg(FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readRegBytes(FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + fifoCount = ((uint16_t) data[0] << 8) | data[1]; + packetCount = fifoCount / 12; // How many sets of full gyro and accelerometer data for averaging + for (int i = 0; i < packetCount; i++) { + int16_t accel_temp[3] = { 0, 0, 0 }, + gyro_temp[3] = { 0, 0, 0 }; + readRegBytes(FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t) data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t) data[2] << 8) | data[3]); + accel_temp[2] = (int16_t) (((int16_t) data[4] << 8) | data[5]); + gyro_temp[0] = (int16_t) (((int16_t) data[6] << 8) | data[7]); + gyro_temp[1] = (int16_t) (((int16_t) data[8] << 8) | data[9]); + gyro_temp[2] = (int16_t) (((int16_t) data[10] << 8) | data[11]); + + accelBias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accelBias[1] += (int32_t) accel_temp[1]; + accelBias[2] += (int32_t) accel_temp[2]; + gyroBias[0] += (int32_t) gyro_temp[0]; + gyroBias[1] += (int32_t) gyro_temp[1]; + gyroBias[2] += (int32_t) gyro_temp[2]; + } + + accelBias[0] /= (int32_t) packetCount; // Normalize sums to get average count biases + accelBias[1] /= (int32_t) packetCount; + accelBias[2] /= (int32_t) packetCount; + gyroBias[0] /= (int32_t) packetCount; + gyroBias[1] /= (int32_t) packetCount; + gyroBias[2] /= (int32_t) packetCount; + + if (accelBias[2] > 0L) { + accelBias[2] -= (int32_t) accelSensitivity; + } // Remove gravity from the z-axis accelerometer bias calculation + else { + accelBias[2] += (int32_t) accelSensitivity; + } + + // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + data[0] = (-gyroBias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + data[1] = (-gyroBias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyroBias[1] / 4 >> 8) & 0xFF; + data[3] = (-gyroBias[1] / 4) & 0xFF; + data[4] = (-gyroBias[2] / 4 >> 8) & 0xFF; + data[5] = (-gyroBias[2] / 4) & 0xFF; + + // Push gyro biases to hardware registers + writeReg(XG_OFFS_USRH, data[0]); + writeReg(XG_OFFS_USRL, data[1]); + writeReg(YG_OFFS_USRH, data[2]); + writeReg(YG_OFFS_USRL, data[3]); + writeReg(ZG_OFFS_USRH, data[4]); + writeReg(ZG_OFFS_USRL, data[5]); + + _gyroBias[0] = (float)gyroBias[0] / (float)gyroSensitivity; // construct gyro bias in deg/s for later manual subtraction + _gyroBias[1] = (float)gyroBias[1] / (float)gyroSensitivity; + _gyroBias[2] = (float)gyroBias[2] / (float)gyroSensitivity; + + // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain + // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold + // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature + // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that + // the accelerometer biases calculated above must be divided by 8. + int32_t accel_bias_reg[3] = { 0, 0, 0 }; // A place to hold the factory accelerometer trim biases + + readRegBytes(XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int16_t) ((int16_t) data[0] << 8) | data[1]; + readRegBytes(YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int16_t) ((int16_t) data[0] << 8) | data[1]; + readRegBytes(ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int16_t) ((int16_t) data[0] << 8) | data[1]; + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = { 0, 0, 0 }; // Define array to hold mask bit for each accelerometer bias axis + + for (int i = 0; i < 3; i++) { + if (accel_bias_reg[i] & mask) + mask_bit[i] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accelBias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accelBias[1] / 8); + accel_bias_reg[2] -= (accelBias[2] / 8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + + // Push accelerometer biases to hardware registers + // writeByte(_address, XA_OFFSET_H, data[0]); + // writeByte(_address, XA_OFFSET_L_TC, data[1]); + // writeByte(_address, YA_OFFSET_H, data[2]); + // writeByte(_address, YA_OFFSET_L_TC, data[3]); + // writeByte(_address, ZA_OFFSET_H, data[4]); + // writeByte(_address, ZA_OFFSET_L_TC, data[5]); + // Output scaled accelerometer biases for manual subtraction in the main program + _accelBias[0] = (float)accelBias[0] / (float)accelSensitivity; + _accelBias[1] = (float)accelBias[1] / (float)accelSensitivity; + _accelBias[2] = (float)accelBias[2] / (float)accelSensitivity; +} + +/** + * @brief Performs accelerometer and gyroscope self test + * @note Checks calibration wrt factory settings. + * Should return percent deviation from factory trim values + * +/- 14 or less deviation is a pass + * @param + * @retval true if passed + * false otherwise + */ +bool MPU6050::selfTestOK() +{ + uint8_t rawData[4] = { 0, 0, 0, 0 }; + uint8_t selfTest[6]; + float factoryTrim[6]; + // + // Configure the accelerometer for self-test + writeReg(ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g + writeReg(GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s +#if MBED_MAJOR_VERSION > 5 + ThisThread::sleep_for(250ms); +#else + wait_ms(250); +#endif + rawData[0] = readReg(SELF_TEST_X); // X-axis self-test results + rawData[1] = readReg(SELF_TEST_Y); // Y-axis self-test results + rawData[2] = readReg(SELF_TEST_Z); // Z-axis self-test results + rawData[3] = readReg(SELF_TEST_A); // Mixed-axis self-test results + + // Extract the acceleration test results first + selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4; // XA_TEST result is a five-bit unsigned integer + selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4; // YA_TEST result is a five-bit unsigned integer + selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4; // ZA_TEST result is a five-bit unsigned integer + + // Extract the gyration test results first + selfTest[3] = rawData[0] & 0x1F; // XG_TEST result is a five-bit unsigned integer + selfTest[4] = rawData[1] & 0x1F; // YG_TEST result is a five-bit unsigned integer + selfTest[5] = rawData[2] & 0x1F; // ZG_TEST result is a five-bit unsigned integer + + // Process results to allow final comparison with factory set values + factoryTrim[0] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[0] - 1.0f) / 30.0f))); // FT[Xa] factory trim calculation + factoryTrim[1] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[1] - 1.0f) / 30.0f))); // FT[Ya] factory trim calculation + factoryTrim[2] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[2] - 1.0f) / 30.0f))); // FT[Za] factory trim calculation + factoryTrim[3] = (25.0f * 131.0f) * (pow(1.046f, (selfTest[3] - 1.0f))); // FT[Xg] factory trim calculation + factoryTrim[4] = (-25.0f * 131.0f) * (pow(1.046f, (selfTest[4] - 1.0f))); // FT[Yg] factory trim calculation + factoryTrim[5] = (25.0f * 131.0f) * (pow(1.046f, (selfTest[5] - 1.0f))); // FT[Zg] factory trim calculation + + // Output self-test results and factory trim calculation if desired + // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]); + // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]); + // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]); + // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]); + // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response + // To get to percent, must multiply by 100 and subtract result from 100 + for (int i = 0; i < 6; i++) { + _selfTest[i] = 100.0f + 100.0f * (selfTest[i] - factoryTrim[i]) / factoryTrim[i]; // Report percent differences + } + + return + ( + _selfTest[0] < 1.0f && + _selfTest[1] < 1.0f && + _selfTest[2] < 1.0f && + _selfTest[3] < 1.0f && + _selfTest[4] < 1.0f && + _selfTest[5] < 1.0f + ); +} + +/** + * @brief Computes acceleration + * @note + * @param + * @retval + */ +float* MPU6050::accel() +{ + accelADC(); + + // Compute actual acceleration values, this depends on used scale + accelX = (float)_accelAdc[0] * _accelRes - _accelBias[0]; + accelY = (float)_accelAdc[1] * _accelRes - _accelBias[1]; + accelZ = (float)_accelAdc[2] * _accelRes - _accelBias[2]; + + return accelData; +} + +/** + * @brief Computes gyro + * @note + * @param + * @retval + */ +float* MPU6050::gyro() +{ + gyroADC(); + + // Compute actual gyro values, this depends on used scale + gyroX = (float)_gyroAdc[0] * _gyroRes - _gyroBias[0]; + gyroY = (float)_gyroAdc[1] * _gyroRes - _gyroBias[1]; + gyroZ = (float)_gyroAdc[2] * _gyroRes - _gyroBias[2]; + return gyroData; +} + +/** + * @brief Adjusts filter gain after device is stabilized + * @note + * @param + * @retval + */ +void MPU6050::setGain(float beta, float zeta) +{ + _beta = beta; // set filter gain + _zeta = zeta; // set bias drift gain +} + +/** + * @brief Madgwick filter + * @note Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" + * (see http://www.x-io.co.uk/category/open-source/ for examples and more details) + * which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative + * device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. + * The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms + * but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + * @param deltaT Integration time in seconds. + * @retval + */ +void MPU6050::madgwickFilter(float deltaT) +{ + // aliases for better readability + float& q1 = _q[0]; + float& q2 = _q[1]; + float& q3 = _q[2]; + float& q4 = _q[3]; + float norm; // vector norm + float f1, f2, f3; // objective funcion 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 ax = accelX; + float ay = accelY; + float az = accelZ; + float gx = gyroX * M_PI / 180.0f; // convert to rad/s + float gy = gyroY * M_PI / 180.0f; // convert to rad/s + float gz = gyroZ * M_PI / 180.0f; // convert to rad/s + float gErrX, gErrY, gErrZ, gBiasX, gBiasY, gBiasZ; // gyro bias errors + + // Auxiliary variables to avoid repeated arithmetic + 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; + //float _2q1q3 = 2.0f * q1 * q3; + //float _2q3q4 = 2.0f * q3 * q4; + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) + return; // handle NaN + norm = 1.0f / norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Compute the objective function and Jacobian + f1 = _2q2 * q4 - _2q1 * q3 - ax; + f2 = _2q1 * q2 + _2q3 * q4 - ay; + f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - 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 estimated gyroscope errors + 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 gyroscope biases + gBiasX += gErrX * deltaT * _zeta; + gBiasY += gErrY * deltaT * _zeta; + gBiasZ += gErrZ * deltaT * _zeta; + + // Remove gyroscope biases + 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 the integrated 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); // normalise quaternion + norm = 1.0f / norm; + _q[0] = q1 * norm; + _q[1] = q2 * norm; + _q[2] = q3 * norm; + _q[3] = q4 * norm; +} + +/** + * @brief Calculates yaw in degree + * @note + * @param + * @retval + */ +float MPU6050::yaw() +{ + float yawRad = 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] + ); + + return yawRad * 180.f / M_PI; +} + +/** + * @brief Calculates pitch in degree + * @note + * @param + * @retval + */ +float MPU6050::pitch() +{ + float pitchRad = -asin(2.0f * (_q[1] * _q[3] - _q[0] * _q[2])); + + return pitchRad * 180.f / M_PI; +} + +/** + * @brief Claculates roll in degree + * @note + * @param + * @retval + */ +float MPU6050::roll() +{ + float rollRad = 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] + ); + + return rollRad * 180.0f / M_PI; +}