library for chair for mpu
chair_MPU9250.cpp@1:ff1d286b3cf4, 2018-07-22 (annotated)
- Committer:
- ryanlin97
- Date:
- Sun Jul 22 06:14:48 2018 +0000
- Revision:
- 1:ff1d286b3cf4
- Parent:
- 0:fd1a49d15f7f
- Child:
- 2:66660dcf55fb
turning
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 0:fd1a49d15f7f | 1 | #include "chair_MPU9250.h" |
ryanlin97 | 1:ff1d286b3cf4 | 2 | float total_yaw = 0; |
ryanlin97 | 0:fd1a49d15f7f | 3 | |
ryanlin97 | 1:ff1d286b3cf4 | 4 | chair_MPU9250::chair_MPU9250(Serial* out, Timer* time) |
ryanlin97 | 0:fd1a49d15f7f | 5 | { |
ryanlin97 | 0:fd1a49d15f7f | 6 | imu = new MPU9250(SDA, SCL); |
ryanlin97 | 0:fd1a49d15f7f | 7 | usb = out; |
ryanlin97 | 1:ff1d286b3cf4 | 8 | t = time; |
ryanlin97 | 1:ff1d286b3cf4 | 9 | start = false; |
ryanlin97 | 0:fd1a49d15f7f | 10 | } |
ryanlin97 | 0:fd1a49d15f7f | 11 | |
ryanlin97 | 1:ff1d286b3cf4 | 12 | chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time) |
ryanlin97 | 0:fd1a49d15f7f | 13 | { |
ryanlin97 | 0:fd1a49d15f7f | 14 | imu = new MPU9250(sda_pin, scl_pin); |
ryanlin97 | 0:fd1a49d15f7f | 15 | usb = out; |
ryanlin97 | 1:ff1d286b3cf4 | 16 | t = time; |
ryanlin97 | 1:ff1d286b3cf4 | 17 | start = false; |
ryanlin97 | 0:fd1a49d15f7f | 18 | } |
ryanlin97 | 0:fd1a49d15f7f | 19 | |
ryanlin97 | 1:ff1d286b3cf4 | 20 | void chair_MPU9250::setStart(){ |
ryanlin97 | 1:ff1d286b3cf4 | 21 | start = true; |
ryanlin97 | 1:ff1d286b3cf4 | 22 | } |
ryanlin97 | 1:ff1d286b3cf4 | 23 | |
ryanlin97 | 0:fd1a49d15f7f | 24 | void chair_MPU9250::setup() |
ryanlin97 | 0:fd1a49d15f7f | 25 | { |
ryanlin97 | 1:ff1d286b3cf4 | 26 | t->start(); |
ryanlin97 | 0:fd1a49d15f7f | 27 | // Read the WHO_AM_I register, this is a good test of communication |
ryanlin97 | 0:fd1a49d15f7f | 28 | uint8_t whoami = imu->readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
ryanlin97 | 0:fd1a49d15f7f | 29 | usb->printf("I AM 0x%x\n\r", whoami); |
ryanlin97 | 0:fd1a49d15f7f | 30 | usb->printf("I SHOULD BE 0x71\n\r"); |
ryanlin97 | 0:fd1a49d15f7f | 31 | |
ryanlin97 | 0:fd1a49d15f7f | 32 | if (whoami == 0x71) { // WHO_AM_I should always be 0x68 |
ryanlin97 | 0:fd1a49d15f7f | 33 | usb->printf("MPU9250 is online...\n\r"); |
ryanlin97 | 0:fd1a49d15f7f | 34 | |
ryanlin97 | 1:ff1d286b3cf4 | 35 | wait(.1); |
ryanlin97 | 0:fd1a49d15f7f | 36 | |
ryanlin97 | 0:fd1a49d15f7f | 37 | imu->resetMPU9250(); // Reset registers to default in preparation for device calibration |
ryanlin97 | 0:fd1a49d15f7f | 38 | imu->calibrateMPU9250(imu->gyroBias, imu->accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
ryanlin97 | 0:fd1a49d15f7f | 39 | imu->initMPU9250(); |
ryanlin97 | 0:fd1a49d15f7f | 40 | imu->initAK8963(imu->magCalibration); |
ryanlin97 | 1:ff1d286b3cf4 | 41 | wait(.1); |
ryanlin97 | 0:fd1a49d15f7f | 42 | } else { |
ryanlin97 | 0:fd1a49d15f7f | 43 | usb->printf("Could not connect to MPU9250: \n\r"); |
ryanlin97 | 0:fd1a49d15f7f | 44 | usb->printf("%#x \n", whoami); |
ryanlin97 | 0:fd1a49d15f7f | 45 | |
ryanlin97 | 0:fd1a49d15f7f | 46 | while(1) ; // Loop forever if communication doesn't happen |
ryanlin97 | 0:fd1a49d15f7f | 47 | } |
ryanlin97 | 0:fd1a49d15f7f | 48 | |
ryanlin97 | 0:fd1a49d15f7f | 49 | |
ryanlin97 | 0:fd1a49d15f7f | 50 | imu->getAres(); // Get accelerometer sensitivity |
ryanlin97 | 0:fd1a49d15f7f | 51 | imu->getGres(); // Get gyro sensitivity |
ryanlin97 | 0:fd1a49d15f7f | 52 | imu->getMres(); // Get magnetometer sensitivity |
ryanlin97 | 0:fd1a49d15f7f | 53 | usb->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu->aRes); |
ryanlin97 | 0:fd1a49d15f7f | 54 | usb->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu->gRes); |
ryanlin97 | 0:fd1a49d15f7f | 55 | usb->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu->mRes); |
ryanlin97 | 0:fd1a49d15f7f | 56 | imu->magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated |
ryanlin97 | 0:fd1a49d15f7f | 57 | imu->magbias[1] = +120.; // User environmental x-axis correction in milliGauss |
ryanlin97 | 1:ff1d286b3cf4 | 58 | imu->magbias[2] = +125.; // User environmental x-axis correction in milliGauss |
ryanlin97 | 1:ff1d286b3cf4 | 59 | usb->printf("setup complete\n"); |
ryanlin97 | 0:fd1a49d15f7f | 60 | |
ryanlin97 | 0:fd1a49d15f7f | 61 | } |
ryanlin97 | 0:fd1a49d15f7f | 62 | |
ryanlin97 | 0:fd1a49d15f7f | 63 | double chair_MPU9250::yaw() |
ryanlin97 | 1:ff1d286b3cf4 | 64 | { |
ryanlin97 | 1:ff1d286b3cf4 | 65 | // If intPin goes high, all data registers have new data |
ryanlin97 | 0:fd1a49d15f7f | 66 | if(imu->readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
ryanlin97 | 0:fd1a49d15f7f | 67 | |
ryanlin97 | 1:ff1d286b3cf4 | 68 | |
ryanlin97 | 0:fd1a49d15f7f | 69 | imu->readAccelData(imu->accelCount); // Read the x/y/z adc values |
ryanlin97 | 0:fd1a49d15f7f | 70 | // Now we'll calculate the accleration value into actual g's |
ryanlin97 | 0:fd1a49d15f7f | 71 | imu->ax = (float)imu->accelCount[0]*imu->aRes - imu->accelBias[0]; // get actual g value, this depends on scale being set |
ryanlin97 | 0:fd1a49d15f7f | 72 | imu->ay = (float)imu->accelCount[1]*imu->aRes - imu->accelBias[1]; |
ryanlin97 | 0:fd1a49d15f7f | 73 | imu->az = (float)imu->accelCount[2]*imu->aRes - imu->accelBias[2]; |
ryanlin97 | 0:fd1a49d15f7f | 74 | |
ryanlin97 | 0:fd1a49d15f7f | 75 | imu->readGyroData(imu->gyroCount); // Read the x/y/z adc values |
ryanlin97 | 0:fd1a49d15f7f | 76 | // Calculate the gyro value into actual degrees per second |
ryanlin97 | 0:fd1a49d15f7f | 77 | imu->gx = (float)imu->gyroCount[0]*imu->gRes - imu->gyroBias[0]; // get actual gyro value, this depends on scale being set |
ryanlin97 | 0:fd1a49d15f7f | 78 | imu->gy = (float)imu->gyroCount[1]*imu->gRes - imu->gyroBias[1]; |
ryanlin97 | 0:fd1a49d15f7f | 79 | imu->gz = (float)imu->gyroCount[2]*imu->gRes - imu->gyroBias[2]; |
ryanlin97 | 0:fd1a49d15f7f | 80 | |
ryanlin97 | 0:fd1a49d15f7f | 81 | imu->readMagData(imu->magCount); // Read the x/y/z adc values |
ryanlin97 | 0:fd1a49d15f7f | 82 | // Calculate the magnetometer values in milliGauss |
ryanlin97 | 0:fd1a49d15f7f | 83 | // Include factory calibration per data sheet and user environmental corrections |
ryanlin97 | 0:fd1a49d15f7f | 84 | imu->mx = (float)imu->magCount[0]*imu->mRes*imu->magCalibration[0] - imu->magbias[0]; // get actual magnetometer value, this depends on scale being set |
ryanlin97 | 0:fd1a49d15f7f | 85 | imu->my = (float)imu->magCount[1]*imu->mRes*imu->magCalibration[1] - imu->magbias[1]; |
ryanlin97 | 0:fd1a49d15f7f | 86 | imu->mz = (float)imu->magCount[2]*imu->mRes*imu->magCalibration[2] - imu->magbias[2]; |
ryanlin97 | 0:fd1a49d15f7f | 87 | } |
ryanlin97 | 1:ff1d286b3cf4 | 88 | |
ryanlin97 | 1:ff1d286b3cf4 | 89 | float readTime; |
ryanlin97 | 1:ff1d286b3cf4 | 90 | if(!start) { |
ryanlin97 | 1:ff1d286b3cf4 | 91 | readTime = 0; |
ryanlin97 | 1:ff1d286b3cf4 | 92 | chair_MPU9250::setStart(); |
ryanlin97 | 1:ff1d286b3cf4 | 93 | } |
ryanlin97 | 1:ff1d286b3cf4 | 94 | else { |
ryanlin97 | 1:ff1d286b3cf4 | 95 | readTime = t->read(); |
ryanlin97 | 1:ff1d286b3cf4 | 96 | } |
ryanlin97 | 1:ff1d286b3cf4 | 97 | |
ryanlin97 | 1:ff1d286b3cf4 | 98 | |
ryanlin97 | 0:fd1a49d15f7f | 99 | if(imu->gz>.3 || imu->gz < -.3) { |
ryanlin97 | 1:ff1d286b3cf4 | 100 | total_yaw = (total_yaw - readTime*imu->gz); |
ryanlin97 | 1:ff1d286b3cf4 | 101 | t->reset(); |
ryanlin97 | 1:ff1d286b3cf4 | 102 | if(total_yaw > 360){ |
ryanlin97 | 1:ff1d286b3cf4 | 103 | total_yaw -= 360; |
ryanlin97 | 1:ff1d286b3cf4 | 104 | } |
ryanlin97 | 1:ff1d286b3cf4 | 105 | |
ryanlin97 | 1:ff1d286b3cf4 | 106 | if(total_yaw < 0){ |
ryanlin97 | 1:ff1d286b3cf4 | 107 | total_yaw += 360; |
ryanlin97 | 1:ff1d286b3cf4 | 108 | } |
ryanlin97 | 0:fd1a49d15f7f | 109 | } |
ryanlin97 | 1:ff1d286b3cf4 | 110 | return total_yaw; |
ryanlin97 | 0:fd1a49d15f7f | 111 | } |