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_DISNEY.cpp@16:a8d3a0dbe4bf, 2021-04-30 (annotated)

Committer:
AndyA
Date:
Fri Apr 30 11:26:34 2021 +0000
Revision:
16:a8d3a0dbe4bf
Child:
17:5ce3fe98e76d
Add support for multiple types of FIZ reader.; Added support for digipower FIZ as used by skycam

Who changed what in which revision?

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