File content as of revision 80:0b7f1b85b626:

#include "FIZDigiPowerActive.h"
#include "LTCApp.h"
FIZDigiPowerActive::FIZDigiPowerActive(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
{
    inputPtr = 0;
    newData = false;
    initialising = true; //not used as lens parameter reading is disabled
    _port.baud(38400);
//    read_lens_parameters();
    _port.attach(callback(this, &FIZDigiPowerActive::OnRx));
    missedPackets=0;
    min_zoom = 0;
    max_zoom = 0;

}

void FIZDigiPowerActive::requestCurrent(void)
{
//    if (initialising) {
//        requestLensParameters();
//        return;
    if (missedPackets>5) {
        // configure output format
        _port.putc(04); //data length (number of paramters)
        _port.putc(0x70); //multi data setting
        _port.putc(0x30); //iris
        _port.putc(0x31); //zoom
        _port.putc(0x32); //focus
        _port.putc(0x53); //Extender
        _port.putc(0xA6); //checksum (mod 256)
        missedPackets = 0;
    }

    _port.putc(00);
    _port.putc(0x60);
    _port.putc(0xA0);
    missedPackets++;
}

//void FIZDigiPowerActive::requestLensParameters(void)
//{
//    if (min_zoom) {
//        _port.putc(00);
//        _port.putc(0x15);
//        _port.putc(0xEB);
//    } else {
//        _port.putc(00);
//        _port.putc(0x14);
//        _port.putc(0xEC);
//    missedPackets++;
//}


void FIZDigiPowerActive::read_lens_parameters()
{
    _port.putc(00);
    _port.putc(0x15); //0x16 for MOD
    _port.putc(0xEB); //checksum is EA for MOD
    wait_ms(50);
    uint16_t raw_val = 0;
    uint8_t tries = 5;
    while (tries) {
        tries--;
        uint8_t dIn =_port.getc();
        inputBuffer[inputPtr] = dIn;
//        printf("%02x ", dIn);
        if (inputPtr==0) {
            if (dIn == 0x02) { // correct length
                inputPtr++;
            }
        } else if (inputPtr == 1) {
            if (dIn == 0x15) { // correct ID
                inputPtr++;
            } else { // wrong ID
                if (dIn == 0x02) {
                    inputBuffer[0] = 7;
                    inputPtr = 1;
                } else {
                    inputPtr = 0;
                }
            }
        } else { // waiting for data.
            inputPtr++;
            if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
                raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
                min_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
                pc.printf("Min Focus: %d\r\n", min_zoom);
                break;
            }
        }
    }
    inputPtr = 0;
    _port.putc(00);
    _port.putc(0x14);
    _port.putc(0xEC); //ec for 0x14
    wait_ms(50);
    raw_val = 0;
    tries = 5;
    while (tries) {
        tries--;
        uint8_t dIn =_port.getc();
        inputBuffer[inputPtr] = dIn;
//        printf("%02x ", dIn);
        if (inputPtr==0) {
            if (dIn == 0x02) { // correct length
                inputPtr++;
            }
        } else if (inputPtr == 1) {
            if (dIn == 0x14) { // correct ID
                inputPtr++;
            } else { // wrong ID
                if (dIn == 0x02) {
                    inputBuffer[0] = 7;
                    inputPtr = 1;
                } else {
                    inputPtr = 0;
                }
            }
        } else { // waiting for data.
            inputPtr++;
            if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
                raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
                max_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
                pc.printf("Max Focus %d\r\n", max_zoom);
                break;
            }
        }
    }

//quantise it



}

float FIZDigiPowerActive::get_16bit_float(uint16_t val)
{
    //Non Standard 16bit float, 4 bit Exponent, 12bit mantissa
    int expo = val >> 12;
    if (expo & 0x8)
        expo|= ~0xF;
    float ret = (float) ((val &0xFFF) * pow((double)10.0, (double)expo));
    return ret;
}

//  expect hex data format:
//  [dat len] [command] [data] [cs]
// only command we care about is multi reply which is 7 bytes of data - iris, zoom, focus, switch 4
// 07 60 [iris1][iris2] .... [cs]
void FIZDigiPowerActive::OnRx(void)
{
//    if (initialising){
//        read_lens_parameters();
//        return;        
    uint8_t dIn = _port.getc();
//    pc.printf("R");
    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 { // waiting for data.
        inputPtr++;
        if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
            parsePacket();
            inputPtr = 0;
            if (missedPackets)
                missedPackets--;
        }
    }
}

void FIZDigiPowerActive::parsePacket()
{
//    pc.puts("FIZ parse\r\n");
    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];
//        pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
    }
    if (cs & 0x00ff) { // cs & 0x000000ff should give 0
//       pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
        return;
    }
//    pc.puts("FIZ good\r\n");
    uint16_t iris_Position =  getAbsoluteIris(((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]); //((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
    uint16_t zoom_Position = getAbsoluteZoom(((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]); //((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
    uint32_t focus_Position = getAbsoluteFocus(((uint32_t)inputBuffer[6])<<8 | inputBuffer[7]); //((uint32_t)inputBuffer[6])<<8 | inputBuffer[7];

// MAY NEED TO SCALE THESE
    _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
    _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
    _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;

    newData = true;
}