File content as of revision 69:47f800793d00:

#include "FIZCanon.h"
#include "LTCApp.h"
FIZCanon::FIZCanon(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
{
    inputPtr = 0;
    newData = false;
    _port.baud(19200);
    _port.format(8, SerialBase::Even, 1);
    _port.attach(callback(this, &FIZCanon::OnRx));
    missedPackets=0;

}

void FIZCanon::requestCurrent(void)
{
    if (missedPackets>5) {
        // configure output format
        _port.putc(0x80);
        _port.putc(0xC6);
        _port.putc(0xBF);
//        pc.printf("W");
        missedPackets = 0;
    }

    _port.putc(0x94);
    _port.putc(0xC1);
    _port.putc(0xBF);
    _port.putc(0x94);
    _port.putc(0xC3);
    _port.putc(0xBF);
    _port.putc(0x94);
    _port.putc(0xC5);
    _port.putc(0xBF);
    missedPackets++;
//    pc.printf("T");
}

//  expect hex data format:
//  [dat len] [command] [data] [cs]
// only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
// 07 60 [zoom1][zoom2] .... [cs]
void FIZCanon::OnRx(void)
{
    uint8_t dIn = _port.getc();
//    pc.putc('R');
    inputBuffer[inputPtr] = dIn;
    if (inputPtr==0) {
        if (dIn == 0x94) { // correct length
            inputPtr++;
        }
    } else if (inputPtr > 30) {
        inputPtr = 0;
    } else { // waiting for data.
        inputPtr++;
        if (inputPtr == (18) && dIn == 0xBF) { // full packet = 2 byte header, 7 byte data, 1 byte cs
            parsePacket();
            inputPtr = 0;
            if (missedPackets)
                missedPackets--;
        }
    }
}

void FIZCanon::parsePacket()
{
//    pc.puts("FIZ parse\r\n");
//    pc.putc('P');
    int read_pos = 0;
    bool valid = false;

    while (read_pos != inputPtr) {
        if (!valid) {
            if (inputBuffer[read_pos] == CANON_START) {
                valid = true;
            }
        } else {
            switch(inputBuffer[read_pos]) {
                case CANON_ZOOM:
//                    pc.putc('Z');
                    _zoom = (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
//                pc.putc(0x01);
//                pc.putc(inputBuffer[read_pos+1]);
//                pc.putc(inputBuffer[read_pos+2]);
//                pc.putc(inputBuffer[read_pos+3]);
                    read_pos += 4; // type + 3 BYTES OF DATA
                    break;
                case CANON_FOCUS:
//                    pc.putc('F');
                    _focus = 1000 - (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
//                pc.putc(0x02);
//                pc.putc(inputBuffer[read_pos+1]);
//                pc.putc(inputBuffer[read_pos+2]);
//                pc.putc(inputBuffer[read_pos+3]);
                    read_pos += 4;
                    break;
                case CANON_IRIS:
                    //IRIS NOT SCALED CORRECTLY -> 0x8000 - 0xD000, 0x1000 between FStops -> Fix Later
//                    pc.putc('I');
                    _iris = (((uint32_t)inputBuffer[read_pos +1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3]);
//                pc.putc(0x03);
//                pc.putc(inputBuffer[read_pos+1]);
//                pc.putc(inputBuffer[read_pos+2]);
//                pc.putc(inputBuffer[read_pos+3]);
                    read_pos += 4;
                    break;
                default:
                    //pc.putc('D');
                    break;
            }
            valid = false;
        } // if valid
        read_pos++;
    } // while

//// MAY NEED TO SCALE THESE
//        _focus = focus_Position;
//        _iris = iris_Position;
//        _zoom = zoom_Position;
//

    newData = true;
}