library for chair for mpu
chair_MPU9250.cpp
- Committer:
- ryanlin97
- Date:
- 2018-07-20
- Revision:
- 0:fd1a49d15f7f
- Child:
- 1:ff1d286b3cf4
File content as of revision 0:fd1a49d15f7f:
#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; }