BE@R lab / Mbed 2 deprecated MPU9250AHRS-PGear_Stabilizer

Stabilizer

Dependencies:   BEAR_Protocol mbed Stabilizer iSerial

Fork of MPU9250AHRS by BE@R lab

main.cpp.orig@21:298aa522db64, 2016-02-03 (annotated)

Committer:
soulx
Date:
Wed Feb 03 16:23:11 2016 +0000
Revision:
21:298aa522db64
renew

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 21:298aa522db64 1 #include "BEAR_Protocol.h"
soulx 21:298aa522db64 2 #include "Stabilizer.h"
soulx 21:298aa522db64 3 #include "Kinematic.h"
soulx 21:298aa522db64 4 #include "MPU9250.h"
soulx 21:298aa522db64 5
soulx 21:298aa522db64 6 #include "param.h"
soulx 21:298aa522db64 7
soulx 21:298aa522db64 8 void Init_IMU();
soulx 21:298aa522db64 9 void Init_Stabilizer();
soulx 21:298aa522db64 10
soulx 21:298aa522db64 11
soulx 21:298aa522db64 12 float sum = 0;
soulx 21:298aa522db64 13 uint32_t sumCount = 0;
soulx 21:298aa522db64 14 char buffer[14];
soulx 21:298aa522db64 15
soulx 21:298aa522db64 16
soulx 21:298aa522db64 17 //init class
soulx 21:298aa522db64 18 MPU9250 mpu9250;
soulx 21:298aa522db64 19 Stabilizer Stabilize(5.0f,0.0f);
soulx 21:298aa522db64 20 Kinematic L('Z',10,10,30,30),R('Z',10,10,30,30);
soulx 21:298aa522db64 21
soulx 21:298aa522db64 22
soulx 21:298aa522db64 23 Timer t;
soulx 21:298aa522db64 24
soulx 21:298aa522db64 25 Serial pc(USBTX, USBRX); // tx, rx
soulx 21:298aa522db64 26
soulx 21:298aa522db64 27 Bear_Communicate bear(PA_15,PB_7,115200);
soulx 21:298aa522db64 28
soulx 21:298aa522db64 29
soulx 21:298aa522db64 30
soulx 21:298aa522db64 31 float xmax = -4914.0f;
soulx 21:298aa522db64 32 float xmin = 4914.0f;
soulx 21:298aa522db64 33
soulx 21:298aa522db64 34 float ymax = -4914.0;
soulx 21:298aa522db64 35 float ymin = 4914.0f;
soulx 21:298aa522db64 36
soulx 21:298aa522db64 37 float zmax = -4914.0;
soulx 21:298aa522db64 38 float zmin = 4914.0f;
soulx 21:298aa522db64 39
soulx 21:298aa522db64 40 float Xsf,Ysf;
soulx 21:298aa522db64 41 float Xoff,Yoff;
soulx 21:298aa522db64 42
soulx 21:298aa522db64 43
soulx 21:298aa522db64 44 //InterruptIn event(PC_13);
soulx 21:298aa522db64 45 DigitalIn enable(PC_13);
soulx 21:298aa522db64 46
soulx 21:298aa522db64 47 //DigitalIn button(USER_BUTTON);
soulx 21:298aa522db64 48
soulx 21:298aa522db64 49 void UI()
soulx 21:298aa522db64 50 {
soulx 21:298aa522db64 51
soulx 21:298aa522db64 52 }
soulx 21:298aa522db64 53
soulx 21:298aa522db64 54 void WheelChair()
soulx 21:298aa522db64 55 {
soulx 21:298aa522db64 56 //Start Here
soulx 21:298aa522db64 57 //Stabilize.set_Body_Lenght(5);
soulx 21:298aa522db64 58 Stabilize.set_current_zeta(roll);
soulx 21:298aa522db64 59 //Stabilize.set_zeta_set(0);
soulx 21:298aa522db64 60 //Stabilize.ZetaErrorCalculation();
soulx 21:298aa522db64 61 Stabilize.PID();
soulx 21:298aa522db64 62
soulx 21:298aa522db64 63 Stabilize.set_New_Height(L.get_Position_Z());
soulx 21:298aa522db64 64
soulx 21:298aa522db64 65 //pc.printf("Height : %f, delta : %f, New Height : %f\n",L.get_Position_Z(),Stabilize.get_delta_h(),Stabilize.get_New_Height());
soulx 21:298aa522db64 66 L.print();
soulx 21:298aa522db64 67 L.set_Position_Z(Stabilize.get_New_Height());
soulx 21:298aa522db64 68 L.InverseKinematicCalculation();
soulx 21:298aa522db64 69 L.print();
soulx 21:298aa522db64 70
soulx 21:298aa522db64 71 R.set_Position_Y(L.get_Position_Y()+Stabilize.get_Offset_Y());
soulx 21:298aa522db64 72 R.set_Position_Z(L.get_Position_Z()+Stabilize.get_Offset_Z());
soulx 21:298aa522db64 73 //R.set_offset_YZ(3,3);
soulx 21:298aa522db64 74 //R.SumPositionWithOffset();
soulx 21:298aa522db64 75 R.InverseKinematicCalculation();
soulx 21:298aa522db64 76 R.print();
soulx 21:298aa522db64 77 pc.printf("\n");
soulx 21:298aa522db64 78
soulx 21:298aa522db64 79 //Send Position of L&R Angle to Motion Board
soulx 21:298aa522db64 80
soulx 21:298aa522db64 81
soulx 21:298aa522db64 82
soulx 21:298aa522db64 83 //End Here
soulx 21:298aa522db64 84 }
soulx 21:298aa522db64 85
soulx 21:298aa522db64 86 int main()
soulx 21:298aa522db64 87 {
soulx 21:298aa522db64 88 pc.baud(115200);
soulx 21:298aa522db64 89
soulx 21:298aa522db64 90
soulx 21:298aa522db64 91 /*while(1){
soulx 21:298aa522db64 92 Kinematic test('P',10,15,10,10);
soulx 21:298aa522db64 93 pc.printf("\n\nLink Hip : %f, Link Knee : %f, Position Y : %f, Position Z : %f\n",test.get_Link_Hip(),test.get_Link_Knee(),test.get_Position_Y(),test.get_Position_Z());
soulx 21:298aa522db64 94 pc.printf("Zeta Hip : %f, Zeta Knee : %f\n",test.get_Zeta_Hip(),test.get_Zeta_Knee());
soulx 21:298aa522db64 95 test.set_Link_Hip(25);
soulx 21:298aa522db64 96 test.set_Link_Knee(30);
soulx 21:298aa522db64 97 test.set_Position_Y(35);
soulx 21:298aa522db64 98 test.set_Position_Z(40);
soulx 21:298aa522db64 99 test.set_Zeta_Hip(45);
soulx 21:298aa522db64 100 test.set_Zeta_Knee(50);
soulx 21:298aa522db64 101 pc.printf("\nLink Hip : %f, Link Knee : %f, Position Y : %f, Position Z : %f\n",test.get_Link_Hip(),test.get_Link_Knee(),test.get_Position_Y(),test.get_Position_Z());
soulx 21:298aa522db64 102 pc.printf("Zeta Hip : %f, Zeta Knee : %f\n",test.get_Zeta_Hip(),test.get_Zeta_Knee());
soulx 21:298aa522db64 103
soulx 21:298aa522db64 104 while(1);
soulx 21:298aa522db64 105 }*/
soulx 21:298aa522db64 106
soulx 21:298aa522db64 107 Init_IMU();
soulx 21:298aa522db64 108
soulx 21:298aa522db64 109 float temp_time;
soulx 21:298aa522db64 110
soulx 21:298aa522db64 111 while(1) {
soulx 21:298aa522db64 112 temp_time = t.read();
soulx 21:298aa522db64 113 // If intPin goes high, all data registers have new data
soulx 21:298aa522db64 114 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
soulx 21:298aa522db64 115
soulx 21:298aa522db64 116 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
soulx 21:298aa522db64 117 // Now we'll calculate the accleration value into actual g's
soulx 21:298aa522db64 118 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
soulx 21:298aa522db64 119 ay = (float)accelCount[1]*aRes - accelBias[1];
soulx 21:298aa522db64 120 az = (float)accelCount[2]*aRes - accelBias[2];
soulx 21:298aa522db64 121
soulx 21:298aa522db64 122 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
soulx 21:298aa522db64 123 // Calculate the gyro value into actual degrees per second
soulx 21:298aa522db64 124 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
soulx 21:298aa522db64 125 gy = (float)gyroCount[1]*gRes - gyroBias[1];
soulx 21:298aa522db64 126 gz = (float)gyroCount[2]*gRes - gyroBias[2];
soulx 21:298aa522db64 127
soulx 21:298aa522db64 128 mpu9250.readMagData(magCount); // Read the x/y/z adc values
soulx 21:298aa522db64 129 // Calculate the magnetometer values in milliGauss
soulx 21:298aa522db64 130 // Include factory calibration per data sheet and user environmental corrections
soulx 21:298aa522db64 131 /* if(magCount[0]<xmin)
soulx 21:298aa522db64 132 xmin = magCount[0];
soulx 21:298aa522db64 133 if(magCount[0]>xmax)
soulx 21:298aa522db64 134 xmax = magCount[0];
soulx 21:298aa522db64 135
soulx 21:298aa522db64 136 if(magCount[1]<ymin)
soulx 21:298aa522db64 137 ymin = magCount[1];
soulx 21:298aa522db64 138 if(magCount[1]>ymax)
soulx 21:298aa522db64 139 ymax = magCount[1];
soulx 21:298aa522db64 140
soulx 21:298aa522db64 141 if(magCount[2]<zmin)
soulx 21:298aa522db64 142 zmin = magCount[2];
soulx 21:298aa522db64 143 if(mz>zmax)
soulx 21:298aa522db64 144 zmax = mz;
soulx 21:298aa522db64 145 wait_ms(1);
soulx 21:298aa522db64 146 */
soulx 21:298aa522db64 147 // pc.printf("FINISH scan\r\n\r\n");
soulx 21:298aa522db64 148
soulx 21:298aa522db64 149 // mx = (float)magCount[0]*mRes*magCalibration[0] + magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 21:298aa522db64 150 // my = (float)magCount[1]*mRes*magCalibration[1] + magbias[1];
soulx 21:298aa522db64 151 // mz = (float)magCount[2]*mRes*magCalibration[2] + magbias[2];
soulx 21:298aa522db64 152
soulx 21:298aa522db64 153 mx = ((float)magCount[0]-xmin)*magCalibration[0] + magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 21:298aa522db64 154 my = ((float)magCount[1]-ymin)*magCalibration[1] + magbias[1];
soulx 21:298aa522db64 155 mz = ((float)magCount[2]-zmin)*magCalibration[2] + magbias[2];
soulx 21:298aa522db64 156
soulx 21:298aa522db64 157 // mx = (float)magCount[0]*1.499389499f - magbias[0]; // get actual magnetometer value, this depends on scale being set
soulx 21:298aa522db64 158 // my = (float)magCount[1]*1.499389499f - magbias[1];
soulx 21:298aa522db64 159 // mz = (float)magCount[2]*1.499389499f - magbias[2];
soulx 21:298aa522db64 160
soulx 21:298aa522db64 161
soulx 21:298aa522db64 162
soulx 21:298aa522db64 163
soulx 21:298aa522db64 164 }
soulx 21:298aa522db64 165
soulx 21:298aa522db64 166 Now = t.read_us();
soulx 21:298aa522db64 167 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
soulx 21:298aa522db64 168 lastUpdate = Now;
soulx 21:298aa522db64 169
soulx 21:298aa522db64 170 sum += deltat;
soulx 21:298aa522db64 171 sumCount++;
soulx 21:298aa522db64 172
soulx 21:298aa522db64 173 // if(lastUpdate - firstUpdate > 10000000.0f) {
soulx 21:298aa522db64 174 // beta = 0.04; // decrease filter gain after stabilized
soulx 21:298aa522db64 175 // zeta = 0.015; // increasey bias drift gain after stabilized
soulx 21:298aa522db64 176 // }
soulx 21:298aa522db64 177
soulx 21:298aa522db64 178 // Pass gyro rate as rad/s
soulx 21:298aa522db64 179 //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
soulx 21:298aa522db64 180 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
soulx 21:298aa522db64 181
soulx 21:298aa522db64 182 // Serial print and/or display at 0.5 s rate independent of data rates
soulx 21:298aa522db64 183 delt_t = t.read_ms() - count;
soulx 21:298aa522db64 184 if(temp_time > 8) {
soulx 21:298aa522db64 185 if (delt_t > 500) { // update LCD once per half-second independent of read rate
soulx 21:298aa522db64 186
soulx 21:298aa522db64 187 /*pc.printf("ax = %f", 1000*ax);
soulx 21:298aa522db64 188 pc.printf(" ay = %f", 1000*ay);
soulx 21:298aa522db64 189 pc.printf(" az = %f mg\n\r", 1000*az);
soulx 21:298aa522db64 190
soulx 21:298aa522db64 191 pc.printf("gx = %f", gx);
soulx 21:298aa522db64 192 pc.printf(" gy = %f", gy);
soulx 21:298aa522db64 193 pc.printf(" gz = %f deg/s\n\r", gz);
soulx 21:298aa522db64 194
soulx 21:298aa522db64 195 pc.printf("mx = %f", mx);
soulx 21:298aa522db64 196 pc.printf(" my = %f", my);
soulx 21:298aa522db64 197 pc.printf(" mz = %f mG\n\r", mz);*/
soulx 21:298aa522db64 198
soulx 21:298aa522db64 199 uint8_t whoami = mpu9250.readByte(AK8963_ADDRESS, AK8963_ST2); // Read WHO_AM_I register for MPU-9250
soulx 21:298aa522db64 200 // pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x10\n\r");
soulx 21:298aa522db64 201 if(whoami == 0x14) {
soulx 21:298aa522db64 202 pc.printf("I AM 0x%x\n\r", whoami);
soulx 21:298aa522db64 203 while(1);
soulx 21:298aa522db64 204 }
soulx 21:298aa522db64 205
soulx 21:298aa522db64 206
soulx 21:298aa522db64 207 tempCount = mpu9250.readTempData(); // Read the adc values
soulx 21:298aa522db64 208 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
soulx 21:298aa522db64 209 //pc.printf(" temperature = %f C\n\r", temperature);
soulx 21:298aa522db64 210
soulx 21:298aa522db64 211 // pc.printf("q0 = %f\n\r", q[0]);
soulx 21:298aa522db64 212 // pc.printf("q1 = %f\n\r", q[1]);
soulx 21:298aa522db64 213 // pc.printf("q2 = %f\n\r", q[2]);
soulx 21:298aa522db64 214 // pc.printf("q3 = %f\n\r", q[3]);
soulx 21:298aa522db64 215
soulx 21:298aa522db64 216 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
soulx 21:298aa522db64 217 // In this coordinate system, the positive z-axis is down toward Earth.
soulx 21:298aa522db64 218 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
soulx 21:298aa522db64 219 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
soulx 21:298aa522db64 220 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
soulx 21:298aa522db64 221 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
soulx 21:298aa522db64 222 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
soulx 21:298aa522db64 223 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
soulx 21:298aa522db64 224 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
soulx 21:298aa522db64 225 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
soulx 21:298aa522db64 226 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
soulx 21:298aa522db64 227 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
soulx 21:298aa522db64 228
soulx 21:298aa522db64 229 float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
soulx 21:298aa522db64 230 float Yh = my*cos(roll)+mz*sin(roll);
soulx 21:298aa522db64 231
soulx 21:298aa522db64 232 float yawmag = atan2(Yh,Xh)+PI;
soulx 21:298aa522db64 233 //pc.printf("Xh= %f Yh= %f ",Xh,Yh);
soulx 21:298aa522db64 234 //pc.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
soulx 21:298aa522db64 235
soulx 21:298aa522db64 236
soulx 21:298aa522db64 237
soulx 21:298aa522db64 238 pitch *= 180.0f / PI;
soulx 21:298aa522db64 239 yaw *= 180.0f / PI;
soulx 21:298aa522db64 240 yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
soulx 21:298aa522db64 241 roll *= 180.0f / PI;
soulx 21:298aa522db64 242
soulx 21:298aa522db64 243 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
soulx 21:298aa522db64 244 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
soulx 21:298aa522db64 245
soulx 21:298aa522db64 246
soulx 21:298aa522db64 247 WheelChair();
soulx 21:298aa522db64 248
soulx 21:298aa522db64 249
soulx 21:298aa522db64 250 myled= !myled;
soulx 21:298aa522db64 251 count = t.read_ms();
soulx 21:298aa522db64 252
soulx 21:298aa522db64 253 if(count > 1<<21) {
soulx 21:298aa522db64 254 t.start(); // start the timer over again if ~30 minutes has passed
soulx 21:298aa522db64 255 count = 0;
soulx 21:298aa522db64 256 deltat= 0;
soulx 21:298aa522db64 257 lastUpdate = t.read_us();
soulx 21:298aa522db64 258 }
soulx 21:298aa522db64 259 sum = 0;
soulx 21:298aa522db64 260 sumCount = 0;
soulx 21:298aa522db64 261 }
soulx 21:298aa522db64 262 }
soulx 21:298aa522db64 263 }
soulx 21:298aa522db64 264 }
soulx 21:298aa522db64 265
soulx 21:298aa522db64 266 void Init_IMU()
soulx 21:298aa522db64 267 {
soulx 21:298aa522db64 268
soulx 21:298aa522db64 269 //Set up I2C
soulx 21:298aa522db64 270 i2c.frequency(400000); // use fast (400 kHz) I2C
soulx 21:298aa522db64 271
soulx 21:298aa522db64 272 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
soulx 21:298aa522db64 273
soulx 21:298aa522db64 274 t.start();
soulx 21:298aa522db64 275
soulx 21:298aa522db64 276 //mpu9250.resetMPU9250();
soulx 21:298aa522db64 277 // Read the WHO_AM_I register, this is a good test of communication
soulx 21:298aa522db64 278 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
soulx 21:298aa522db64 279 pc.printf("I AM 0x%x\n\r", whoami);
soulx 21:298aa522db64 280 pc.printf("I SHOULD BE 0x71\n\r");
soulx 21:298aa522db64 281
soulx 21:298aa522db64 282 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
soulx 21:298aa522db64 283 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
soulx 21:298aa522db64 284 pc.printf("MPU9250 is online...\n\r");
soulx 21:298aa522db64 285 sprintf(buffer, "0x%x", whoami);
soulx 21:298aa522db64 286 wait(1);
soulx 21:298aa522db64 287
soulx 21:298aa522db64 288 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
soulx 21:298aa522db64 289 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
soulx 21:298aa522db64 290 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
soulx 21:298aa522db64 291 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
soulx 21:298aa522db64 292 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
soulx 21:298aa522db64 293 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
soulx 21:298aa522db64 294 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
soulx 21:298aa522db64 295 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
soulx 21:298aa522db64 296 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
soulx 21:298aa522db64 297 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
soulx 21:298aa522db64 298 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
soulx 21:298aa522db64 299 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
soulx 21:298aa522db64 300 pc.printf("x accel bias = %f\n\r", accelBias[0]);
soulx 21:298aa522db64 301 pc.printf("y accel bias = %f\n\r", accelBias[1]);
soulx 21:298aa522db64 302 pc.printf("z accel bias = %f\n\r", accelBias[2]);
soulx 21:298aa522db64 303 wait(2);
soulx 21:298aa522db64 304 mpu9250.initMPU9250();
soulx 21:298aa522db64 305 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
soulx 21:298aa522db64 306 mpu9250.initAK8963(magCalibration);
soulx 21:298aa522db64 307 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
soulx 21:298aa522db64 308
soulx 21:298aa522db64 309 whoami = mpu9250.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for MPU-9250
soulx 21:298aa522db64 310 pc.printf("I AM 0x%x\n\r", whoami);
soulx 21:298aa522db64 311 pc.printf("I SHOULD BE 0x48\n\r");
soulx 21:298aa522db64 312 if(whoami != 0x48) {
soulx 21:298aa522db64 313 while(1);
soulx 21:298aa522db64 314 }
soulx 21:298aa522db64 315 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
soulx 21:298aa522db64 316 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
soulx 21:298aa522db64 317 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
soulx 21:298aa522db64 318 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
soulx 21:298aa522db64 319 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
soulx 21:298aa522db64 320 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
soulx 21:298aa522db64 321 wait(1);
soulx 21:298aa522db64 322 } else {
soulx 21:298aa522db64 323 pc.printf("Could not connect to MPU9250: \n\r");
soulx 21:298aa522db64 324 pc.printf("%#x \n", whoami);
soulx 21:298aa522db64 325
soulx 21:298aa522db64 326 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
soulx 21:298aa522db64 327
soulx 21:298aa522db64 328 while(1) ; // Loop forever if communication doesn't happen
soulx 21:298aa522db64 329 }
soulx 21:298aa522db64 330
soulx 21:298aa522db64 331 mpu9250.getAres(); // Get accelerometer sensitivity
soulx 21:298aa522db64 332 mpu9250.getGres(); // Get gyro sensitivity
soulx 21:298aa522db64 333 mpu9250.getMres(); // Get magnetometer sensitivity
soulx 21:298aa522db64 334 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
soulx 21:298aa522db64 335 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
soulx 21:298aa522db64 336 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
soulx 21:298aa522db64 337
soulx 21:298aa522db64 338 /*pc.printf("START scan mag\n\r\n\r\n\r");
soulx 21:298aa522db64 339 //wait(1);
soulx 21:298aa522db64 340 for(int i=0; i<4000; i++) {
soulx 21:298aa522db64 341 mpu9250.readMagData(magCount);
soulx 21:298aa522db64 342
soulx 21:298aa522db64 343 if(magCount[0]<xmin)
soulx 21:298aa522db64 344 xmin = magCount[0];
soulx 21:298aa522db64 345 if(magCount[0]>xmax)
soulx 21:298aa522db64 346 xmax = magCount[0];
soulx 21:298aa522db64 347
soulx 21:298aa522db64 348 if(magCount[1]<ymin)
soulx 21:298aa522db64 349 ymin = magCount[1];
soulx 21:298aa522db64 350 if(magCount[1]>ymax)
soulx 21:298aa522db64 351 ymax = magCount[1];
soulx 21:298aa522db64 352
soulx 21:298aa522db64 353 if(magCount[2]<zmin)
soulx 21:298aa522db64 354 zmin = magCount[2];
soulx 21:298aa522db64 355 if(magCount[2]>zmax)
soulx 21:298aa522db64 356 zmax = magCount[2];
soulx 21:298aa522db64 357
soulx 21:298aa522db64 358
soulx 21:298aa522db64 359 wait_ms(10);
soulx 21:298aa522db64 360 }
soulx 21:298aa522db64 361 pc.printf("FINISH scan\r\n\r\n");
soulx 21:298aa522db64 362 pc.printf("Mx Max= %f Min= %f\n\r",xmax,xmin);
soulx 21:298aa522db64 363 pc.printf("My Max= %f Min= %f\n\r",ymax,ymin);
soulx 21:298aa522db64 364 pc.printf("Mz Max= %f Min= %f\n\r",zmax,zmin);*/
soulx 21:298aa522db64 365
soulx 21:298aa522db64 366 /*xmax = 188.000000;
soulx 21:298aa522db64 367 xmin = -316.000000;
soulx 21:298aa522db64 368 ymax = 485.000000;
soulx 21:298aa522db64 369 ymin = -26.000000;
soulx 21:298aa522db64 370 zmax = 165.000000;
soulx 21:298aa522db64 371 xmin = -230.000000;
soulx 21:298aa522db64 372
soulx 21:298aa522db64 373 //Ice room
soulx 21:298aa522db64 374 xmax = 101.000000;
soulx 21:298aa522db64 375 xmin = -296.000000;
soulx 21:298aa522db64 376 ymax = 320.000000;
soulx 21:298aa522db64 377 ymin = -85.000000;
soulx 21:298aa522db64 378 zmax = 208.000000;
soulx 21:298aa522db64 379 xmin = -202.000000;
soulx 21:298aa522db64 380
soulx 21:298aa522db64 381 xmax = 115.000000;
soulx 21:298aa522db64 382 xmin = -309.000000;
soulx 21:298aa522db64 383 ymax = 350.000000;
soulx 21:298aa522db64 384 ymin = -119.000000;
soulx 21:298aa522db64 385 zmax = 235.000000;
soulx 21:298aa522db64 386 zmin = -224.000000;*/
soulx 21:298aa522db64 387
soulx 21:298aa522db64 388 xmax = 120.000000;
soulx 21:298aa522db64 389 xmin = -306.000000;
soulx 21:298aa522db64 390 ymax = 340.000000;
soulx 21:298aa522db64 391 ymin = -90.000000;
soulx 21:298aa522db64 392 zmax = 219.000000;
soulx 21:298aa522db64 393 zmin = -195.000000;
soulx 21:298aa522db64 394
soulx 21:298aa522db64 395
soulx 21:298aa522db64 396
soulx 21:298aa522db64 397 magbias[0] = -1.0;
soulx 21:298aa522db64 398 magbias[1] = -1.0;
soulx 21:298aa522db64 399 magbias[2] = -1.0;
soulx 21:298aa522db64 400
soulx 21:298aa522db64 401 magCalibration[0] = 2.0f / (xmax -xmin);
soulx 21:298aa522db64 402 magCalibration[1] = 2.0f / (ymax -ymin);
soulx 21:298aa522db64 403 magCalibration[2] = 2.0f / (zmax -zmin);
soulx 21:298aa522db64 404
soulx 21:298aa522db64 405 //magbias[0] = (xmin-xmax)/2.0f; // User environmental x-axis correction in milliGauss, should be automatically calculated
soulx 21:298aa522db64 406 //magbias[1] = (ymin-ymax)/2.0f; // User environmental x-axis correction in milliGauss
soulx 21:298aa522db64 407 //magbias[2] = (zmin-zmax)/2.0f; // User environmental x-axis correction in milliGauss
soulx 21:298aa522db64 408 pc.printf("mag[0] %f",magbias[0]);
soulx 21:298aa522db64 409 pc.printf("mag[1] %f",magbias[1]);
soulx 21:298aa522db64 410 pc.printf("mag[2] %f\n\r",magbias[2]);
soulx 21:298aa522db64 411 // resalt = atan(magY+((yMin-yMax)/2),magX+(xMin-xMax)/2))*180/PI;
soulx 21:298aa522db64 412
soulx 21:298aa522db64 413 }
soulx 21:298aa522db64 414
soulx 21:298aa522db64 415
soulx 21:298aa522db64 416 void Init_Stabilizer()
soulx 21:298aa522db64 417 {
soulx 21:298aa522db64 418
soulx 21:298aa522db64 419
soulx 21:298aa522db64 420
soulx 21:298aa522db64 421
soulx 21:298aa522db64 422 }