fin
main.cpp@0:f2207cfaf993, 2019-11-19 (annotated)
- Committer:
- jbeason3
- Date:
- Tue Nov 19 04:30:59 2019 +0000
- Revision:
- 0:f2207cfaf993
fin;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jbeason3 | 0:f2207cfaf993 | 1 | #include "mbed.h" |
jbeason3 | 0:f2207cfaf993 | 2 | #include "MPU9250.h" |
jbeason3 | 0:f2207cfaf993 | 3 | #include "SDFileSystem.h" |
jbeason3 | 0:f2207cfaf993 | 4 | SDFileSystem sd(p5,p6,p7,p8, "sd"); |
jbeason3 | 0:f2207cfaf993 | 5 | DigitalOut myled(LED1); |
jbeason3 | 0:f2207cfaf993 | 6 | MPU9250 mpu9250(p28,p27); |
jbeason3 | 0:f2207cfaf993 | 7 | PwmOut servo(p26); |
jbeason3 | 0:f2207cfaf993 | 8 | Serial pc(USBTX, USBRX); // tx, rx |
jbeason3 | 0:f2207cfaf993 | 9 | |
jbeason3 | 0:f2207cfaf993 | 10 | float Time; |
jbeason3 | 0:f2207cfaf993 | 11 | float sum = 0; |
jbeason3 | 0:f2207cfaf993 | 12 | float servo_PWM; |
jbeason3 | 0:f2207cfaf993 | 13 | float angle; |
jbeason3 | 0:f2207cfaf993 | 14 | char buffer[100]; |
jbeason3 | 0:f2207cfaf993 | 15 | char file_name[100]; |
jbeason3 | 0:f2207cfaf993 | 16 | uint32_t sumCount = 0; |
jbeason3 | 0:f2207cfaf993 | 17 | Timer t; |
jbeason3 | 0:f2207cfaf993 | 18 | Ticker logTicker; |
jbeason3 | 0:f2207cfaf993 | 19 | |
jbeason3 | 0:f2207cfaf993 | 20 | |
jbeason3 | 0:f2207cfaf993 | 21 | void mpu9250_initialization(){ |
jbeason3 | 0:f2207cfaf993 | 22 | pc.printf("####CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); |
jbeason3 | 0:f2207cfaf993 | 23 | //initial com check |
jbeason3 | 0:f2207cfaf993 | 24 | uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
jbeason3 | 0:f2207cfaf993 | 25 | pc.printf("####I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); |
jbeason3 | 0:f2207cfaf993 | 26 | if (whoami == 0x71) // WHO_AM_I should always be 0x68 |
jbeason3 | 0:f2207cfaf993 | 27 | { |
jbeason3 | 0:f2207cfaf993 | 28 | pc.printf("####MPU9250 WHO_AM_I is 0x%x\n\r", whoami); |
jbeason3 | 0:f2207cfaf993 | 29 | pc.printf("####MPU9250 is online...\n\r"); |
jbeason3 | 0:f2207cfaf993 | 30 | wait(1); |
jbeason3 | 0:f2207cfaf993 | 31 | //reset MPU and conduct self test// |
jbeason3 | 0:f2207cfaf993 | 32 | pc.printf("####Please wait,IMU Resetting####\r\n"); |
jbeason3 | 0:f2207cfaf993 | 33 | mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration |
jbeason3 | 0:f2207cfaf993 | 34 | pc.printf("####Self Test####\r\n"); |
jbeason3 | 0:f2207cfaf993 | 35 | //initial MPU9250 Parameters |
jbeason3 | 0:f2207cfaf993 | 36 | mpu9250.Ascale = AFS_2G; |
jbeason3 | 0:f2207cfaf993 | 37 | mpu9250.Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS |
jbeason3 | 0:f2207cfaf993 | 38 | mpu9250.Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution |
jbeason3 | 0:f2207cfaf993 | 39 | mpu9250.Mmode = 0x06; |
jbeason3 | 0:f2207cfaf993 | 40 | mpu9250.delt_t=0; |
jbeason3 | 0:f2207cfaf993 | 41 | mpu9250.deltat=0.0f; |
jbeason3 | 0:f2207cfaf993 | 42 | mpu9250.lastUpdate = 0; |
jbeason3 | 0:f2207cfaf993 | 43 | mpu9250.firstUpdate = 0; |
jbeason3 | 0:f2207cfaf993 | 44 | mpu9250.Now = 0; |
jbeason3 | 0:f2207cfaf993 | 45 | mpu9250.count=0; |
jbeason3 | 0:f2207cfaf993 | 46 | mpu9250.q[0] = 1.0f; |
jbeason3 | 0:f2207cfaf993 | 47 | mpu9250.q[1] = 0.0f; |
jbeason3 | 0:f2207cfaf993 | 48 | mpu9250.q[2] = 0.0f; |
jbeason3 | 0:f2207cfaf993 | 49 | mpu9250.q[3] = 0.0f; |
jbeason3 | 0:f2207cfaf993 | 50 | // mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values |
jbeason3 | 0:f2207cfaf993 | 51 | //pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); |
jbeason3 | 0:f2207cfaf993 | 52 | // pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); |
jbeason3 | 0:f2207cfaf993 | 53 | //pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); |
jbeason3 | 0:f2207cfaf993 | 54 | //pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); |
jbeason3 | 0:f2207cfaf993 | 55 | //pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); |
jbeason3 | 0:f2207cfaf993 | 56 | //pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); |
jbeason3 | 0:f2207cfaf993 | 57 | pc.printf("####Gyro and accelerometer Calibration will start in 5 seconds####\r\n"); |
jbeason3 | 0:f2207cfaf993 | 58 | pc.printf("####Please keep the IMU still\r\n"); |
jbeason3 | 0:f2207cfaf993 | 59 | wait(5); |
jbeason3 | 0:f2207cfaf993 | 60 | pc.printf("####Calibration starts\r\n"); |
jbeason3 | 0:f2207cfaf993 | 61 | mpu9250.calibrateMPU9250(mpu9250.gyroBias, mpu9250.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
jbeason3 | 0:f2207cfaf993 | 62 | pc.printf("x gyro bias = %f\n\r", mpu9250.gyroBias[0]); |
jbeason3 | 0:f2207cfaf993 | 63 | pc.printf("y gyro bias = %f\n\r", mpu9250.gyroBias[1]); |
jbeason3 | 0:f2207cfaf993 | 64 | pc.printf("z gyro bias = %f\n\r", mpu9250.gyroBias[2]); |
jbeason3 | 0:f2207cfaf993 | 65 | pc.printf("x accel bias = %f\n\r", mpu9250.accelBias[0]); |
jbeason3 | 0:f2207cfaf993 | 66 | pc.printf("y accel bias = %f\n\r", mpu9250.accelBias[1]); |
jbeason3 | 0:f2207cfaf993 | 67 | pc.printf("z accel bias = %f\n\r", mpu9250.accelBias[2]); |
jbeason3 | 0:f2207cfaf993 | 68 | wait(2); |
jbeason3 | 0:f2207cfaf993 | 69 | ///initialization |
jbeason3 | 0:f2207cfaf993 | 70 | mpu9250.initMPU9250(); |
jbeason3 | 0:f2207cfaf993 | 71 | pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
jbeason3 | 0:f2207cfaf993 | 72 | mpu9250.initAK8963(mpu9250.magCalibration); |
jbeason3 | 0:f2207cfaf993 | 73 | pc.printf("Magnetometer initilized\r\n"); |
jbeason3 | 0:f2207cfaf993 | 74 | // pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer |
jbeason3 | 0:f2207cfaf993 | 75 | // pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); |
jbeason3 | 0:f2207cfaf993 | 76 | // pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); |
jbeason3 | 0:f2207cfaf993 | 77 | if(mpu9250.Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); |
jbeason3 | 0:f2207cfaf993 | 78 | if(mpu9250.Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); |
jbeason3 | 0:f2207cfaf993 | 79 | if(mpu9250.Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); |
jbeason3 | 0:f2207cfaf993 | 80 | if(mpu9250.Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); |
jbeason3 | 0:f2207cfaf993 | 81 | } |
jbeason3 | 0:f2207cfaf993 | 82 | else |
jbeason3 | 0:f2207cfaf993 | 83 | { |
jbeason3 | 0:f2207cfaf993 | 84 | pc.printf("Could not connect to MPU9250: \n\r"); |
jbeason3 | 0:f2207cfaf993 | 85 | pc.printf("%#x \n", whoami); |
jbeason3 | 0:f2207cfaf993 | 86 | while(1) ; // Loop forever if communication doesn't happen |
jbeason3 | 0:f2207cfaf993 | 87 | } |
jbeason3 | 0:f2207cfaf993 | 88 | mpu9250.getAres(); // Get accelerometer sensitivity |
jbeason3 | 0:f2207cfaf993 | 89 | mpu9250.getGres(); // Get gyro sensitivity |
jbeason3 | 0:f2207cfaf993 | 90 | mpu9250.getMres(); // Get magnetometer sensitivity |
jbeason3 | 0:f2207cfaf993 | 91 | pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/mpu9250.aRes); |
jbeason3 | 0:f2207cfaf993 | 92 | pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/mpu9250.gRes); |
jbeason3 | 0:f2207cfaf993 | 93 | pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mpu9250.mRes); |
jbeason3 | 0:f2207cfaf993 | 94 | pc.printf("####IMU initialization done####\r\n"); |
jbeason3 | 0:f2207cfaf993 | 95 | wait(1); |
jbeason3 | 0:f2207cfaf993 | 96 | } |
jbeason3 | 0:f2207cfaf993 | 97 | |
jbeason3 | 0:f2207cfaf993 | 98 | |
jbeason3 | 0:f2207cfaf993 | 99 | void mag_cali(){ |
jbeason3 | 0:f2207cfaf993 | 100 | int32_t mag_bias[3] = {0, 0, 0}; |
jbeason3 | 0:f2207cfaf993 | 101 | int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0}; |
jbeason3 | 0:f2207cfaf993 | 102 | //float dest1[3]={0,0,0}, dest2[3]={0,0,0}; |
jbeason3 | 0:f2207cfaf993 | 103 | pc.printf("####Compass Calibration starts in 5 seconds\r\n"); |
jbeason3 | 0:f2207cfaf993 | 104 | wait(5); |
jbeason3 | 0:f2207cfaf993 | 105 | pc.printf("###Start moving your imu in figure 8\r\n"); |
jbeason3 | 0:f2207cfaf993 | 106 | for (int i=0;i<1500;i++){ //1500 for 100 Hz |
jbeason3 | 0:f2207cfaf993 | 107 | mpu9250.readMagData(mag_temp); // Read the x/y/z adc values |
jbeason3 | 0:f2207cfaf993 | 108 | for(int jj=0; jj<3; jj++){ |
jbeason3 | 0:f2207cfaf993 | 109 | if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; |
jbeason3 | 0:f2207cfaf993 | 110 | if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; |
jbeason3 | 0:f2207cfaf993 | 111 | } |
jbeason3 | 0:f2207cfaf993 | 112 | wait(0.01);//delay for 10 ms. |
jbeason3 | 0:f2207cfaf993 | 113 | } |
jbeason3 | 0:f2207cfaf993 | 114 | //get hard iron correction |
jbeason3 | 0:f2207cfaf993 | 115 | // Get hard iron correction |
jbeason3 | 0:f2207cfaf993 | 116 | mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts |
jbeason3 | 0:f2207cfaf993 | 117 | mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts |
jbeason3 | 0:f2207cfaf993 | 118 | mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts |
jbeason3 | 0:f2207cfaf993 | 119 | mpu9250.magbias[0] = (float) mag_bias[0]*mpu9250.mRes*mpu9250.magCalibration[0]; // save mag biases in G for main program |
jbeason3 | 0:f2207cfaf993 | 120 | mpu9250.magbias[1] = (float) mag_bias[1]*mpu9250.mRes*mpu9250.magCalibration[1]; |
jbeason3 | 0:f2207cfaf993 | 121 | mpu9250.magbias[2] = (float) mag_bias[2]*mpu9250.mRes*mpu9250.magCalibration[2]; |
jbeason3 | 0:f2207cfaf993 | 122 | pc.printf("####Mag bias =%f,%f,%f\r\n",mpu9250.magbias[0],mpu9250.magbias[1],mpu9250.magbias[2]); |
jbeason3 | 0:f2207cfaf993 | 123 | /*//get soft iron correction |
jbeason3 | 0:f2207cfaf993 | 124 | // Get soft iron correction estimate |
jbeason3 | 0:f2207cfaf993 | 125 | mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts |
jbeason3 | 0:f2207cfaf993 | 126 | mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts |
jbeason3 | 0:f2207cfaf993 | 127 | mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts |
jbeason3 | 0:f2207cfaf993 | 128 | |
jbeason3 | 0:f2207cfaf993 | 129 | float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; |
jbeason3 | 0:f2207cfaf993 | 130 | avg_rad /= 3.0; |
jbeason3 | 0:f2207cfaf993 | 131 | |
jbeason3 | 0:f2207cfaf993 | 132 | dest2[0] = avg_rad/((float)mag_scale[0]); |
jbeason3 | 0:f2207cfaf993 | 133 | dest2[1] = avg_rad/((float)mag_scale[1]); |
jbeason3 | 0:f2207cfaf993 | 134 | dest2[2] = avg_rad/((float)mag_scale[2]); |
jbeason3 | 0:f2207cfaf993 | 135 | */ |
jbeason3 | 0:f2207cfaf993 | 136 | pc.printf("####Mag Calibration done!\r\n"); |
jbeason3 | 0:f2207cfaf993 | 137 | } |
jbeason3 | 0:f2207cfaf993 | 138 | |
jbeason3 | 0:f2207cfaf993 | 139 | void set_servo_angle(){ |
jbeason3 | 0:f2207cfaf993 | 140 | if(mpu9250.pitch > 90){ |
jbeason3 | 0:f2207cfaf993 | 141 | angle = 90; |
jbeason3 | 0:f2207cfaf993 | 142 | } |
jbeason3 | 0:f2207cfaf993 | 143 | if(mpu9250.pitch < -90){ |
jbeason3 | 0:f2207cfaf993 | 144 | angle = -90; |
jbeason3 | 0:f2207cfaf993 | 145 | } |
jbeason3 | 0:f2207cfaf993 | 146 | else{ |
jbeason3 | 0:f2207cfaf993 | 147 | angle=mpu9250.pitch; |
jbeason3 | 0:f2207cfaf993 | 148 | } |
jbeason3 | 0:f2207cfaf993 | 149 | servo_PWM = .0005/(angle+180); |
jbeason3 | 0:f2207cfaf993 | 150 | servo.period(0.2); |
jbeason3 | 0:f2207cfaf993 | 151 | servo.pulsewidth(servo_PWM); |
jbeason3 | 0:f2207cfaf993 | 152 | } |
jbeason3 | 0:f2207cfaf993 | 153 | |
jbeason3 | 0:f2207cfaf993 | 154 | void log_data(){ |
jbeason3 | 0:f2207cfaf993 | 155 | |
jbeason3 | 0:f2207cfaf993 | 156 | FILE *fp = fopen(file_name, "a"); |
jbeason3 | 0:f2207cfaf993 | 157 | if(fp == NULL) error("Could not open file for write\n"); |
jbeason3 | 0:f2207cfaf993 | 158 | |
jbeason3 | 0:f2207cfaf993 | 159 | fprintf(fp, "$IMURW,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", Time,mpu9250.ax,mpu9250.ay,mpu9250.az,mpu9250.gx,mpu9250.gy,mpu9250.gz,mpu9250.mx,mpu9250.my,mpu9250.mz); |
jbeason3 | 0:f2207cfaf993 | 160 | fprintf(fp, "$IMUPS,%f,%f,%f,%f,%f,%f,%f,%f;\r\n", Time, mpu9250.roll, mpu9250.pitch, mpu9250.yaw,mpu9250.q[0],mpu9250.q[1],mpu9250.q[2],mpu9250.q[3]); |
jbeason3 | 0:f2207cfaf993 | 161 | fprintf(fp, "$SERVO,%f,%f,%f,%f;\r\n", Time, servo_PWM, angle, mpu9250.pitch); |
jbeason3 | 0:f2207cfaf993 | 162 | fclose(fp); |
jbeason3 | 0:f2207cfaf993 | 163 | |
jbeason3 | 0:f2207cfaf993 | 164 | } |
jbeason3 | 0:f2207cfaf993 | 165 | |
jbeason3 | 0:f2207cfaf993 | 166 | int main() { |
jbeason3 | 0:f2207cfaf993 | 167 | pc.baud(9600); |
jbeason3 | 0:f2207cfaf993 | 168 | pc.printf("Please set a file name \r\n"); |
jbeason3 | 0:f2207cfaf993 | 169 | pc.scanf("%s",buffer); |
jbeason3 | 0:f2207cfaf993 | 170 | sprintf(file_name,"/sd/mydir/%s.txt",buffer); |
jbeason3 | 0:f2207cfaf993 | 171 | pc.printf("The file name and directory is: %s\r\n", file_name); |
jbeason3 | 0:f2207cfaf993 | 172 | logTicker.attach(&log_data,1.0); |
jbeason3 | 0:f2207cfaf993 | 173 | t.start(); |
jbeason3 | 0:f2207cfaf993 | 174 | mpu9250_initialization(); |
jbeason3 | 0:f2207cfaf993 | 175 | mag_cali(); |
jbeason3 | 0:f2207cfaf993 | 176 | |
jbeason3 | 0:f2207cfaf993 | 177 | |
jbeason3 | 0:f2207cfaf993 | 178 | pc.printf("####IMU is all set, going to start sensing in 5 seconds\r\n"); |
jbeason3 | 0:f2207cfaf993 | 179 | wait(5); |
jbeason3 | 0:f2207cfaf993 | 180 | while(1){ |
jbeason3 | 0:f2207cfaf993 | 181 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
jbeason3 | 0:f2207cfaf993 | 182 | mpu9250.readAccelData(mpu9250.accelCount); // Read the x/y/z adc values |
jbeason3 | 0:f2207cfaf993 | 183 | // Now we'll calculate the accleration value into actual g's |
jbeason3 | 0:f2207cfaf993 | 184 | mpu9250.ax = (float)mpu9250.accelCount[0]*mpu9250.aRes - mpu9250.accelBias[0]; // get actual g value, this depends on scale being set |
jbeason3 | 0:f2207cfaf993 | 185 | mpu9250.ay = (float)mpu9250.accelCount[1]*mpu9250.aRes - mpu9250.accelBias[1]; |
jbeason3 | 0:f2207cfaf993 | 186 | mpu9250.az = (float)mpu9250.accelCount[2]*mpu9250.aRes - mpu9250.accelBias[2]; |
jbeason3 | 0:f2207cfaf993 | 187 | |
jbeason3 | 0:f2207cfaf993 | 188 | mpu9250.readGyroData(mpu9250.gyroCount); // Read the x/y/z adc values |
jbeason3 | 0:f2207cfaf993 | 189 | // Calculate the gyro value into actual degrees per second |
jbeason3 | 0:f2207cfaf993 | 190 | mpu9250.gx = (float)mpu9250.gyroCount[0]*mpu9250.gRes - mpu9250.gyroBias[0]; // get actual gyro value, this depends on scale being set |
jbeason3 | 0:f2207cfaf993 | 191 | mpu9250.gy = (float)mpu9250.gyroCount[1]*mpu9250.gRes - mpu9250.gyroBias[1]; |
jbeason3 | 0:f2207cfaf993 | 192 | mpu9250.gz = (float)mpu9250.gyroCount[2]*mpu9250.gRes - mpu9250.gyroBias[2]; |
jbeason3 | 0:f2207cfaf993 | 193 | |
jbeason3 | 0:f2207cfaf993 | 194 | mpu9250.readMagData(mpu9250.magCount); // Read the x/y/z adc values |
jbeason3 | 0:f2207cfaf993 | 195 | // Calculate the magnetometer values in milliGauss |
jbeason3 | 0:f2207cfaf993 | 196 | // Include factory calibration per data sheet and user environmental corrections |
jbeason3 | 0:f2207cfaf993 | 197 | mpu9250.mx = (float)mpu9250.magCount[0]*mpu9250.mRes*mpu9250.magCalibration[0] - mpu9250.magbias[0]; // get actual magnetometer value, this depends on scale being set |
jbeason3 | 0:f2207cfaf993 | 198 | mpu9250.my = (float)mpu9250.magCount[1]*mpu9250.mRes*mpu9250.magCalibration[1] - mpu9250.magbias[1]; |
jbeason3 | 0:f2207cfaf993 | 199 | mpu9250.mz = (float)mpu9250.magCount[2]*mpu9250.mRes*mpu9250.magCalibration[2] - mpu9250.magbias[2]; |
jbeason3 | 0:f2207cfaf993 | 200 | } |
jbeason3 | 0:f2207cfaf993 | 201 | |
jbeason3 | 0:f2207cfaf993 | 202 | mpu9250.Now = t.read_us(); |
jbeason3 | 0:f2207cfaf993 | 203 | mpu9250.deltat = (float)((mpu9250.Now - mpu9250.lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
jbeason3 | 0:f2207cfaf993 | 204 | mpu9250.lastUpdate = mpu9250.Now; |
jbeason3 | 0:f2207cfaf993 | 205 | sum += mpu9250.deltat; |
jbeason3 | 0:f2207cfaf993 | 206 | sumCount++; |
jbeason3 | 0:f2207cfaf993 | 207 | //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); |
jbeason3 | 0:f2207cfaf993 | 208 | //compute the quaternion. |
jbeason3 | 0:f2207cfaf993 | 209 | |
jbeason3 | 0:f2207cfaf993 | 210 | mpu9250.MahonyQuaternionUpdate(mpu9250.ax, mpu9250.ay, mpu9250.az, |
jbeason3 | 0:f2207cfaf993 | 211 | mpu9250.gx*PI/180.00, mpu9250.gy*PI/180.00, mpu9250.gz*PI/180.00, |
jbeason3 | 0:f2207cfaf993 | 212 | mpu9250.mx, mpu9250.my, mpu9250.mz); |
jbeason3 | 0:f2207cfaf993 | 213 | |
jbeason3 | 0:f2207cfaf993 | 214 | // pc.printf("Q=%f,%f,%f,%f\r\n",mpu9250.q[0],mpu9250.q[1],mpu9250.q[2],mpu9250.q[3]); |
jbeason3 | 0:f2207cfaf993 | 215 | // Serial print and/or display at 0.5 s rate independent of data rates |
jbeason3 | 0:f2207cfaf993 | 216 | mpu9250.delt_t = t.read_ms() - mpu9250.count; |
jbeason3 | 0:f2207cfaf993 | 217 | if (mpu9250.delt_t > 100) { |
jbeason3 | 0:f2207cfaf993 | 218 | // pc.printf("ax = %f", 1000*mpu9250.ax); |
jbeason3 | 0:f2207cfaf993 | 219 | // pc.printf(" ay = %f", 1000*mpu9250.ay); |
jbeason3 | 0:f2207cfaf993 | 220 | // pc.printf(" az = %f mg\n\r", 1000*mpu9250.az); |
jbeason3 | 0:f2207cfaf993 | 221 | // pc.printf("gx = %f", mpu9250.gx); |
jbeason3 | 0:f2207cfaf993 | 222 | // pc.printf("gy = %f", mpu9250.gy); |
jbeason3 | 0:f2207cfaf993 | 223 | // pc.printf("gz = %f deg/s\n\r", mpu9250.gz); |
jbeason3 | 0:f2207cfaf993 | 224 | //pc.printf("mx = %f", mpu9250.mx); |
jbeason3 | 0:f2207cfaf993 | 225 | //pc.printf(" my = %f", mpu9250.my); |
jbeason3 | 0:f2207cfaf993 | 226 | //pc.printf(" mz = %f mG\n\r", mpu9250.mz); |
jbeason3 | 0:f2207cfaf993 | 227 | // tempCount = mpu9250.readTempData(); // Read the adc values |
jbeason3 | 0:f2207cfaf993 | 228 | // temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade |
jbeason3 | 0:f2207cfaf993 | 229 | // pc.printf(" temperature = %f C\n\r", temperature); |
jbeason3 | 0:f2207cfaf993 | 230 | // |
jbeason3 | 0:f2207cfaf993 | 231 | mpu9250.roll = atan2(2.0f * (mpu9250.q[0] * mpu9250.q[1] + mpu9250.q[2] * mpu9250.q[3]), |
jbeason3 | 0:f2207cfaf993 | 232 | mpu9250.q[0] * mpu9250.q[0] - mpu9250.q[1] * mpu9250.q[1] - mpu9250.q[2] * mpu9250.q[2] + mpu9250.q[3] * mpu9250.q[3]); |
jbeason3 | 0:f2207cfaf993 | 233 | mpu9250.pitch = -asin(2.0f * (mpu9250.q[1] * mpu9250.q[3] - mpu9250.q[0] * mpu9250.q[2])); |
jbeason3 | 0:f2207cfaf993 | 234 | mpu9250.yaw = atan2(2.0f * (mpu9250.q[1] * mpu9250.q[2] + mpu9250.q[0] * mpu9250.q[3]), |
jbeason3 | 0:f2207cfaf993 | 235 | mpu9250.q[0] * mpu9250.q[0] + mpu9250.q[1] * mpu9250.q[1] - mpu9250.q[2] * mpu9250.q[2] - mpu9250.q[3] * mpu9250.q[3]); |
jbeason3 | 0:f2207cfaf993 | 236 | mpu9250.pitch *= 180.0f / PI; |
jbeason3 | 0:f2207cfaf993 | 237 | mpu9250.yaw *= 180.0f / PI; |
jbeason3 | 0:f2207cfaf993 | 238 | mpu9250.yaw += 15.0f; // Declination at RI |
jbeason3 | 0:f2207cfaf993 | 239 | mpu9250.roll *= 180.0f / PI; |
jbeason3 | 0:f2207cfaf993 | 240 | |
jbeason3 | 0:f2207cfaf993 | 241 | |
jbeason3 | 0:f2207cfaf993 | 242 | |
jbeason3 | 0:f2207cfaf993 | 243 | |
jbeason3 | 0:f2207cfaf993 | 244 | Time = t.read(); |
jbeason3 | 0:f2207cfaf993 | 245 | set_servo_angle(); |
jbeason3 | 0:f2207cfaf993 | 246 | pc.printf("Pitch, Servo Angle, Time: %f %f %f %f\n\r", mpu9250.pitch, angle, Time); |
jbeason3 | 0:f2207cfaf993 | 247 | myled= !myled; |
jbeason3 | 0:f2207cfaf993 | 248 | mpu9250.count = t.read_ms(); |
jbeason3 | 0:f2207cfaf993 | 249 | |
jbeason3 | 0:f2207cfaf993 | 250 | if(mpu9250.count > 1<<21) { |
jbeason3 | 0:f2207cfaf993 | 251 | t.start(); // start the timer over again if ~30 minutes has passed |
jbeason3 | 0:f2207cfaf993 | 252 | mpu9250.count = 0; |
jbeason3 | 0:f2207cfaf993 | 253 | mpu9250.deltat= 0; |
jbeason3 | 0:f2207cfaf993 | 254 | mpu9250.lastUpdate = t.read_us(); |
jbeason3 | 0:f2207cfaf993 | 255 | } |
jbeason3 | 0:f2207cfaf993 | 256 | sum = 0; |
jbeason3 | 0:f2207cfaf993 | 257 | sumCount = 0; |
jbeason3 | 0:f2207cfaf993 | 258 | } |
jbeason3 | 0:f2207cfaf993 | 259 | |
jbeason3 | 0:f2207cfaf993 | 260 | |
jbeason3 | 0:f2207cfaf993 | 261 | } |
jbeason3 | 0:f2207cfaf993 | 262 | |
jbeason3 | 0:f2207cfaf993 | 263 | } |