alonso palomino / Mbed OS ST

iot

Dependencies:   MPU9250 mbed-os

main.cpp@1:3eec9883598a, 2017-10-12 (annotated)

Committer:
alonsopg
Date:
Thu Oct 12 18:47:26 2017 +0000
Revision:
1:3eec9883598a
Parent:
0:30a995e45e2a
Child:
2:02175845b24c
before integrating bluetooth

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alonsopg 0:30a995e45e2a 1 #include "mbed.h"
alonsopg 0:30a995e45e2a 2 #include "MPU9250.h"
alonsopg 0:30a995e45e2a 3
alonsopg 0:30a995e45e2a 4 // Serial comms
alonsopg 0:30a995e45e2a 5 Serial pc(USBTX, USBRX);
alonsopg 0:30a995e45e2a 6
alonsopg 0:30a995e45e2a 7 // Sensor board library
alonsopg 0:30a995e45e2a 8 MPU9250 mpu = MPU9250(p26, p27);
alonsopg 0:30a995e45e2a 9
alonsopg 0:30a995e45e2a 10 // Configuration
alonsopg 1:3eec9883598a 11 bool testMPUConnection = true;
alonsopg 1:3eec9883598a 12 bool doSensorInitialization = false;
alonsopg 1:3eec9883598a 13 bool printAccelerometer = true;
alonsopg 1:3eec9883598a 14 bool printGyroscope = true;
alonsopg 0:30a995e45e2a 15
alonsopg 0:30a995e45e2a 16 int main () {
alonsopg 1:3eec9883598a 17 int16_t accelerometer[3] = {0,0,0};
alonsopg 1:3eec9883598a 18 int16_t gyroscope[3] = {0,0,0};
alonsopg 1:3eec9883598a 19
alonsopg 1:3eec9883598a 20 if (testMPUConnection) {
alonsopg 0:30a995e45e2a 21 uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
alonsopg 0:30a995e45e2a 22 pc.printf("I AM 0x%x\n\r", whoami);
alonsopg 0:30a995e45e2a 23 pc.printf("I SHOULD BE 0x71\n\r");
alonsopg 0:30a995e45e2a 24 wait(1);
alonsopg 0:30a995e45e2a 25 }
alonsopg 0:30a995e45e2a 26
alonsopg 1:3eec9883598a 27 if (doSensorInitialization) {
alonsopg 1:3eec9883598a 28 // Initialise sensor board
alonsopg 1:3eec9883598a 29 pc.printf("Initializing sensor\n\r");
alonsopg 1:3eec9883598a 30 mpu.initMPU9250();
alonsopg 1:3eec9883598a 31 pc.printf("Initialization finished\n\r");
alonsopg 1:3eec9883598a 32 wait(1);
alonsopg 1:3eec9883598a 33 }
alonsopg 1:3eec9883598a 34
alonsopg 0:30a995e45e2a 35 while(1) {
alonsopg 1:3eec9883598a 36 pc.printf("Starting to stream data");
alonsopg 1:3eec9883598a 37 if(printAccelerometer){
alonsopg 1:3eec9883598a 38 mpu.readAccelData(accelerometer);
alonsopg 1:3eec9883598a 39 float ax = accelerometer[0] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 40 float ay = accelerometer[1] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 41 float az = accelerometer[2] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 42
alonsopg 1:3eec9883598a 43 pc.printf("Acelerometer information, AX, AY, AZ \n");
alonsopg 1:3eec9883598a 44 pc.printf("(%f, %f, %f)\n", ax,ay,az);
alonsopg 1:3eec9883598a 45
alonsopg 1:3eec9883598a 46 float roll = float(atan2(ay, az) * 180/3.1416f);
alonsopg 1:3eec9883598a 47 float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
alonsopg 1:3eec9883598a 48 float yaw = atan(ax/-ay);
alonsopg 1:3eec9883598a 49
alonsopg 1:3eec9883598a 50 pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);
alonsopg 1:3eec9883598a 51 }
alonsopg 1:3eec9883598a 52
alonsopg 1:3eec9883598a 53 if(printGyroscope){
alonsopg 1:3eec9883598a 54 mpu.readGyroData(gyroscope);
alonsopg 1:3eec9883598a 55 float gx = gyroscope[0] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 56 float gy = gyroscope[1] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 57 float gz = gyroscope[2] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 58 pc.printf("Gyroscope information, GX, GY, GZ \n");
alonsopg 1:3eec9883598a 59 pc.printf("(:%f, %f, %f)\n", gx,gy,gz);
alonsopg 1:3eec9883598a 60 }
alonsopg 1:3eec9883598a 61
alonsopg 1:3eec9883598a 62 wait(0.3);
alonsopg 0:30a995e45e2a 63 }
alonsopg 0:30a995e45e2a 64 }