File content as of revision 16:a8d3a0dbe4bf:

#include "FIZ_digiPower.h"
#include "LTCApp.h"
FIZDigiPower::FIZDigiPower(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
{
    inputPtr = 0;
    newData = false;
    _port.baud(38400);
    _port.attach(callback(this, &FIZDigiPower::OnRx));
}

void FIZDigiPower::requestCurrent(void)
{
// is listen only
}

//  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 FIZDigiPower::OnRx(void)
{
    uint8_t dIn = _port.getc();
    inputBuffer[inputPtr] = dIn;
    if (inputPtr==0) {
        if (dIn == 0x07) // correct length
            inputPtr++;
    } else if (inputPtr == 1) {
        if (dIn == 0x60) // correct ID
            inputPtr++;
        else { // wrong ID
            if (dIn == 0x07) {
                inputBuffer[0] = 7;
                inputPtr = 1;
            } else {
                inputPtr = 0;
            }
        }
    } else if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
        parsePacket();
        inputPtr = 0;
    } else { // waiting for data.
        inputPtr++;
    }
}

void FIZDigiPower::parsePacket()
{    
    if (inputBuffer[0] != 0x07)
        return;
    if (inputBuffer[1] != 0x60)
        return;
    // checksum is sum of all bytes mod 256 = 00      
    int cs = 0;
    for (int i=0;i<inputPtr;i++) {
        cs += inputBuffer[i];
        }
    if (cs & 0x00ff) // cs & 0x000000ff should give 0
    return;
    
    uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
    uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
    uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];

// NEED TO SCALE THESE
    _focus = focus_Position;
    _iris = iris_Position;
    _zoom = zoom_Position;
    
    newData = true;
}