Shuto Naruse
IR/IR.cpp@2:8aa491f77a0b, 2012-04-26 (annotated)
- Committer:
- narshu
- Date:
- Thu Apr 26 20:11:48 2012 +0000
- Revision:
- 2:8aa491f77a0b
- Parent:
- 0:e238496b8073
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| narshu | 0:e238496b8073 | 1 | #include "IR.h" |
| narshu | 0:e238496b8073 | 2 | #include "Kalman.h" |
| narshu | 0:e238496b8073 | 3 | #include "system.h" |
| narshu | 0:e238496b8073 | 4 | #include "geometryfuncs.h" |
| narshu | 0:e238496b8073 | 5 | |
| narshu | 2:8aa491f77a0b | 6 | IR::IR(Kalman &kalmanin, PinName TX, PinName RX): |
| narshu | 2:8aa491f77a0b | 7 | IRserial(TX, RX), |
| narshu | 0:e238496b8073 | 8 | kalman(kalmanin) { |
| narshu | 0:e238496b8073 | 9 | |
| narshu | 0:e238496b8073 | 10 | //Starting values of IR calibration |
| narshu | 0:e238496b8073 | 11 | angleInit = false; |
| narshu | 0:e238496b8073 | 12 | angleOffset = 0; |
| narshu | 0:e238496b8073 | 13 | |
| narshu | 0:e238496b8073 | 14 | //Setting up IR serial |
| narshu | 0:e238496b8073 | 15 | IRserial.baud(115200); |
| narshu | 0:e238496b8073 | 16 | IRserial.format(8,Serial::Odd,1); |
| narshu | 0:e238496b8073 | 17 | IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); |
| narshu | 0:e238496b8073 | 18 | |
| narshu | 0:e238496b8073 | 19 | } |
| narshu | 0:e238496b8073 | 20 | |
| narshu | 0:e238496b8073 | 21 | void IR::detachisr(){ |
| narshu | 0:e238496b8073 | 22 | IRserial.attach(NULL,Serial::RxIrq); |
| narshu | 0:e238496b8073 | 23 | } |
| narshu | 0:e238496b8073 | 24 | |
| narshu | 0:e238496b8073 | 25 | void IR::attachisr(){ |
| narshu | 0:e238496b8073 | 26 | IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); |
| narshu | 0:e238496b8073 | 27 | } |
| narshu | 0:e238496b8073 | 28 | |
| narshu | 0:e238496b8073 | 29 | void IR::vIRValueISR (void) { |
| narshu | 0:e238496b8073 | 30 | |
| narshu | 0:e238496b8073 | 31 | // bytes packing/unpacking for IR turret serial comm |
| narshu | 0:e238496b8073 | 32 | union IRValue_t { |
| narshu | 0:e238496b8073 | 33 | float IR_floats[3]; |
| narshu | 0:e238496b8073 | 34 | int IR_ints[3]; |
| narshu | 0:e238496b8073 | 35 | unsigned char IR_chars[12]; |
| narshu | 0:e238496b8073 | 36 | } IRValues; |
| narshu | 0:e238496b8073 | 37 | |
| narshu | 0:e238496b8073 | 38 | |
| narshu | 0:e238496b8073 | 39 | // A workaround for mbed UART ISR bug |
| narshu | 0:e238496b8073 | 40 | // Clear the RBR flag to make sure the interrupt doesn't loop |
| narshu | 0:e238496b8073 | 41 | // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14. |
| narshu | 0:e238496b8073 | 42 | // UART0 for USB UART |
| narshu | 0:e238496b8073 | 43 | unsigned char RBR = LPC_UART1->RBR; |
| narshu | 0:e238496b8073 | 44 | |
| narshu | 0:e238496b8073 | 45 | const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC}; |
| narshu | 0:e238496b8073 | 46 | static int Alignment_ptr = 0; |
| narshu | 0:e238496b8073 | 47 | static bool data_flag = false; |
| narshu | 0:e238496b8073 | 48 | static int buff_pointer = 0; |
| narshu | 0:e238496b8073 | 49 | |
| narshu | 0:e238496b8073 | 50 | if (!data_flag) { // look for alignment bytes |
| narshu | 0:e238496b8073 | 51 | if (RBR == Alignment_char[Alignment_ptr]) { |
| narshu | 0:e238496b8073 | 52 | Alignment_ptr ++; |
| narshu | 0:e238496b8073 | 53 | } |
| narshu | 0:e238496b8073 | 54 | if (Alignment_ptr >= 4) { |
| narshu | 0:e238496b8073 | 55 | Alignment_ptr = 0; |
| narshu | 0:e238496b8073 | 56 | data_flag = true; // set the dataflag |
| narshu | 0:e238496b8073 | 57 | } |
| narshu | 0:e238496b8073 | 58 | } else { // fetch data bytes |
| narshu | 0:e238496b8073 | 59 | IRValues.IR_chars[buff_pointer] = RBR; |
| narshu | 0:e238496b8073 | 60 | buff_pointer ++; |
| narshu | 0:e238496b8073 | 61 | if (buff_pointer >= 12) { |
| narshu | 0:e238496b8073 | 62 | buff_pointer = 0; |
| narshu | 0:e238496b8073 | 63 | data_flag = false; // dessert the dataflag |
| narshu | 0:e238496b8073 | 64 | OLED3 = !OLED3; |
| narshu | 0:e238496b8073 | 65 | if (angleInit) { |
| narshu | 0:e238496b8073 | 66 | kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]); |
| narshu | 0:e238496b8073 | 67 | } else { |
| narshu | 0:e238496b8073 | 68 | //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix |
| narshu | 0:e238496b8073 | 69 | //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]); |
| narshu | 0:e238496b8073 | 70 | |
| narshu | 0:e238496b8073 | 71 | //only update the IRMeasures used by kalman init |
| narshu | 0:e238496b8073 | 72 | kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1]; |
| narshu | 0:e238496b8073 | 73 | } |
| narshu | 0:e238496b8073 | 74 | } |
| narshu | 0:e238496b8073 | 75 | |
| narshu | 0:e238496b8073 | 76 | } |
| narshu | 0:e238496b8073 | 77 | } |