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

FIZReader.cpp@6:61274e214f46, 2021-02-09 (annotated)

Committer:
AndyA
Date:
Tue Feb 09 16:55:52 2021 +0000
Revision:
6:61274e214f46
Parent:
5:7994913a15fe
Child:
8:961bb15570a1
Fix FIZ stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 3:14d241e29be3 1 #include "FIZReader.h"
AndyA 6:61274e214f46 2 #include "LTCApp.h"
AndyA 3:14d241e29be3 3 FIZReader::FIZReader(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
AndyA 3:14d241e29be3 4 {
AndyA 3:14d241e29be3 5 _port.baud(115200);
AndyA 3:14d241e29be3 6 inputPtr = 0;
AndyA 3:14d241e29be3 7 _focus = 0;
AndyA 3:14d241e29be3 8 _iris = 0;
AndyA 3:14d241e29be3 9 _zoom = 0;
AndyA 3:14d241e29be3 10 newData = false;
AndyA 3:14d241e29be3 11 _port.attach(callback(this, &FIZReader::OnRx));
AndyA 3:14d241e29be3 12 }
AndyA 3:14d241e29be3 13
AndyA 3:14d241e29be3 14 void FIZReader::requestCurrent(void)
AndyA 3:14d241e29be3 15 {
AndyA 3:14d241e29be3 16 _port.putc(0x02);
AndyA 5:7994913a15fe 17 _port.putc('0');
AndyA 5:7994913a15fe 18 _port.putc('C');
AndyA 5:7994913a15fe 19 _port.putc('0');
AndyA 5:7994913a15fe 20 _port.putc('0');
AndyA 5:7994913a15fe 21 _port.putc('D');
AndyA 5:7994913a15fe 22 _port.putc('5');
AndyA 3:14d241e29be3 23 _port.putc(0x03);
AndyA 3:14d241e29be3 24 }
AndyA 3:14d241e29be3 25
AndyA 3:14d241e29be3 26
AndyA 3:14d241e29be3 27 bool FIZReader::getMostRecent(uint32_t *focus, uint16_t *iris, uint16_t *zoom)
AndyA 3:14d241e29be3 28 {
AndyA 3:14d241e29be3 29 *focus = _focus;
AndyA 3:14d241e29be3 30 *iris = _iris;
AndyA 3:14d241e29be3 31 *zoom = _zoom;
AndyA 3:14d241e29be3 32 bool wasNew = newData;
AndyA 3:14d241e29be3 33 newData = false;
AndyA 3:14d241e29be3 34 return wasNew;
AndyA 3:14d241e29be3 35 }
AndyA 3:14d241e29be3 36
AndyA 3:14d241e29be3 37
AndyA 3:14d241e29be3 38 void FIZReader::OnRx(void)
AndyA 3:14d241e29be3 39 {
AndyA 3:14d241e29be3 40 inputBuffer[inputPtr] = _port.getc();
AndyA 3:14d241e29be3 41 if (inputPtr==0) {
AndyA 5:7994913a15fe 42 if (inputBuffer[inputPtr] == 0x02)
AndyA 3:14d241e29be3 43 inputPtr++;
AndyA 5:7994913a15fe 44 } else if (inputBuffer[inputPtr] == 0x03) {
AndyA 3:14d241e29be3 45 parsePacket();
AndyA 3:14d241e29be3 46 inputPtr = 0;
AndyA 5:7994913a15fe 47 } else {
AndyA 3:14d241e29be3 48 inputPtr++;
AndyA 5:7994913a15fe 49 if (inputPtr == InBufferSize)
AndyA 5:7994913a15fe 50 inputPtr = 0;
AndyA 5:7994913a15fe 51 }
AndyA 3:14d241e29be3 52 }
AndyA 5:7994913a15fe 53
AndyA 3:14d241e29be3 54 void FIZReader::parsePacket()
AndyA 3:14d241e29be3 55 {
AndyA 5:7994913a15fe 56 // expect ASCII string of "0C07ffffffiiiizzzzcs" where ffffff is 6 char hex value for focus
AndyA 5:7994913a15fe 57 // iiii is 4 char hex value for tstop, zzzz is 4 char hex value for zoom and cs is 2 char checksum
AndyA 5:7994913a15fe 58 if (inputPtr != 21)
AndyA 5:7994913a15fe 59 return;
AndyA 5:7994913a15fe 60 if (inputBuffer[1] != '0')
AndyA 5:7994913a15fe 61 return;
AndyA 5:7994913a15fe 62 if (inputBuffer[2] != 'C')
AndyA 5:7994913a15fe 63 return;
AndyA 5:7994913a15fe 64 _focus = 0;
AndyA 5:7994913a15fe 65 _iris = 0;
AndyA 5:7994913a15fe 66 _zoom = 0;
AndyA 5:7994913a15fe 67 for (int count = 0;count<6;count++) {
AndyA 5:7994913a15fe 68 _focus*=16;
AndyA 5:7994913a15fe 69 _focus += hexValue(inputBuffer[5+count]);
AndyA 5:7994913a15fe 70 }
AndyA 5:7994913a15fe 71 for (int count = 0;count<4;count++) {
AndyA 5:7994913a15fe 72 _iris*=16;
AndyA 5:7994913a15fe 73 _iris += hexValue(inputBuffer[11+count]);
AndyA 5:7994913a15fe 74 _zoom*=16;
AndyA 5:7994913a15fe 75 _zoom += hexValue(inputBuffer[15+count]);
AndyA 5:7994913a15fe 76 }
AndyA 3:14d241e29be3 77 newData = true;
AndyA 3:14d241e29be3 78 }