library for chair for mpu
Diff: chair_MPU9250.cpp
- Revision:
- 0:fd1a49d15f7f
- Child:
- 1:ff1d286b3cf4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/chair_MPU9250.cpp Fri Jul 20 17:54:29 2018 +0000 @@ -0,0 +1,92 @@ +#include "chair_MPU9250.h" + +Timer t; + +chair_MPU9250::chair_MPU9250(Serial* out) +{ + imu = new MPU9250(SDA, SCL); + usb = out; +} + +chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out) +{ + imu = new MPU9250(sda_pin, scl_pin); + usb = out; +} + +void chair_MPU9250::setup() +{ + // Read the WHO_AM_I register, this is a good test of communication + uint8_t whoami = imu->readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 + usb->printf("I AM 0x%x\n\r", whoami); + usb->printf("I SHOULD BE 0x71\n\r"); + + if (whoami == 0x71) { // WHO_AM_I should always be 0x68 + usb->printf("MPU9250 is online...\n\r"); + + wait(1); + + imu->resetMPU9250(); // Reset registers to default in preparation for device calibration + imu->calibrateMPU9250(imu->gyroBias, imu->accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + imu->initMPU9250(); + imu->initAK8963(imu->magCalibration); + wait(2); + } else { + usb->printf("Could not connect to MPU9250: \n\r"); + usb->printf("%#x \n", whoami); + + while(1) ; // Loop forever if communication doesn't happen + } + + + imu->getAres(); // Get accelerometer sensitivity + imu->getGres(); // Get gyro sensitivity + imu->getMres(); // Get magnetometer sensitivity + usb->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu->aRes); + usb->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu->gRes); + usb->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu->mRes); + imu->magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated + imu->magbias[1] = +120.; // User environmental x-axis correction in milliGauss + imu->magbias[2] = +125.; // User environmental x-axis correction in milliGauss + t.start(); + +} + +double chair_MPU9250::yaw() +{ + +// If intPin goes high, all data registers have new data + if(imu->readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt + + imu->readAccelData(imu->accelCount); // Read the x/y/z adc values + // Now we'll calculate the accleration value into actual g's + imu->ax = (float)imu->accelCount[0]*imu->aRes - imu->accelBias[0]; // get actual g value, this depends on scale being set + imu->ay = (float)imu->accelCount[1]*imu->aRes - imu->accelBias[1]; + imu->az = (float)imu->accelCount[2]*imu->aRes - imu->accelBias[2]; + + imu->readGyroData(imu->gyroCount); // Read the x/y/z adc values + // Calculate the gyro value into actual degrees per second + imu->gx = (float)imu->gyroCount[0]*imu->gRes - imu->gyroBias[0]; // get actual gyro value, this depends on scale being set + imu->gy = (float)imu->gyroCount[1]*imu->gRes - imu->gyroBias[1]; + imu->gz = (float)imu->gyroCount[2]*imu->gRes - imu->gyroBias[2]; + + imu->readMagData(imu->magCount); // Read the x/y/z adc values + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental corrections + imu->mx = (float)imu->magCount[0]*imu->mRes*imu->magCalibration[0] - imu->magbias[0]; // get actual magnetometer value, this depends on scale being set + imu->my = (float)imu->magCount[1]*imu->mRes*imu->magCalibration[1] - imu->magbias[1]; + imu->mz = (float)imu->magCount[2]*imu->mRes*imu->magCalibration[2] - imu->magbias[2]; + } + + float yaw = 0; + if(imu->gz>.3 || imu->gz < -.3) { + yaw = (yaw - t.read()*imu->gz); + t.reset(); + if(yaw > 360) + yaw -= 360; + if(yaw < 0) + yaw += 360; + } + + return (double)yaw; +}