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_ArriCmotion.cpp
- Committer:
- AndyA
- Date:
- 2021-10-25
- Revision:
- 62:dcb92159ac8e
- Child:
- 63:11ee40ec32bd
File content as of revision 62:dcb92159ac8e:
#include "FIZ_ArriCmotion.h" #include "LTCApp.h" FIZ_ArriCmotion::FIZ_ArriCmotion(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx) { inputPtr = 0; newData = false; _port.baud(38400); _port.attach(callback(this, &FIZ_ArriCmotion::OnRx)); } // expect hex data format: // 0x02 [commandID] [LEN] [len-5 bytes data] [checksum] [0x33] // checksum is xor of all data bytes. #define HeaderByte 0x02 #define FooterByte 0x33 // some ambiguity as to how large the packets should be #define packetSizeToSend 12 void FIZ_ArriCmotion::requestCurrent(void) { _port.putc(HeaderByte); _port.putc('O'); _port.putc(packetSizeToSend); // value of 0 for the first byte indicates client device with invalid data. // So shouldn't move the motor, only read the values. // may possibly need to change this to 0x40 0x00 0x00 0x00 for each axis // (master but invalid data) for (int i=0;i<packetSizeToSend;i++) _port.putc(0); // checksum - if all data is 0 then cs is 0. If data above changes then change this. _port.putc(0); _port.putc(FooterByte); } void FIZ_ArriCmotion::OnRx(void) { uint8_t dIn = _port.getc(); inputBuffer[inputPtr] = dIn; if (inputPtr==0) { if (dIn == HeaderByte) { inputPtr++; } } else if (inputPtr == 1) { if ((dIn == 'o')||(dIn == 'O')) { // ID 'o' = Motor status. or 'O' = motor command inputPtr++; } else { // wrong ID if (dIn == HeaderByte) { inputBuffer[0] = HeaderByte; inputPtr = 1; } else { inputPtr = 0; } } } else { // waiting for data. if (inputPtr == 2) { // just got length if (inputBuffer[2]>20) // length is too high inputPtr=0; else inputPtr++; } else { inputPtr++; if ((inputPtr>4) && (inputPtr == (inputBuffer[2]+5))) { parsePacket(); inputPtr = 0; } } } } void FIZ_ArriCmotion::parsePacket() { if (inputBuffer[0] != HeaderByte) return; if (inputBuffer[1] != 'o') // only parse status return; int DataLen = inputBuffer[2]; if (inputBuffer[DataLen+4] != FooterByte) return; int checksum = inputBuffer[DataLen+3]; uint8_t *dataValues = inputBuffer+3; // ptr to data part of message // checksum is xor of all bytes. int cs = 0; for (int i=0; i<DataLen; i++) { cs ^= dataValues[i]; } if (cs != checksum) { // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs); return; } // format is 2 bytes status then 16 bit value, MSB first. // This is repeated for Motor (focus?), iris and zoom uint16_t focus_Position = ((uint16_t)dataValues[2])<<8 | inputBuffer[3]; uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7]; uint16_t zoom_Position = ((uint16_t)inputBuffer[10])<<8 | inputBuffer[11]; // MAY NEED TO SCALE THESE _focus = focus_Position; _iris = iris_Position; _zoom = zoom_Position; newData = true; }