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/FIZ_digiPower.cpp
- Committer:
- AndyA
- Date:
- 2021-04-30
- Revision:
- 17:5ce3fe98e76d
- Parent:
- 16:a8d3a0dbe4bf
- Child:
- 18:ad407a2ed4c9
File content as of revision 17:5ce3fe98e76d:
#include "FIZ_digiPower.h" #include "LTCApp.h" FIZDigiPower::FIZDigiPower(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx) { inputPtr = 0; newData = false; _port.baud(38400); _port.attach(callback(this, &FIZDigiPower::OnRx)); } void FIZDigiPower::requestCurrent(void) { // is listen only } // 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 FIZDigiPower::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 if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs parsePacket(); inputPtr = 0; } else { // waiting for data. inputPtr++; } } void FIZDigiPower::parsePacket() { 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]; } if (cs & 0x00ff) // cs & 0x000000ff should give 0 return; 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; }