Jordan Beason / main

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?

UserRevisionLine numberNew 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 }