IR/IR.cpp
- Committer:
- narshu
- Date:
- 2012-04-26
- Revision:
- 2:8aa491f77a0b
- Parent:
- 0:e238496b8073
File content as of revision 2:8aa491f77a0b:
#include "IR.h" #include "Kalman.h" #include "system.h" #include "geometryfuncs.h" IR::IR(Kalman &kalmanin, PinName TX, PinName RX): IRserial(TX, RX), kalman(kalmanin) { //Starting values of IR calibration angleInit = false; angleOffset = 0; //Setting up IR serial IRserial.baud(115200); IRserial.format(8,Serial::Odd,1); IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); } void IR::detachisr(){ IRserial.attach(NULL,Serial::RxIrq); } void IR::attachisr(){ IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq); } void IR::vIRValueISR (void) { // bytes packing/unpacking for IR turret serial comm union IRValue_t { float IR_floats[3]; int IR_ints[3]; unsigned char IR_chars[12]; } IRValues; // A workaround for mbed UART ISR bug // Clear the RBR flag to make sure the interrupt doesn't loop // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14. // UART0 for USB UART unsigned char RBR = LPC_UART1->RBR; const char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC}; static int Alignment_ptr = 0; static bool data_flag = false; static int buff_pointer = 0; if (!data_flag) { // look for alignment bytes if (RBR == Alignment_char[Alignment_ptr]) { Alignment_ptr ++; } if (Alignment_ptr >= 4) { Alignment_ptr = 0; data_flag = true; // set the dataflag } } else { // fetch data bytes IRValues.IR_chars[buff_pointer] = RBR; buff_pointer ++; if (buff_pointer >= 12) { buff_pointer = 0; data_flag = false; // dessert the dataflag OLED3 = !OLED3; if (angleInit) { kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1] - angleOffset),IRValues.IR_floats[2]); } else { //dont bother to update if we dont know the offset of the IR, as it messes up the P matrix //kalman->runupdate(kalman.measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]); //only update the IRMeasures used by kalman init kalman.IRMeasures[IRValues.IR_ints[0]] = IRValues.IR_floats[1]; } } } }