Austin Ward
Library to interface mbed with ArduIMU
Arduimu.cpp@0:d0d40a6fe11c, 2014-03-24 (annotated)
- Committer:
- ifwui
- Date:
- Mon Mar 24 20:26:15 2014 +0000
- Revision:
- 0:d0d40a6fe11c
Initial Commit
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ifwui | 0:d0d40a6fe11c | 1 | #include "Arduimu.h" |
| ifwui | 0:d0d40a6fe11c | 2 | #include <cstring> |
| ifwui | 0:d0d40a6fe11c | 3 | |
| ifwui | 0:d0d40a6fe11c | 4 | #define preamble "DIYd" |
| ifwui | 0:d0d40a6fe11c | 5 | |
| ifwui | 0:d0d40a6fe11c | 6 | typedef union { |
| ifwui | 0:d0d40a6fe11c | 7 | int16_t integerVal; |
| ifwui | 0:d0d40a6fe11c | 8 | char bytes[2]; |
| ifwui | 0:d0d40a6fe11c | 9 | } value_t; |
| ifwui | 0:d0d40a6fe11c | 10 | |
| ifwui | 0:d0d40a6fe11c | 11 | |
| ifwui | 0:d0d40a6fe11c | 12 | Arduimu::Arduimu(PinName tx, PinName rx):imu(tx,rx) |
| ifwui | 0:d0d40a6fe11c | 13 | { |
| ifwui | 0:d0d40a6fe11c | 14 | imu.baud(38400); |
| ifwui | 0:d0d40a6fe11c | 15 | imu.attach(this,&Arduimu::receive); |
| ifwui | 0:d0d40a6fe11c | 16 | } |
| ifwui | 0:d0d40a6fe11c | 17 | |
| ifwui | 0:d0d40a6fe11c | 18 | float Arduimu::getRoll() |
| ifwui | 0:d0d40a6fe11c | 19 | { |
| ifwui | 0:d0d40a6fe11c | 20 | return roll; |
| ifwui | 0:d0d40a6fe11c | 21 | } |
| ifwui | 0:d0d40a6fe11c | 22 | float Arduimu::getPitch() |
| ifwui | 0:d0d40a6fe11c | 23 | { |
| ifwui | 0:d0d40a6fe11c | 24 | return pitch; |
| ifwui | 0:d0d40a6fe11c | 25 | } |
| ifwui | 0:d0d40a6fe11c | 26 | float Arduimu::getYaw() |
| ifwui | 0:d0d40a6fe11c | 27 | { |
| ifwui | 0:d0d40a6fe11c | 28 | return yaw; |
| ifwui | 0:d0d40a6fe11c | 29 | } |
| ifwui | 0:d0d40a6fe11c | 30 | void Arduimu::getOrientation(float& roll, float& pitch, float& yaw) |
| ifwui | 0:d0d40a6fe11c | 31 | { |
| ifwui | 0:d0d40a6fe11c | 32 | roll = this->roll; |
| ifwui | 0:d0d40a6fe11c | 33 | pitch = this->pitch; |
| ifwui | 0:d0d40a6fe11c | 34 | yaw = this->yaw; |
| ifwui | 0:d0d40a6fe11c | 35 | } |
| ifwui | 0:d0d40a6fe11c | 36 | |
| ifwui | 0:d0d40a6fe11c | 37 | void Arduimu::receive() |
| ifwui | 0:d0d40a6fe11c | 38 | { |
| ifwui | 0:d0d40a6fe11c | 39 | char chk_a = 0; |
| ifwui | 0:d0d40a6fe11c | 40 | char chk_b = 0; |
| ifwui | 0:d0d40a6fe11c | 41 | value_t temp; |
| ifwui | 0:d0d40a6fe11c | 42 | temp.integerVal = 0; |
| ifwui | 0:d0d40a6fe11c | 43 | //The message is 14 bytes with a preamble of 'DIYd' + 0x06 + 0x02 |
| ifwui | 0:d0d40a6fe11c | 44 | char msgBuf[14]; |
| ifwui | 0:d0d40a6fe11c | 45 | //Wait for next message |
| ifwui | 0:d0d40a6fe11c | 46 | while(imu.getc() != 'D'); |
| ifwui | 0:d0d40a6fe11c | 47 | __disable_irq(); // Disable Interrupts so we dont loose data |
| ifwui | 0:d0d40a6fe11c | 48 | msgBuf[0] = 'D'; |
| ifwui | 0:d0d40a6fe11c | 49 | msgBuf[1] = imu.getc(); //I |
| ifwui | 0:d0d40a6fe11c | 50 | msgBuf[2] = imu.getc(); //Y |
| ifwui | 0:d0d40a6fe11c | 51 | msgBuf[3] = imu.getc(); //d |
| ifwui | 0:d0d40a6fe11c | 52 | msgBuf[4] = imu.getc(); //0x06 |
| ifwui | 0:d0d40a6fe11c | 53 | msgBuf[5] = imu.getc(); //0x02 |
| ifwui | 0:d0d40a6fe11c | 54 | |
| ifwui | 0:d0d40a6fe11c | 55 | if(strcmp(msgBuf,preamble) && msgBuf[4] == 0x06 && msgBuf[5] == 0x02) { |
| ifwui | 0:d0d40a6fe11c | 56 | //we have a valid message |
| ifwui | 0:d0d40a6fe11c | 57 | //Fill in the rest of the values |
| ifwui | 0:d0d40a6fe11c | 58 | for(int i = 6; i < 14; i++) { |
| ifwui | 0:d0d40a6fe11c | 59 | msgBuf[i] = imu.getc(); |
| ifwui | 0:d0d40a6fe11c | 60 | } |
| ifwui | 0:d0d40a6fe11c | 61 | __enable_irq(); // Enable Interrupts |
| ifwui | 0:d0d40a6fe11c | 62 | for(int i = 6; i < 12; i++) { |
| ifwui | 0:d0d40a6fe11c | 63 | chk_a+=msgBuf[i]; |
| ifwui | 0:d0d40a6fe11c | 64 | chk_b+=chk_a; |
| ifwui | 0:d0d40a6fe11c | 65 | } |
| ifwui | 0:d0d40a6fe11c | 66 | |
| ifwui | 0:d0d40a6fe11c | 67 | temp.bytes[0] = msgBuf[6]; |
| ifwui | 0:d0d40a6fe11c | 68 | temp.bytes[1] = msgBuf[7]; |
| ifwui | 0:d0d40a6fe11c | 69 | roll = (float) temp.integerVal / 100.0; |
| ifwui | 0:d0d40a6fe11c | 70 | |
| ifwui | 0:d0d40a6fe11c | 71 | temp.bytes[0] = msgBuf[8]; |
| ifwui | 0:d0d40a6fe11c | 72 | temp.bytes[1] = msgBuf[9]; |
| ifwui | 0:d0d40a6fe11c | 73 | pitch = (float) temp.integerVal / 100.0; |
| ifwui | 0:d0d40a6fe11c | 74 | |
| ifwui | 0:d0d40a6fe11c | 75 | temp.bytes[0] = msgBuf[10]; |
| ifwui | 0:d0d40a6fe11c | 76 | temp.bytes[1] = msgBuf[11]; |
| ifwui | 0:d0d40a6fe11c | 77 | yaw = (float) temp.integerVal / 100.0; |
| ifwui | 0:d0d40a6fe11c | 78 | } else |
| ifwui | 0:d0d40a6fe11c | 79 | __enable_irq(); // Enable Interrupts |
| ifwui | 0:d0d40a6fe11c | 80 | } |
| ifwui | 0:d0d40a6fe11c | 81 | |
| ifwui | 0:d0d40a6fe11c | 82 | void Arduimu::putc(char c) |
| ifwui | 0:d0d40a6fe11c | 83 | { |
| ifwui | 0:d0d40a6fe11c | 84 | imu.putc(c); |
| ifwui | 0:d0d40a6fe11c | 85 | } |
ArduIMU