Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

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@63:11ee40ec32bd, 2021-11-11 (annotated)

Committer:
JamieB
Date:
Thu Nov 11 20:18:17 2021 +0000
Revision:
63:11ee40ec32bd
Parent:
62:dcb92159ac8e 
Changes to Arri Message parse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 62:dcb92159ac8e 1 #include "FIZ_ArriCmotion.h"
AndyA 62:dcb92159ac8e 2 #include "LTCApp.h"
AndyA 62:dcb92159ac8e 3
AndyA 62:dcb92159ac8e 4 FIZ_ArriCmotion::FIZ_ArriCmotion(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
AndyA 62:dcb92159ac8e 5 {
AndyA 62:dcb92159ac8e 6 inputPtr = 0;
AndyA 62:dcb92159ac8e 7 newData = false;
AndyA 62:dcb92159ac8e 8 _port.baud(38400);
AndyA 62:dcb92159ac8e 9 _port.attach(callback(this, &FIZ_ArriCmotion::OnRx));
AndyA 62:dcb92159ac8e 10 }
AndyA 62:dcb92159ac8e 11
AndyA 62:dcb92159ac8e 12 // expect hex data format:
AndyA 62:dcb92159ac8e 13 // 0x02 [commandID] [LEN] [len-5 bytes data] [checksum] [0x33]
AndyA 62:dcb92159ac8e 14 // checksum is xor of all data bytes.
AndyA 62:dcb92159ac8e 15 #define HeaderByte 0x02
AndyA 62:dcb92159ac8e 16 #define FooterByte 0x33
AndyA 62:dcb92159ac8e 17
AndyA 62:dcb92159ac8e 18 // some ambiguity as to how large the packets should be
AndyA 62:dcb92159ac8e 19 #define packetSizeToSend 12
AndyA 62:dcb92159ac8e 20
AndyA 62:dcb92159ac8e 21 void FIZ_ArriCmotion::requestCurrent(void)
AndyA 62:dcb92159ac8e 22 {
AndyA 62:dcb92159ac8e 23 _port.putc(HeaderByte);
JamieB 63:11ee40ec32bd 24 _port.putc(packetSizeToSend+5); //packet length
AndyA 62:dcb92159ac8e 25 _port.putc('O');
JamieB 63:11ee40ec32bd 26 _port.putc(0x40);//FOCUS
JamieB 63:11ee40ec32bd 27 _port.putc(0);
JamieB 63:11ee40ec32bd 28 _port.putc(0);
JamieB 63:11ee40ec32bd 29 _port.putc(0);
JamieB 63:11ee40ec32bd 30 _port.putc(0x40);//IRIS
JamieB 63:11ee40ec32bd 31 _port.putc(0);
JamieB 63:11ee40ec32bd 32 _port.putc(0);
JamieB 63:11ee40ec32bd 33 _port.putc(0);
JamieB 63:11ee40ec32bd 34 _port.putc(0x40);//ZOOM
JamieB 63:11ee40ec32bd 35 _port.putc(0);
JamieB 63:11ee40ec32bd 36 _port.putc(0);
JamieB 63:11ee40ec32bd 37 _port.putc(0);
AndyA 62:dcb92159ac8e 38 // value of 0 for the first byte indicates client device with invalid data.
AndyA 62:dcb92159ac8e 39 // So shouldn't move the motor, only read the values.
AndyA 62:dcb92159ac8e 40 // may possibly need to change this to 0x40 0x00 0x00 0x00 for each axis
AndyA 62:dcb92159ac8e 41 // (master but invalid data)
JamieB 63:11ee40ec32bd 42 // for (int i=0;i<packetSizeToSend;i++)
JamieB 63:11ee40ec32bd 43 // _port.putc(0);
AndyA 62:dcb92159ac8e 44
AndyA 62:dcb92159ac8e 45 // checksum - if all data is 0 then cs is 0. If data above changes then change this.
AndyA 62:dcb92159ac8e 46 _port.putc(0);
AndyA 62:dcb92159ac8e 47
AndyA 62:dcb92159ac8e 48 _port.putc(FooterByte);
JamieB 63:11ee40ec32bd 49 pc.printf("S");
AndyA 62:dcb92159ac8e 50 }
AndyA 62:dcb92159ac8e 51
AndyA 62:dcb92159ac8e 52
AndyA 62:dcb92159ac8e 53 void FIZ_ArriCmotion::OnRx(void)
AndyA 62:dcb92159ac8e 54 {
AndyA 62:dcb92159ac8e 55 uint8_t dIn = _port.getc();
AndyA 62:dcb92159ac8e 56 inputBuffer[inputPtr] = dIn;
JamieB 63:11ee40ec32bd 57 pc.printf("R");
AndyA 62:dcb92159ac8e 58 if (inputPtr==0) {
AndyA 62:dcb92159ac8e 59 if (dIn == HeaderByte) {
AndyA 62:dcb92159ac8e 60 inputPtr++;
AndyA 62:dcb92159ac8e 61 }
JamieB 63:11ee40ec32bd 62 } else if (inputPtr == 2) {
AndyA 62:dcb92159ac8e 63 if ((dIn == 'o')||(dIn == 'O')) { // ID 'o' = Motor status. or 'O' = motor command
AndyA 62:dcb92159ac8e 64 inputPtr++;
AndyA 62:dcb92159ac8e 65 } else { // wrong ID
AndyA 62:dcb92159ac8e 66 if (dIn == HeaderByte) {
AndyA 62:dcb92159ac8e 67 inputBuffer[0] = HeaderByte;
AndyA 62:dcb92159ac8e 68 inputPtr = 1;
AndyA 62:dcb92159ac8e 69 } else {
AndyA 62:dcb92159ac8e 70 inputPtr = 0;
AndyA 62:dcb92159ac8e 71 }
AndyA 62:dcb92159ac8e 72 }
AndyA 62:dcb92159ac8e 73 } else { // waiting for data.
AndyA 62:dcb92159ac8e 74 if (inputPtr == 2) { // just got length
JamieB 63:11ee40ec32bd 75 if (inputBuffer[1]>20) // length is too high
AndyA 62:dcb92159ac8e 76 inputPtr=0;
AndyA 62:dcb92159ac8e 77 else
AndyA 62:dcb92159ac8e 78 inputPtr++;
AndyA 62:dcb92159ac8e 79 } else {
AndyA 62:dcb92159ac8e 80 inputPtr++;
JamieB 63:11ee40ec32bd 81 if ((inputPtr>4) && (inputPtr == (inputBuffer[1]))) {
AndyA 62:dcb92159ac8e 82 parsePacket();
AndyA 62:dcb92159ac8e 83 inputPtr = 0;
AndyA 62:dcb92159ac8e 84 }
AndyA 62:dcb92159ac8e 85 }
AndyA 62:dcb92159ac8e 86 }
AndyA 62:dcb92159ac8e 87 }
AndyA 62:dcb92159ac8e 88
AndyA 62:dcb92159ac8e 89 void FIZ_ArriCmotion::parsePacket()
AndyA 62:dcb92159ac8e 90 {
JamieB 63:11ee40ec32bd 91 pc.printf("P");
AndyA 62:dcb92159ac8e 92 if (inputBuffer[0] != HeaderByte)
AndyA 62:dcb92159ac8e 93 return;
AndyA 62:dcb92159ac8e 94 if (inputBuffer[1] != 'o') // only parse status
AndyA 62:dcb92159ac8e 95 return;
AndyA 62:dcb92159ac8e 96 int DataLen = inputBuffer[2];
AndyA 62:dcb92159ac8e 97 if (inputBuffer[DataLen+4] != FooterByte)
AndyA 62:dcb92159ac8e 98 return;
AndyA 62:dcb92159ac8e 99 int checksum = inputBuffer[DataLen+3];
AndyA 62:dcb92159ac8e 100 uint8_t *dataValues = inputBuffer+3; // ptr to data part of message
AndyA 62:dcb92159ac8e 101 // checksum is xor of all bytes.
AndyA 62:dcb92159ac8e 102 int cs = 0;
AndyA 62:dcb92159ac8e 103 for (int i=0; i<DataLen; i++) {
AndyA 62:dcb92159ac8e 104 cs ^= dataValues[i];
AndyA 62:dcb92159ac8e 105 }
AndyA 62:dcb92159ac8e 106 if (cs != checksum) {
AndyA 62:dcb92159ac8e 107 // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
AndyA 62:dcb92159ac8e 108 return;
AndyA 62:dcb92159ac8e 109 }
AndyA 62:dcb92159ac8e 110
AndyA 62:dcb92159ac8e 111 // format is 2 bytes status then 16 bit value, MSB first.
AndyA 62:dcb92159ac8e 112 // This is repeated for Motor (focus?), iris and zoom
AndyA 62:dcb92159ac8e 113 uint16_t focus_Position = ((uint16_t)dataValues[2])<<8 | inputBuffer[3];
AndyA 62:dcb92159ac8e 114
AndyA 62:dcb92159ac8e 115 uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
AndyA 62:dcb92159ac8e 116
AndyA 62:dcb92159ac8e 117 uint16_t zoom_Position = ((uint16_t)inputBuffer[10])<<8 | inputBuffer[11];
AndyA 62:dcb92159ac8e 118
AndyA 62:dcb92159ac8e 119 // MAY NEED TO SCALE THESE
AndyA 62:dcb92159ac8e 120 _focus = focus_Position;
AndyA 62:dcb92159ac8e 121 _iris = iris_Position;
AndyA 62:dcb92159ac8e 122 _zoom = zoom_Position;
AndyA 62:dcb92159ac8e 123
AndyA 62:dcb92159ac8e 124 newData = true;
AndyA 62:dcb92159ac8e 125 }