FIZCanon.cpp

00001 #include "FIZCanon.h"
00002 #include "LTCApp.h"
00003 FIZCanon::FIZCanon(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
00004 {
00005     inputPtr = 0;
00006     newData = false;
00007     _port.baud(19200);
00008     _port.format(8, SerialBase::Even, 1);
00009     _port.attach(callback(this, &FIZCanon::OnRx));
00010     missedPackets=0;
00011 
00012 }
00013 
00014 void FIZCanon::requestCurrent(void)
00015 {
00016     if (missedPackets>5) {
00017         // configure output format
00018         _port.putc(0x80);
00019         _port.putc(0xC6);
00020         _port.putc(0xBF);
00021 //        pc.printf("W");
00022         missedPackets = 0;
00023     }
00024 
00025     _port.putc(0x94);
00026     _port.putc(0xC1);
00027     _port.putc(0xBF);
00028     _port.putc(0x94);
00029     _port.putc(0xC3);
00030     _port.putc(0xBF);
00031     _port.putc(0x94);
00032     _port.putc(0xC5);
00033     _port.putc(0xBF);
00034     missedPackets++;
00035 //    pc.printf("T");
00036 }
00037 
00038 //  expect hex data format:
00039 //  [dat len] [command] [data] [cs]
00040 // only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
00041 // 07 60 [zoom1][zoom2] .... [cs]
00042 void FIZCanon::OnRx(void)
00043 {
00044     uint8_t dIn = _port.getc();
00045 //    pc.putc('R');
00046     inputBuffer[inputPtr] = dIn;
00047     if (inputPtr==0) {
00048         if (dIn == 0x94) { // correct length
00049             inputPtr++;
00050         }
00051     } else if (inputPtr > 30) {
00052         inputPtr = 0;
00053     } else { // waiting for data.
00054         inputPtr++;
00055         if (inputPtr == (18) && dIn == 0xBF) { // full packet = 2 byte header, 7 byte data, 1 byte cs
00056             parsePacket();
00057             inputPtr = 0;
00058             if (missedPackets)
00059                 missedPackets--;
00060         }
00061     }
00062 }
00063 
00064 void FIZCanon::parsePacket()
00065 {
00066 //    pc.puts("FIZ parse\r\n");
00067 //    pc.putc('P');
00068     int read_pos = 0;
00069     bool valid = false;
00070 
00071     while (read_pos != inputPtr) {
00072         if (!valid) {
00073             if (inputBuffer[read_pos] == CANON_START) {
00074                 valid = true;
00075             }
00076         } else {
00077             switch(inputBuffer[read_pos]) {
00078                 case CANON_ZOOM:
00079 //                    pc.putc('Z');
00080                     _zoom = (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
00081 //                pc.putc(0x01);
00082 //                pc.putc(inputBuffer[read_pos+1]);
00083 //                pc.putc(inputBuffer[read_pos+2]);
00084 //                pc.putc(inputBuffer[read_pos+3]);
00085                     read_pos += 4; // type + 3 BYTES OF DATA
00086                     break;
00087                 case CANON_FOCUS:
00088 //                    pc.putc('F');
00089                     _focus = 1000 - (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
00090 //                pc.putc(0x02);
00091 //                pc.putc(inputBuffer[read_pos+1]);
00092 //                pc.putc(inputBuffer[read_pos+2]);
00093 //                pc.putc(inputBuffer[read_pos+3]);
00094                     read_pos += 4;
00095                     break;
00096                 case CANON_IRIS:
00097                     //IRIS NOT SCALED CORRECTLY -> 0x8000 - 0xD000, 0x1000 between FStops -> Fix Later
00098 //                    pc.putc('I');
00099                     _iris = (((uint32_t)inputBuffer[read_pos +1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3]);
00100 //                pc.putc(0x03);
00101 //                pc.putc(inputBuffer[read_pos+1]);
00102 //                pc.putc(inputBuffer[read_pos+2]);
00103 //                pc.putc(inputBuffer[read_pos+3]);
00104                     read_pos += 4;
00105                     break;
00106                 default:
00107                     //pc.putc('D');
00108                     break;
00109             }
00110             valid = false;
00111         } // if valid
00112         read_pos++;
00113     } // while
00114 
00115 //// MAY NEED TO SCALE THESE
00116 //        _focus = focus_Position;
00117 //        _iris = iris_Position;
00118 //        _zoom = zoom_Position;
00119 //
00120 
00121     newData = true;
00122 }