Audren Cloitre / Mbed 2 deprecated UM6LT_v1

Same library as the one in "UM6LT" but this publication has a main file to show how to use the library.

Dependencies:   UM6LT mbed

main.cpp@0:c5fbc6927336, 2012-09-30 (annotated)

Committer:
acloitre
Date:
Sun Sep 30 21:00:04 2012 +0000
Revision:
0:c5fbc6927336
Same library as the one published under the Name UM6LT but this one include s a main file to show how to use th elibrary.; Still not the best way to implement the code/make good use of this hardware but it works.; Set the IMU in silent mode (broadcast di...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acloitre 0:c5fbc6927336 1 #include "mbed.h"
acloitre 0:c5fbc6927336 2 #include "UM6LT.h"
acloitre 0:c5fbc6927336 3
acloitre 0:c5fbc6927336 4 Serial pc(USBTX, USBRX);
acloitre 0:c5fbc6927336 5 UM6LT imu(p28, p27);
acloitre 0:c5fbc6927336 6
acloitre 0:c5fbc6927336 7 int stdDelayMs = 2;
acloitre 0:c5fbc6927336 8
acloitre 0:c5fbc6927336 9 int main() {
acloitre 0:c5fbc6927336 10
acloitre 0:c5fbc6927336 11 pc.baud(115200);
acloitre 0:c5fbc6927336 12 imu.baud(115200);
acloitre 0:c5fbc6927336 13
acloitre 0:c5fbc6927336 14 int broadcastRate = 22;
acloitre 0:c5fbc6927336 15 int baudrate = 115200;
acloitre 0:c5fbc6927336 16
acloitre 0:c5fbc6927336 17 int wantCov = 0;
acloitre 0:c5fbc6927336 18 int wantEulerAngles = 0;
acloitre 0:c5fbc6927336 19 int wantQuat = 0;
acloitre 0:c5fbc6927336 20 int wantProcMag = 0;
acloitre 0:c5fbc6927336 21 int wantProcAccel = 0;
acloitre 0:c5fbc6927336 22 int wantProcGyro = 0;
acloitre 0:c5fbc6927336 23 int wantRawMag = 0;
acloitre 0:c5fbc6927336 24 int wantRawAccel = 0;
acloitre 0:c5fbc6927336 25 int wantRawGyro = 0;
acloitre 0:c5fbc6927336 26
acloitre 0:c5fbc6927336 27 int dataToTransmit [9] = {wantCov, wantEulerAngles, wantQuat, wantProcMag, wantProcAccel, wantProcGyro, wantRawMag, wantRawAccel, wantRawGyro};
acloitre 0:c5fbc6927336 28 int broadcastEnabled = 0;
acloitre 0:c5fbc6927336 29
acloitre 0:c5fbc6927336 30 imu.setCommParams(broadcastRate, baudrate, dataToTransmit, broadcastEnabled);
acloitre 0:c5fbc6927336 31
acloitre 0:c5fbc6927336 32 int wantPPS = 0;
acloitre 0:c5fbc6927336 33 int wantQuatUpdate = 1;
acloitre 0:c5fbc6927336 34 int wantCal = 1;
acloitre 0:c5fbc6927336 35 int wantAccelUpdate = 1;
acloitre 0:c5fbc6927336 36 int wantMagUpdate = 1;
acloitre 0:c5fbc6927336 37
acloitre 0:c5fbc6927336 38 imu.setConfigParams(wantPPS, wantQuatUpdate, wantCal, wantAccelUpdate, wantMagUpdate);
acloitre 0:c5fbc6927336 39
acloitre 0:c5fbc6927336 40 if(imu.getStatus()){
acloitre 0:c5fbc6927336 41
acloitre 0:c5fbc6927336 42 int roll = 0;
acloitre 0:c5fbc6927336 43 int pitch = 0;
acloitre 0:c5fbc6927336 44 int yaw = 0;
acloitre 0:c5fbc6927336 45 int rollRate = 0;
acloitre 0:c5fbc6927336 46 int pitchRate = 0;
acloitre 0:c5fbc6927336 47 int yawRate = 0;
acloitre 0:c5fbc6927336 48 int accelX = 0 ;
acloitre 0:c5fbc6927336 49 int accelY = 0 ;
acloitre 0:c5fbc6927336 50 int accelZ = 0 ;
acloitre 0:c5fbc6927336 51 int magX = 0;
acloitre 0:c5fbc6927336 52 int magY = 0;
acloitre 0:c5fbc6927336 53 int magZ = 0;
acloitre 0:c5fbc6927336 54 int gyroBiasX = 0;
acloitre 0:c5fbc6927336 55 int gyroBiasY = 0;
acloitre 0:c5fbc6927336 56 int gyroBiasZ = 0;
acloitre 0:c5fbc6927336 57 int a = 0;
acloitre 0:c5fbc6927336 58 int b = 0;
acloitre 0:c5fbc6927336 59 int c = 0;
acloitre 0:c5fbc6927336 60 int d = 0;
acloitre 0:c5fbc6927336 61
acloitre 0:c5fbc6927336 62 int byteBuffer;
acloitre 0:c5fbc6927336 63
acloitre 0:c5fbc6927336 64 for(int i=0; i<5; i++){
acloitre 0:c5fbc6927336 65 if(imu.getAngles(roll, pitch, yaw)){
acloitre 0:c5fbc6927336 66 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 67 printf("roll: %d pitch: %d yaw: %d\r\n", roll, pitch, yaw);
acloitre 0:c5fbc6927336 68 }
acloitre 0:c5fbc6927336 69 }
acloitre 0:c5fbc6927336 70
acloitre 0:c5fbc6927336 71 printf("\r\n\r\nAngles should be random but consistent\r\n\r\n");
acloitre 0:c5fbc6927336 72
acloitre 0:c5fbc6927336 73 if(imu.zeroGyros(gyroBiasX, gyroBiasY,gyroBiasZ)){
acloitre 0:c5fbc6927336 74 printf("Gyro Bias X: %d Gyro Bias Y: %d Gyro Bias Z: %d\r\n", gyroBiasX, gyroBiasY, gyroBiasZ);
acloitre 0:c5fbc6927336 75 }
acloitre 0:c5fbc6927336 76
acloitre 0:c5fbc6927336 77 if(imu.autoSetAccelRef() && imu.autoSetMagRef() && imu.resetEKF()){
acloitre 0:c5fbc6927336 78
acloitre 0:c5fbc6927336 79 printf("Press (1) for Euler Angles.\r\n");
acloitre 0:c5fbc6927336 80 printf("Press (2) for Accelerations.\r\n");
acloitre 0:c5fbc6927336 81 printf("Press (3) for Magnetic field.\r\n");
acloitre 0:c5fbc6927336 82 printf("Press (4) for Angle rates.\r\n");
acloitre 0:c5fbc6927336 83 printf("Press (5) for Quaternion.\r\n");
acloitre 0:c5fbc6927336 84 printf("Press (0) to stop.\r\n");
acloitre 0:c5fbc6927336 85
acloitre 0:c5fbc6927336 86 while(!pc.readable()){
acloitre 0:c5fbc6927336 87 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 88 }
acloitre 0:c5fbc6927336 89 byteBuffer = pc.getc();
acloitre 0:c5fbc6927336 90
acloitre 0:c5fbc6927336 91 while(1){
acloitre 0:c5fbc6927336 92
acloitre 0:c5fbc6927336 93 if(pc.readable()){
acloitre 0:c5fbc6927336 94 byteBuffer = pc.getc();
acloitre 0:c5fbc6927336 95 }
acloitre 0:c5fbc6927336 96
acloitre 0:c5fbc6927336 97 switch(byteBuffer){
acloitre 0:c5fbc6927336 98 case '0':
acloitre 0:c5fbc6927336 99 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 100 break;
acloitre 0:c5fbc6927336 101 case '1':
acloitre 0:c5fbc6927336 102 if(imu.getAngles(roll, pitch, yaw)){
acloitre 0:c5fbc6927336 103 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 104 printf("roll: %d pitch: %d yaw: %d\r\n", roll, pitch, yaw);
acloitre 0:c5fbc6927336 105 }
acloitre 0:c5fbc6927336 106 break;
acloitre 0:c5fbc6927336 107 case '2':
acloitre 0:c5fbc6927336 108 if(imu.getAccel(accelX, accelY, accelZ)){
acloitre 0:c5fbc6927336 109 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 110 printf("accelX: %d accelY: %d accelZ: %d\r\n", accelX, accelY, accelZ);
acloitre 0:c5fbc6927336 111 }
acloitre 0:c5fbc6927336 112 break;
acloitre 0:c5fbc6927336 113 case '3':
acloitre 0:c5fbc6927336 114 if(imu.getMag(magX, magY, magZ)){
acloitre 0:c5fbc6927336 115 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 116 printf("magX: %d magY: %d magZ: %d\r\n", magX, magY, magZ);
acloitre 0:c5fbc6927336 117 }
acloitre 0:c5fbc6927336 118 break;
acloitre 0:c5fbc6927336 119 case '4':
acloitre 0:c5fbc6927336 120 if(imu.getAngleRates(rollRate, pitchRate, yawRate)){
acloitre 0:c5fbc6927336 121 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 122 printf("rollRate: %d pitchRate: %d yawRate: %d\r\n", rollRate, pitchRate, yawRate);
acloitre 0:c5fbc6927336 123 }
acloitre 0:c5fbc6927336 124 break;
acloitre 0:c5fbc6927336 125 case '5':
acloitre 0:c5fbc6927336 126 if(imu.getQuaternion(a, b, c, d)){
acloitre 0:c5fbc6927336 127 wait_ms(stdDelayMs);
acloitre 0:c5fbc6927336 128 printf("a: %d b: %d c: %d d: %d\r\n", a, b, c, d);
acloitre 0:c5fbc6927336 129 }
acloitre 0:c5fbc6927336 130 break;
acloitre 0:c5fbc6927336 131 default:
acloitre 0:c5fbc6927336 132 printf("Wrong command. Enter '1', '2', '3', '4', '5' or '6'.\r\n");
acloitre 0:c5fbc6927336 133 byteBuffer = '0';
acloitre 0:c5fbc6927336 134 break;
acloitre 0:c5fbc6927336 135 }
acloitre 0:c5fbc6927336 136 }
acloitre 0:c5fbc6927336 137
acloitre 0:c5fbc6927336 138 }
acloitre 0:c5fbc6927336 139 }
acloitre 0:c5fbc6927336 140 }