勇輝 関本 / Mbed 2 deprecated SkipperS2mputest

SkipperS2v2 and bmp

Dependencies:   BMP280 mpu9250forskipper mbed

main.cpp@2:c1c7afb86416, 2016-08-22 (annotated)

Committer:
motoseki
Date:
Mon Aug 22 02:51:01 2016 +0000
Revision:
2:c1c7afb86416
Parent:
1:5c8e961811d1 
for bmp280

Who changed what in which revision?

UserRevisionLine numberNew contents of line
motoseki 0:e6b1cd3646df 1 #include "mbed.h"
motoseki 0:e6b1cd3646df 2 #include "MPU9250.h"
motoseki 1:5c8e961811d1 3 #include "BMP280.h"
motoseki 1:5c8e961811d1 4
motoseki 1:5c8e961811d1 5 #define bmpadr 0x76<<1 // SD0 pulldown 0x76<<1 , pullup 0x77<<1
motoseki 1:5c8e961811d1 6
motoseki 1:5c8e961811d1 7 //I2C SDA,SCL: PC_9, PA_8
motoseki 0:e6b1cd3646df 8
motoseki 0:e6b1cd3646df 9 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
motoseki 0:e6b1cd3646df 10 DigitalOut cs(PA_15);
motoseki 1:5c8e961811d1 11 Serial pc(PA_2,PA_3); //v2
motoseki 1:5c8e961811d1 12 //Serial pc(PA_9,PA_10); //original
motoseki 0:e6b1cd3646df 13
motoseki 0:e6b1cd3646df 14 float sum = 0;
motoseki 0:e6b1cd3646df 15 uint32_t sumCount = 0;
motoseki 2:c1c7afb86416 16 float bmpTEM , bmpPRE;
motoseki 0:e6b1cd3646df 17 MPU9250 mpu9250;
motoseki 1:5c8e961811d1 18 BMP280 bmp(PC_9, PA_8 ,bmpadr); //(PinName sda, PinName scl, char slave_adr)
motoseki 0:e6b1cd3646df 19 Timer t;
motoseki 2:c1c7afb86416 20 float PRES_first;
motoseki 0:e6b1cd3646df 21
motoseki 0:e6b1cd3646df 22 int main() {
motoseki 1:5c8e961811d1 23 wait(2);
motoseki 0:e6b1cd3646df 24 pc.baud(9600);
motoseki 0:e6b1cd3646df 25 //a8=0;
motoseki 0:e6b1cd3646df 26 //c9=0;
motoseki 0:e6b1cd3646df 27 pc.printf("SkipperS2 Peripheral Test...\n");wait(1);
motoseki 0:e6b1cd3646df 28 pc.printf("L2G2IS SPI 0xD9?\n");
motoseki 0:e6b1cd3646df 29 // Chip must be deselected
motoseki 0:e6b1cd3646df 30 cs = 1;
motoseki 0:e6b1cd3646df 31 // Setup the spi for 8 bit data, high steady state clock,
motoseki 0:e6b1cd3646df 32 // second edge capture, with a 1MHz clock rate
motoseki 0:e6b1cd3646df 33 spi.format(8,3);
motoseki 0:e6b1cd3646df 34 spi.frequency(1000000);
motoseki 0:e6b1cd3646df 35 // Select the device by seting chip select low
motoseki 0:e6b1cd3646df 36 cs = 0;
motoseki 0:e6b1cd3646df 37 // Send 0x8f, the command to read the WHOAMI register
motoseki 0:e6b1cd3646df 38 spi.write(0x80);
motoseki 0:e6b1cd3646df 39 // Send a dummy byte to receive the contents of the WHOAMI register
motoseki 0:e6b1cd3646df 40 int whoami_l2 = spi.write(0x00);
motoseki 0:e6b1cd3646df 41 pc.printf("WHOAMI register = 0x%X\n", whoami_l2);
motoseki 0:e6b1cd3646df 42 // Deselect the device
motoseki 0:e6b1cd3646df 43 cs = 1;
motoseki 0:e6b1cd3646df 44
motoseki 0:e6b1cd3646df 45 pc.printf("MPU9250 I2C test\n");
motoseki 0:e6b1cd3646df 46
motoseki 0:e6b1cd3646df 47
motoseki 0:e6b1cd3646df 48
motoseki 0:e6b1cd3646df 49
motoseki 0:e6b1cd3646df 50 //Set up I2C
motoseki 0:e6b1cd3646df 51 i2c.frequency(400000); // use fast (400 kHz) I2C
motoseki 0:e6b1cd3646df 52
motoseki 0:e6b1cd3646df 53 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
motoseki 0:e6b1cd3646df 54
motoseki 0:e6b1cd3646df 55 t.start();
motoseki 0:e6b1cd3646df 56
motoseki 1:5c8e961811d1 57 //BMP initialize
motoseki 1:5c8e961811d1 58 bmp.initialize();
motoseki 2:c1c7afb86416 59 PRES_first = bmp.getPressure();
motoseki 0:e6b1cd3646df 60
motoseki 0:e6b1cd3646df 61 // Read the WHO_AM_I register, this is a good test of communication
motoseki 0:e6b1cd3646df 62 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
motoseki 0:e6b1cd3646df 63 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
motoseki 0:e6b1cd3646df 64
motoseki 0:e6b1cd3646df 65 if (whoami == 0x71) // WHO_AM_I should always be 0x68
motoseki 0:e6b1cd3646df 66 {
motoseki 0:e6b1cd3646df 67 pc.printf("MPU9250 is online...\n\r");
motoseki 0:e6b1cd3646df 68 wait(1);
motoseki 0:e6b1cd3646df 69
motoseki 0:e6b1cd3646df 70
motoseki 0:e6b1cd3646df 71 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
motoseki 0:e6b1cd3646df 72 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
motoseki 0:e6b1cd3646df 73 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
motoseki 0:e6b1cd3646df 74 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
motoseki 0:e6b1cd3646df 75 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
motoseki 0:e6b1cd3646df 76 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
motoseki 0:e6b1cd3646df 77 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
motoseki 0:e6b1cd3646df 78 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
motoseki 0:e6b1cd3646df 79 wait(2);
motoseki 0:e6b1cd3646df 80 mpu9250.initMPU9250();
motoseki 0:e6b1cd3646df 81 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
motoseki 0:e6b1cd3646df 82 mpu9250.initAK8963(magCalibration);
motoseki 0:e6b1cd3646df 83 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
motoseki 0:e6b1cd3646df 84 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
motoseki 0:e6b1cd3646df 85 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
motoseki 0:e6b1cd3646df 86 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
motoseki 0:e6b1cd3646df 87 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
motoseki 0:e6b1cd3646df 88 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
motoseki 0:e6b1cd3646df 89 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
motoseki 0:e6b1cd3646df 90 wait(2);
motoseki 0:e6b1cd3646df 91 }
motoseki 0:e6b1cd3646df 92 else
motoseki 0:e6b1cd3646df 93 {
motoseki 0:e6b1cd3646df 94 pc.printf("Could not connect to MPU9250: \n\r");
motoseki 0:e6b1cd3646df 95 pc.printf("%#x \n", whoami);
motoseki 0:e6b1cd3646df 96
motoseki 2:c1c7afb86416 97
motoseki 0:e6b1cd3646df 98 while(1) ; // Loop forever if communication doesn't happen
motoseki 0:e6b1cd3646df 99 }
motoseki 0:e6b1cd3646df 100
motoseki 0:e6b1cd3646df 101 mpu9250.getAres(); // Get accelerometer sensitivity
motoseki 0:e6b1cd3646df 102 mpu9250.getGres(); // Get gyro sensitivity
motoseki 0:e6b1cd3646df 103 mpu9250.getMres(); // Get magnetometer sensitivity
motoseki 0:e6b1cd3646df 104 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
motoseki 0:e6b1cd3646df 105 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
motoseki 0:e6b1cd3646df 106 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
motoseki 0:e6b1cd3646df 107 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
motoseki 0:e6b1cd3646df 108 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
motoseki 0:e6b1cd3646df 109 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
motoseki 0:e6b1cd3646df 110
motoseki 0:e6b1cd3646df 111 while(1) {
motoseki 0:e6b1cd3646df 112
motoseki 0:e6b1cd3646df 113 // If intPin goes high, all data registers have new data
motoseki 0:e6b1cd3646df 114 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
motoseki 0:e6b1cd3646df 115
motoseki 0:e6b1cd3646df 116 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
motoseki 0:e6b1cd3646df 117 // Now we'll calculate the accleration value into actual g's
motoseki 0:e6b1cd3646df 118 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
motoseki 0:e6b1cd3646df 119 ay = (float)accelCount[1]*aRes - accelBias[1];
motoseki 0:e6b1cd3646df 120 az = (float)accelCount[2]*aRes - accelBias[2];
motoseki 0:e6b1cd3646df 121
motoseki 0:e6b1cd3646df 122 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
motoseki 0:e6b1cd3646df 123 // Calculate the gyro value into actual degrees per second
motoseki 0:e6b1cd3646df 124 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
motoseki 0:e6b1cd3646df 125 gy = (float)gyroCount[1]*gRes - gyroBias[1];
motoseki 0:e6b1cd3646df 126 gz = (float)gyroCount[2]*gRes - gyroBias[2];
motoseki 0:e6b1cd3646df 127
motoseki 0:e6b1cd3646df 128 mpu9250.readMagData(magCount); // Read the x/y/z adc values
motoseki 0:e6b1cd3646df 129 // Calculate the magnetometer values in milliGauss
motoseki 0:e6b1cd3646df 130 // Include factory calibration per data sheet and user environmental corrections
motoseki 0:e6b1cd3646df 131 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
motoseki 0:e6b1cd3646df 132 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
motoseki 0:e6b1cd3646df 133 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
motoseki 0:e6b1cd3646df 134 }
motoseki 0:e6b1cd3646df 135
motoseki 0:e6b1cd3646df 136 Now = t.read_us();
motoseki 0:e6b1cd3646df 137 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
motoseki 0:e6b1cd3646df 138 lastUpdate = Now;
motoseki 0:e6b1cd3646df 139
motoseki 0:e6b1cd3646df 140 sum += deltat;
motoseki 0:e6b1cd3646df 141 sumCount++;
motoseki 0:e6b1cd3646df 142
motoseki 0:e6b1cd3646df 143 // if(lastUpdate - firstUpdate > 10000000.0f) {
motoseki 0:e6b1cd3646df 144 // beta = 0.04; // decrease filter gain after stabilized
motoseki 0:e6b1cd3646df 145 // zeta = 0.015; // increasey bias drift gain after stabilized
motoseki 0:e6b1cd3646df 146 // }
motoseki 0:e6b1cd3646df 147
motoseki 0:e6b1cd3646df 148 // Pass gyro rate as rad/s
motoseki 0:e6b1cd3646df 149 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
motoseki 0:e6b1cd3646df 150 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
motoseki 0:e6b1cd3646df 151
motoseki 0:e6b1cd3646df 152 // Serial print and/or display at 0.5 s rate independent of data rates
motoseki 0:e6b1cd3646df 153 delt_t = t.read_ms() - count;
motoseki 0:e6b1cd3646df 154 if (delt_t > 500) { // update LCD once per half-second independent of read rate
motoseki 0:e6b1cd3646df 155
motoseki 0:e6b1cd3646df 156 //pc.printf("ax = %f", 1000*ax);
motoseki 0:e6b1cd3646df 157 //pc.printf(" ay = %f", 1000*ay);
motoseki 0:e6b1cd3646df 158 //pc.printf(" az = %f mg\n\r", 1000*az);
motoseki 0:e6b1cd3646df 159
motoseki 1:5c8e961811d1 160 //pc.printf("gx = %f", gx);
motoseki 1:5c8e961811d1 161 //pc.printf(" gy = %f", gy);
motoseki 1:5c8e961811d1 162 //pc.printf(" gz = %f deg/s\n\r", gz);
motoseki 0:e6b1cd3646df 163
motoseki 0:e6b1cd3646df 164 //pc.printf("gx = %f", mx);
motoseki 0:e6b1cd3646df 165 //pc.printf(" gy = %f", my);
motoseki 0:e6b1cd3646df 166 //pc.printf(" gz = %f mG\n\r", mz);
motoseki 0:e6b1cd3646df 167
motoseki 0:e6b1cd3646df 168 tempCount = mpu9250.readTempData(); // Read the adc values
motoseki 0:e6b1cd3646df 169 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
motoseki 0:e6b1cd3646df 170 //pc.printf(" temperature = %f C\n\r", temperature);
motoseki 0:e6b1cd3646df 171
motoseki 0:e6b1cd3646df 172 //pc.printf("q0 = %f\n\r", q[0]);
motoseki 0:e6b1cd3646df 173 //pc.printf("q1 = %f\n\r", q[1]);
motoseki 0:e6b1cd3646df 174 //pc.printf("q2 = %f\n\r", q[2]);
motoseki 0:e6b1cd3646df 175 //pc.printf("q3 = %f\n\r", q[3]);
motoseki 0:e6b1cd3646df 176
motoseki 0:e6b1cd3646df 177
motoseki 0:e6b1cd3646df 178 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
motoseki 0:e6b1cd3646df 179 // In this coordinate system, the positive z-axis is down toward Earth.
motoseki 0:e6b1cd3646df 180 // 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.
motoseki 0:e6b1cd3646df 181 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
motoseki 0:e6b1cd3646df 182 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
motoseki 0:e6b1cd3646df 183 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
motoseki 0:e6b1cd3646df 184 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
motoseki 0:e6b1cd3646df 185 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
motoseki 0:e6b1cd3646df 186 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
motoseki 0:e6b1cd3646df 187 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]);
motoseki 0:e6b1cd3646df 188 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
motoseki 0:e6b1cd3646df 189 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]);
motoseki 0:e6b1cd3646df 190 pitch *= 180.0f / PI;
motoseki 0:e6b1cd3646df 191 yaw *= 180.0f / PI;
motoseki 0:e6b1cd3646df 192 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
motoseki 0:e6b1cd3646df 193 roll *= 180.0f / PI;
motoseki 0:e6b1cd3646df 194
motoseki 2:c1c7afb86416 195 //pc.printf("Yaw, Pitch, Roll: %f %f %f \n\r", yaw, pitch, roll);
motoseki 0:e6b1cd3646df 196 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
motoseki 2:c1c7afb86416 197
motoseki 2:c1c7afb86416 198 bmpTEM=bmp.getTemperature();
motoseki 2:c1c7afb86416 199 bmpPRE=bmp.getPressure();
motoseki 2:c1c7afb86416 200 pc.printf("PRE : %f\n",bmpPRE - PRES_first);
motoseki 0:e6b1cd3646df 201
motoseki 0:e6b1cd3646df 202 myled= !myled;
motoseki 0:e6b1cd3646df 203 count = t.read_ms();
motoseki 0:e6b1cd3646df 204 sum = 0;
motoseki 0:e6b1cd3646df 205 sumCount = 0;
motoseki 0:e6b1cd3646df 206 }
motoseki 0:e6b1cd3646df 207 }
motoseki 0:e6b1cd3646df 208
motoseki 0:e6b1cd3646df 209 while(1){
motoseki 0:e6b1cd3646df 210 pc.printf(".");
motoseki 0:e6b1cd3646df 211 wait(1);
motoseki 0:e6b1cd3646df 212 }
motoseki 0:e6b1cd3646df 213 }
motoseki 0:e6b1cd3646df 214