I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
FIZ_readers/FIZDigiPowerActive.cpp
- Committer:
- JamieB
- Date:
- 2022-08-30
- Revision:
- 80:0b7f1b85b626
- Parent:
- 71:7305a35cee58
- Child:
- 81:aee60dcce61b
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; }