Audren Cloitre
/
UM6LT_v1
Same library as the one in "UM6LT" but this publication has a main file to show how to use the library.
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?
User | Revision | Line number | New 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 | } |