File content as of revision 85:0cc5931bb9ef:

#include "FIZ_ArriCmotion.h"
#include "LTCApp.h"

FIZ_ArriCmotion::FIZ_ArriCmotion(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
{
    inputPtr = 0;
    newData = false;
    _port.baud(38400);
    _port.attach(callback(this, &FIZ_ArriCmotion::OnRx));
}

//  expect hex data format:
//  0x02 [commandID] [LEN] [len-5 bytes data] [checksum] [0x33]
// checksum is xor of all data bytes.
#define HeaderByte 0x02
#define FooterByte 0x33

// some ambiguity as to how large the packets should be
#define packetSizeToSend 12

void FIZ_ArriCmotion::requestCurrent(void)
{
    _port.putc(HeaderByte);
    _port.putc(packetSizeToSend+5); //packet length
    _port.putc('O'); 
    _port.putc(0x40);//FOCUS
    _port.putc(0);
    _port.putc(0);
    _port.putc(0);
    _port.putc(0x40);//IRIS
    _port.putc(0);
    _port.putc(0);
    _port.putc(0);
    _port.putc(0x40);//ZOOM
    _port.putc(0);
    _port.putc(0);
    _port.putc(0);
    // value of 0 for the first byte indicates client device with invalid data.
    // So shouldn't move the motor, only read the values. 
    // may possibly need to change this to 0x40 0x00 0x00 0x00 for each axis
    // (master but invalid data)
//    for (int i=0;i<packetSizeToSend;i++)
//      _port.putc(0);
    
    // checksum - if all data is 0 then cs is 0. If data above changes then change this.
    _port.putc(0);
    
    _port.putc(FooterByte);
    pc.printf("S");
}


void FIZ_ArriCmotion::OnRx(void)
{
    uint8_t dIn = _port.getc();
    inputBuffer[inputPtr] = dIn;
    pc.printf("R");
    if (inputPtr==0) {
        if (dIn == HeaderByte) {
            inputPtr++;
        }
    } else if (inputPtr == 2) {
        if ((dIn == 'o')||(dIn == 'O')) { // ID 'o' = Motor status. or 'O' = motor command
            inputPtr++;
        } else { // wrong ID
            if (dIn == HeaderByte) {
                inputBuffer[0] = HeaderByte;
                inputPtr = 1;
            } else {
                inputPtr = 0;
            }
        }
    } else { // waiting for data.
        if (inputPtr == 2) { // just got length
            if (inputBuffer[1]>20) // length is too high
                inputPtr=0;
            else
                inputPtr++;
        } else {
            inputPtr++;
            if ((inputPtr>4) && (inputPtr == (inputBuffer[1]))) {
                parsePacket();
                inputPtr = 0;
            }
        }
    }
}

void FIZ_ArriCmotion::parsePacket()
{
    pc.printf("P");
    if (inputBuffer[0] != HeaderByte)
        return;
    if (inputBuffer[1] != 'o') // only parse status
        return;
    int DataLen = inputBuffer[2];    
    if (inputBuffer[DataLen+4] != FooterByte)
        return;
    int checksum = inputBuffer[DataLen+3];
    uint8_t *dataValues = inputBuffer+3; // ptr to data part of message
    // checksum is xor of all bytes.
    int cs = 0;
    for (int i=0; i<DataLen; i++) {
        cs ^= dataValues[i];
    }
    if (cs != checksum) {
//       pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
        return;
    }

// format is 2 bytes status then 16 bit value, MSB first.
// This is repeated for Motor (focus?), iris and zoom
    uint16_t focus_Position = ((uint16_t)dataValues[2])<<8 | inputBuffer[3];

    uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];

    uint16_t zoom_Position = ((uint16_t)inputBuffer[10])<<8 | inputBuffer[11];

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

    newData = true;
}