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/FIZ_ArriCmotion.cpp
- Revision:
- 62:dcb92159ac8e
- Child:
- 63:11ee40ec32bd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FIZ_readers/FIZ_ArriCmotion.cpp Mon Oct 25 09:04:21 2021 +0000 @@ -0,0 +1,110 @@ +#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; +}