File content as of revision 5:7ac07bf30707:

#include "IR.h"
#include "Kalman.h"
#include "system.h"
#include "geometryfuncs.h"
#include "globals.h"
#include "mbed.h"

IR::IR(Kalman &kalmanin):
#ifdef ROBOT_PRIMARY
        IRserial(p9, p10),
#else
        IRserial(p13, p14),
#endif
        kalman(kalmanin) {

    //Starting values of IR calibration
    angleInit = false;
    angleOffset = 0;

    //Setting up IR serial
    IRserial.baud(115200);
    IRserial.format(8,Serial::Odd,1);
}

void IR::detachisr() {
    IRserial.attach(NULL,Serial::RxIrq);
}

void IR::attachisr() {
    IRserial.attach(this, &IR::vIRValueISR, Serial::RxIrq);
}

void IR::vIRValueISR (void) {




    // 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
#ifdef ROBOT_PRIMARY
    unsigned char RBR = LPC_UART3->RBR;
#else
    unsigned char RBR = LPC_UART1->RBR;
#endif

    //printf("%#x/n", RBR);

    OLED3 = !OLED3;
    return;

    // bytes packing/unpacking for IR turret serial comm
    static union IRValue_t {
        float IR_floats[3];
        int IR_ints[3];
        unsigned char IR_chars[12];
    } IRValues;

    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



            /*
            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];
            }
            */


        }

    }
}