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@3:14d241e29be3, 2021-01-28 (annotated)

Committer:
AndyA
Date:
Thu Jan 28 15:13:05 2021 +0000
Revision:
3:14d241e29be3
Child:
5:7994913a15fe
Added FIZ reading code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 3:14d241e29be3 1 #include "FIZReader.h"
AndyA 3:14d241e29be3 2
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 3:14d241e29be3 17 _port.putc(0x0c);
AndyA 3:14d241e29be3 18 _port.putc(0x00);
AndyA 3:14d241e29be3 19 _port.putc(0xD5);
AndyA 3:14d241e29be3 20 _port.putc(0x03);
AndyA 3:14d241e29be3 21 }
AndyA 3:14d241e29be3 22
AndyA 3:14d241e29be3 23
AndyA 3:14d241e29be3 24 bool FIZReader::getMostRecent(uint32_t *focus, uint16_t *iris, uint16_t *zoom)
AndyA 3:14d241e29be3 25 {
AndyA 3:14d241e29be3 26 *focus = _focus;
AndyA 3:14d241e29be3 27 *iris = _iris;
AndyA 3:14d241e29be3 28 *zoom = _zoom;
AndyA 3:14d241e29be3 29 bool wasNew = newData;
AndyA 3:14d241e29be3 30 newData = false;
AndyA 3:14d241e29be3 31 return wasNew;
AndyA 3:14d241e29be3 32 }
AndyA 3:14d241e29be3 33
AndyA 3:14d241e29be3 34
AndyA 3:14d241e29be3 35 void FIZReader::OnRx(void)
AndyA 3:14d241e29be3 36 {
AndyA 3:14d241e29be3 37 while (_port.readable()) {
AndyA 3:14d241e29be3 38 inputBuffer[inputPtr] = _port.getc();
AndyA 3:14d241e29be3 39 if (inputPtr==0) {
AndyA 3:14d241e29be3 40 if (inputBuffer[0] == 0x0C)
AndyA 3:14d241e29be3 41 inputPtr++;
AndyA 3:14d241e29be3 42 } else if (inputPtr==1) {
AndyA 3:14d241e29be3 43 if (inputBuffer[1] == 0x07)
AndyA 3:14d241e29be3 44 inputPtr++;
AndyA 3:14d241e29be3 45 else
AndyA 3:14d241e29be3 46 inputPtr = 0;
AndyA 3:14d241e29be3 47 }
AndyA 3:14d241e29be3 48 if (inputPtr == 9) {
AndyA 3:14d241e29be3 49 parsePacket();
AndyA 3:14d241e29be3 50 inputPtr = 0;
AndyA 3:14d241e29be3 51 } else
AndyA 3:14d241e29be3 52 inputPtr++;
AndyA 3:14d241e29be3 53 }
AndyA 3:14d241e29be3 54 }
AndyA 3:14d241e29be3 55 void FIZReader::parsePacket()
AndyA 3:14d241e29be3 56 {
AndyA 3:14d241e29be3 57 _focus = inputBuffer[4] | ((uint32_t)inputBuffer[3])<<8 | ((uint32_t)inputBuffer[2])<<16;
AndyA 3:14d241e29be3 58 _iris = inputBuffer[6] | ((uint16_t)inputBuffer[5])<<8;
AndyA 3:14d241e29be3 59 _zoom = inputBuffer[8] | ((uint16_t)inputBuffer[7])<<8;
AndyA 3:14d241e29be3 60 newData = true;
AndyA 3:14d241e29be3 61 }