Diff: IR/IR.cpp

Revision:

0:e238496b8073

Child:

2:8aa491f77a0b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR/IR.cpp	Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,77 @@
+#include "IR.h"
+#include "Kalman.h"
+#include "system.h"
+#include "geometryfuncs.h"
+
+IR::IR(Kalman &kalmanin):
+    IRserial(p13, p14),
+    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];
+            }
+        }
+
+    }
+}
\ No newline at end of file