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
Diff: FIZ_readers/FIZDigiPowerActive.cpp
- Revision:
- 19:08e6a2283d58
- Child:
- 26:7f66ac76cd5d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FIZ_readers/FIZDigiPowerActive.cpp Tue Jun 29 08:40:25 2021 +0000 @@ -0,0 +1,95 @@ +#include "FIZDigiPowerActive.h" +#include "LTCApp.h" +FIZDigiPowerActive::FIZDigiPowerActive(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx) +{ + inputPtr = 0; + newData = false; + _port.baud(38400); + _port.attach(callback(this, &FIZDigiPowerActive::OnRx)); + missedPackets=0; + +} + +void FIZDigiPowerActive::requestCurrent(void) +{ + if (missedPackets>5) { + // configure output format + _port.putc(04); + _port.putc(0x70); + _port.putc(0x30); + _port.putc(0x31); + _port.putc(0x32); + _port.putc(0x53); + _port.putc(0xA6); + missedPackets = 0; + } + + _port.putc(00); + _port.putc(0x60); + _port.putc(0xA0); + missedPackets++; +} + +// expect hex data format: +// [dat len] [command] [data] [cs] +// only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4 +// 07 60 [zoom1][zoom2] .... [cs] +void FIZDigiPowerActive::OnRx(void) +{ + uint8_t dIn = _port.getc(); + 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 zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; + uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]; + uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7]; + +// MAY NEED TO SCALE THESE + _focus = focus_Position; + _iris = iris_Position; + _zoom = zoom_Position; + + newData = true; +}