commit!
Kalman/IR/IR.cpp@0:42026f893a2d, 2012-06-15 (annotated)
- Committer:
- narshu
- Date:
- Fri Jun 15 20:40:17 2012 +0000
- Revision:
- 0:42026f893a2d
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
narshu | 0:42026f893a2d | 1 | #include "IR.h" |
narshu | 0:42026f893a2d | 2 | #include "Kalman.h" |
narshu | 0:42026f893a2d | 3 | #include "system.h" |
narshu | 0:42026f893a2d | 4 | #include "geometryfuncs.h" |
narshu | 0:42026f893a2d | 5 | #include "globals.h" |
narshu | 0:42026f893a2d | 6 | #include "mbed.h" |
narshu | 0:42026f893a2d | 7 | |
narshu | 0:42026f893a2d | 8 | IR::IR(Kalman &kalmanin): |
narshu | 0:42026f893a2d | 9 | #ifdef ROBOT_PRIMARY |
narshu | 0:42026f893a2d | 10 | IRserial(p9, p10), |
narshu | 0:42026f893a2d | 11 | #else |
narshu | 0:42026f893a2d | 12 | IRserial(p13, p14), |
narshu | 0:42026f893a2d | 13 | #endif |
narshu | 0:42026f893a2d | 14 | kalman(kalmanin) { |
narshu | 0:42026f893a2d | 15 | |
narshu | 0:42026f893a2d | 16 | //Setting up IR serial |
narshu | 0:42026f893a2d | 17 | IRserial.baud(115200); |
narshu | 0:42026f893a2d | 18 | IRserial.format(8,Serial::Odd,1); |
narshu | 0:42026f893a2d | 19 | } |
narshu | 0:42026f893a2d | 20 | |
narshu | 0:42026f893a2d | 21 | void IR::detachisr() { |
narshu | 0:42026f893a2d | 22 | IRserial.attach(NULL,Serial::RxIrq); |
narshu | 0:42026f893a2d | 23 | } |
narshu | 0:42026f893a2d | 24 | |
narshu | 0:42026f893a2d | 25 | void IR::attachisr() { |
narshu | 0:42026f893a2d | 26 | IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); |
narshu | 0:42026f893a2d | 27 | } |
narshu | 0:42026f893a2d | 28 | |
narshu | 0:42026f893a2d | 29 | void IR::vIRValueISR (void) { |
narshu | 0:42026f893a2d | 30 | |
narshu | 0:42026f893a2d | 31 | // A workaround for mbed UART ISR bug |
narshu | 0:42026f893a2d | 32 | // Clear the RBR flag to make sure the interrupt doesn't loop |
narshu | 0:42026f893a2d | 33 | // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14. |
narshu | 0:42026f893a2d | 34 | // UART0 for USB UART |
narshu | 0:42026f893a2d | 35 | |
narshu | 0:42026f893a2d | 36 | #ifdef ROBOT_PRIMARY |
narshu | 0:42026f893a2d | 37 | unsigned char RBR = LPC_UART3->RBR; |
narshu | 0:42026f893a2d | 38 | #else |
narshu | 0:42026f893a2d | 39 | unsigned char RBR = LPC_UART1->RBR; |
narshu | 0:42026f893a2d | 40 | #endif |
narshu | 0:42026f893a2d | 41 | |
narshu | 0:42026f893a2d | 42 | // bytes packing/unpacking for IR turret serial comm |
narshu | 0:42026f893a2d | 43 | static union IRValue_t { |
narshu | 0:42026f893a2d | 44 | float IR_floats[3]; |
narshu | 0:42026f893a2d | 45 | int IR_ints[3]; |
narshu | 0:42026f893a2d | 46 | unsigned char IR_chars[12]; |
narshu | 0:42026f893a2d | 47 | } IRValues; |
narshu | 0:42026f893a2d | 48 | |
narshu | 0:42026f893a2d | 49 | const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC}; |
narshu | 0:42026f893a2d | 50 | static int Alignment_ptr = 0; |
narshu | 0:42026f893a2d | 51 | static bool data_flag = false; |
narshu | 0:42026f893a2d | 52 | static int buff_pointer = 0; |
narshu | 0:42026f893a2d | 53 | |
narshu | 0:42026f893a2d | 54 | if (!data_flag) { // look for alignment bytes |
narshu | 0:42026f893a2d | 55 | if (RBR == Alignment_char[Alignment_ptr]) { |
narshu | 0:42026f893a2d | 56 | Alignment_ptr ++; |
narshu | 0:42026f893a2d | 57 | } |
narshu | 0:42026f893a2d | 58 | if (Alignment_ptr >= 4) { |
narshu | 0:42026f893a2d | 59 | Alignment_ptr = 0; |
narshu | 0:42026f893a2d | 60 | data_flag = true; // set the dataflag |
narshu | 0:42026f893a2d | 61 | } |
narshu | 0:42026f893a2d | 62 | } else { // fetch data bytes |
narshu | 0:42026f893a2d | 63 | IRValues.IR_chars[buff_pointer] = RBR; |
narshu | 0:42026f893a2d | 64 | buff_pointer ++; |
narshu | 0:42026f893a2d | 65 | if (buff_pointer >= 12) { |
narshu | 0:42026f893a2d | 66 | buff_pointer = 0; |
narshu | 0:42026f893a2d | 67 | data_flag = false; // dessert the dataflag |
narshu | 0:42026f893a2d | 68 | kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRvariance); |
narshu | 0:42026f893a2d | 69 | |
narshu | 0:42026f893a2d | 70 | |
narshu | 0:42026f893a2d | 71 | } |
narshu | 0:42026f893a2d | 72 | |
narshu | 0:42026f893a2d | 73 | } |
narshu | 0:42026f893a2d | 74 | } |