FIZ_digiPower.cpp

00001 #include "FIZ_digiPower.h"
00002 #include "LTCApp.h"
00003 FIZDigiPower::FIZDigiPower(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
00004 {
00005     inputPtr = 0;
00006     newData = false;
00007     _port.baud(38400);
00008     _port.attach(callback(this, &FIZDigiPower::OnRx));
00009 }
00010 
00011 void FIZDigiPower::requestCurrent(void)
00012 {
00013 // is listen only
00014 }
00015 
00016 //  expect hex data format:
00017 //  [dat len] [command] [data] [cs]
00018 // only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
00019 // 07 60 [zoom1][zoom2] .... [cs]
00020 void FIZDigiPower::OnRx(void)
00021 {
00022     uint8_t dIn = _port.getc();
00023     inputBuffer[inputPtr] = dIn;
00024     if (inputPtr==0) {
00025         if (dIn == 0x07) { // correct length
00026             inputPtr++;
00027         }
00028     } else if (inputPtr == 1) {
00029         if (dIn == 0x60) { // correct ID
00030             inputPtr++;
00031         } else { // wrong ID
00032             if (dIn == 0x07) {
00033                 inputBuffer[0] = 7;
00034                 inputPtr = 1;
00035             } else {
00036                 inputPtr = 0;
00037             }
00038         }
00039     } else { // waiting for data.
00040         inputPtr++;
00041         if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
00042             parsePacket();
00043             inputPtr = 0;
00044         }
00045     }
00046 }
00047 
00048 void FIZDigiPower::parsePacket()
00049 {
00050 //    pc.puts("FIZ parse\r\n");
00051     if (inputBuffer[0] != 0x07)
00052         return;
00053     if (inputBuffer[1] != 0x60)
00054         return;
00055     // checksum is sum of all bytes mod 256 = 00
00056     int cs = 0;
00057     for (int i=0; i<inputPtr; i++) {
00058         cs += inputBuffer[i];
00059 //        pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
00060     }
00061     if (cs & 0x00ff) { // cs & 0x000000ff should give 0
00062 //       pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
00063         return;
00064     }
00065 //    pc.puts("FIZ good\r\n");
00066     uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
00067     uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
00068     uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
00069 
00070 // MAY NEED TO SCALE THESE
00071     _focus = focus_Position;
00072     _iris = iris_Position;
00073     _zoom = zoom_Position;
00074 
00075     newData = true;
00076 }