FIZDigiPowerActive.cpp

00001 #include "FIZDigiPowerActive.h"
00002 #include "LTCApp.h"
00003 FIZDigiPowerActive::FIZDigiPowerActive(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
00004 {
00005     inputPtr = 0;
00006     newData = false;
00007     initialising = true; //not used as lens parameter reading is disabled
00008     _port.baud(38400);
00009 //    read_lens_parameters();
00010     _port.attach(callback(this, &FIZDigiPowerActive::OnRx));
00011     missedPackets=0;
00012     min_zoom = 0;
00013     max_zoom = 0;
00014 
00015 }
00016 
00017 void FIZDigiPowerActive::requestCurrent(void)
00018 {
00019 //    if (initialising) {
00020 //        requestLensParameters();
00021 //        return;
00022     if (missedPackets>5) {
00023         // configure output format
00024         _port.putc(04); //data length (number of paramters)
00025         _port.putc(0x70); //multi data setting
00026         _port.putc(0x30); //iris
00027         _port.putc(0x31); //zoom
00028         _port.putc(0x32); //focus
00029         _port.putc(0x53); //Extender
00030         _port.putc(0xA6); //checksum (mod 256)
00031         missedPackets = 0;
00032     }
00033 
00034     _port.putc(00);
00035     _port.putc(0x60);
00036     _port.putc(0xA0);
00037     missedPackets++;
00038 }
00039 
00040 //void FIZDigiPowerActive::requestLensParameters(void)
00041 //{
00042 //    if (min_zoom) {
00043 //        _port.putc(00);
00044 //        _port.putc(0x15);
00045 //        _port.putc(0xEB);
00046 //    } else {
00047 //        _port.putc(00);
00048 //        _port.putc(0x14);
00049 //        _port.putc(0xEC);
00050 //    missedPackets++;
00051 //}
00052 
00053 
00054 void FIZDigiPowerActive::read_lens_parameters()
00055 {
00056     _port.putc(00);
00057     _port.putc(0x15); //0x16 for MOD
00058     _port.putc(0xEB); //checksum is EA for MOD
00059     wait_ms(50);
00060     uint16_t raw_val = 0;
00061     uint8_t tries = 5;
00062     while (tries) {
00063         tries--;
00064         uint8_t dIn =_port.getc();
00065         inputBuffer[inputPtr] = dIn;
00066 //        printf("%02x ", dIn);
00067         if (inputPtr==0) {
00068             if (dIn == 0x02) { // correct length
00069                 inputPtr++;
00070             }
00071         } else if (inputPtr == 1) {
00072             if (dIn == 0x15) { // correct ID
00073                 inputPtr++;
00074             } else { // wrong ID
00075                 if (dIn == 0x02) {
00076                     inputBuffer[0] = 7;
00077                     inputPtr = 1;
00078                 } else {
00079                     inputPtr = 0;
00080                 }
00081             }
00082         } else { // waiting for data.
00083             inputPtr++;
00084             if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
00085                 raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
00086                 min_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
00087                 pc.printf("Min Focus: %d\r\n", min_zoom);
00088                 break;
00089             }
00090         }
00091     }
00092     inputPtr = 0;
00093     _port.putc(00);
00094     _port.putc(0x14);
00095     _port.putc(0xEC); //ec for 0x14
00096     wait_ms(50);
00097     raw_val = 0;
00098     tries = 5;
00099     while (tries) {
00100         tries--;
00101         uint8_t dIn =_port.getc();
00102         inputBuffer[inputPtr] = dIn;
00103 //        printf("%02x ", dIn);
00104         if (inputPtr==0) {
00105             if (dIn == 0x02) { // correct length
00106                 inputPtr++;
00107             }
00108         } else if (inputPtr == 1) {
00109             if (dIn == 0x14) { // correct ID
00110                 inputPtr++;
00111             } else { // wrong ID
00112                 if (dIn == 0x02) {
00113                     inputBuffer[0] = 7;
00114                     inputPtr = 1;
00115                 } else {
00116                     inputPtr = 0;
00117                 }
00118             }
00119         } else { // waiting for data.
00120             inputPtr++;
00121             if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
00122                 raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
00123                 max_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
00124                 pc.printf("Max Focus %d\r\n", max_zoom);
00125                 break;
00126             }
00127         }
00128     }
00129 
00130 //quantise it
00131 
00132 
00133 
00134 }
00135 
00136 float FIZDigiPowerActive::get_16bit_float(uint16_t val)
00137 {
00138     //Non Standard 16bit float, 4 bit Exponent, 12bit mantissa
00139     int expo = val >> 12;
00140     if (expo & 0x8)
00141         expo|= ~0xF;
00142     float ret = (float) ((val &0xFFF) * pow((double)10.0, (double)expo));
00143     return ret;
00144 }
00145 
00146 //  expect hex data format:
00147 //  [dat len] [command] [data] [cs]
00148 // only command we care about is multi reply which is 7 bytes of data - iris, zoom, focus, switch 4
00149 // 07 60 [iris1][iris2] .... [cs]
00150 void FIZDigiPowerActive::OnRx(void)
00151 {
00152 //    if (initialising){
00153 //        read_lens_parameters();
00154 //        return;        
00155     uint8_t dIn = _port.getc();
00156 //    pc.printf("R");
00157     inputBuffer[inputPtr] = dIn;
00158     if (inputPtr==0) {
00159         if (dIn == 0x07) { // correct length
00160             inputPtr++;
00161         }
00162     } else if (inputPtr == 1) {
00163         if (dIn == 0x60) { // correct ID
00164             inputPtr++;
00165         } else { // wrong ID
00166             if (dIn == 0x07) {
00167                 inputBuffer[0] = 7;
00168                 inputPtr = 1;
00169             } else {
00170                 inputPtr = 0;
00171             }
00172         }
00173     } else { // waiting for data.
00174         inputPtr++;
00175         if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
00176             parsePacket();
00177             inputPtr = 0;
00178             if (missedPackets)
00179                 missedPackets--;
00180         }
00181     }
00182 }
00183 
00184 void FIZDigiPowerActive::parsePacket()
00185 {
00186 //    pc.puts("FIZ parse\r\n");
00187     if (inputBuffer[0] != 0x07)
00188         return;
00189     if (inputBuffer[1] != 0x60)
00190         return;
00191     // checksum is sum of all bytes mod 256 = 00
00192     int cs = 0;
00193     for (int i=0; i<inputPtr; i++) {
00194         cs += inputBuffer[i];
00195 //        pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
00196     }
00197     if (cs & 0x00ff) { // cs & 0x000000ff should give 0
00198 //       pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
00199         return;
00200     }
00201 //    pc.puts("FIZ good\r\n");
00202     uint16_t iris_Position =  ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
00203     uint16_t zoom_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
00204     uint32_t focus_Position = ((uint32_t)inputBuffer[6])<<8 | inputBuffer[7];
00205 
00206     if (UserSettings.absolute_focus) {
00207         _focus = getAbsoluteFocus(focus_Position);
00208     } else {
00209         _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
00210     }
00211     if (UserSettings.absolute_iris) {
00212         _iris = getAbsoluteIris(iris_Position);
00213     } else {
00214         _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
00215     }
00216     if (UserSettings.absolute_zoom) {
00217         _zoom = getAbsoluteZoom(zoom_Position);
00218     } else {
00219         _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;
00220     }
00221 // // MAY NEED TO SCALE THESE
00222 //     _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
00223 //     _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
00224 //     _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;
00225 
00226     newData = true;
00227 }