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/FIZDigiPowerActive.cpp@26:7f66ac76cd5d, 2021-07-08 (annotated)

Committer:
JamieB
Date:
Thu Jul 08 17:31:11 2021 +0000
Revision:
26:7f66ac76cd5d
Parent:
19:08e6a2283d58
Child:
71:7305a35cee58
Fixed FizPort Configuration always defaulting to Preston

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 19:08e6a2283d58 1 #include "FIZDigiPowerActive.h"
AndyA 19:08e6a2283d58 2 #include "LTCApp.h"
AndyA 19:08e6a2283d58 3 FIZDigiPowerActive::FIZDigiPowerActive(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
AndyA 19:08e6a2283d58 4 {
AndyA 19:08e6a2283d58 5 inputPtr = 0;
AndyA 19:08e6a2283d58 6 newData = false;
AndyA 19:08e6a2283d58 7 _port.baud(38400);
AndyA 19:08e6a2283d58 8 _port.attach(callback(this, &FIZDigiPowerActive::OnRx));
AndyA 19:08e6a2283d58 9 missedPackets=0;
AndyA 19:08e6a2283d58 10
AndyA 19:08e6a2283d58 11 }
AndyA 19:08e6a2283d58 12
AndyA 19:08e6a2283d58 13 void FIZDigiPowerActive::requestCurrent(void)
AndyA 19:08e6a2283d58 14 {
AndyA 19:08e6a2283d58 15 if (missedPackets>5) {
AndyA 19:08e6a2283d58 16 // configure output format
AndyA 19:08e6a2283d58 17 _port.putc(04);
AndyA 19:08e6a2283d58 18 _port.putc(0x70);
AndyA 19:08e6a2283d58 19 _port.putc(0x30);
AndyA 19:08e6a2283d58 20 _port.putc(0x31);
AndyA 19:08e6a2283d58 21 _port.putc(0x32);
AndyA 19:08e6a2283d58 22 _port.putc(0x53);
AndyA 19:08e6a2283d58 23 _port.putc(0xA6);
AndyA 19:08e6a2283d58 24 missedPackets = 0;
AndyA 19:08e6a2283d58 25 }
AndyA 19:08e6a2283d58 26
AndyA 19:08e6a2283d58 27 _port.putc(00);
AndyA 19:08e6a2283d58 28 _port.putc(0x60);
AndyA 19:08e6a2283d58 29 _port.putc(0xA0);
AndyA 19:08e6a2283d58 30 missedPackets++;
AndyA 19:08e6a2283d58 31 }
AndyA 19:08e6a2283d58 32
AndyA 19:08e6a2283d58 33 // expect hex data format:
AndyA 19:08e6a2283d58 34 // [dat len] [command] [data] [cs]
AndyA 19:08e6a2283d58 35 // only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
AndyA 19:08e6a2283d58 36 // 07 60 [zoom1][zoom2] .... [cs]
AndyA 19:08e6a2283d58 37 void FIZDigiPowerActive::OnRx(void)
AndyA 19:08e6a2283d58 38 {
AndyA 19:08e6a2283d58 39 uint8_t dIn = _port.getc();
JamieB 26:7f66ac76cd5d 40 // pc.printf("R");
AndyA 19:08e6a2283d58 41 inputBuffer[inputPtr] = dIn;
AndyA 19:08e6a2283d58 42 if (inputPtr==0) {
AndyA 19:08e6a2283d58 43 if (dIn == 0x07) { // correct length
AndyA 19:08e6a2283d58 44 inputPtr++;
AndyA 19:08e6a2283d58 45 }
AndyA 19:08e6a2283d58 46 } else if (inputPtr == 1) {
AndyA 19:08e6a2283d58 47 if (dIn == 0x60) { // correct ID
AndyA 19:08e6a2283d58 48 inputPtr++;
AndyA 19:08e6a2283d58 49 } else { // wrong ID
AndyA 19:08e6a2283d58 50 if (dIn == 0x07) {
AndyA 19:08e6a2283d58 51 inputBuffer[0] = 7;
AndyA 19:08e6a2283d58 52 inputPtr = 1;
AndyA 19:08e6a2283d58 53 } else {
AndyA 19:08e6a2283d58 54 inputPtr = 0;
AndyA 19:08e6a2283d58 55 }
AndyA 19:08e6a2283d58 56 }
AndyA 19:08e6a2283d58 57 } else { // waiting for data.
AndyA 19:08e6a2283d58 58 inputPtr++;
AndyA 19:08e6a2283d58 59 if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
AndyA 19:08e6a2283d58 60 parsePacket();
AndyA 19:08e6a2283d58 61 inputPtr = 0;
AndyA 19:08e6a2283d58 62 if (missedPackets)
AndyA 19:08e6a2283d58 63 missedPackets--;
AndyA 19:08e6a2283d58 64 }
AndyA 19:08e6a2283d58 65 }
AndyA 19:08e6a2283d58 66 }
AndyA 19:08e6a2283d58 67
AndyA 19:08e6a2283d58 68 void FIZDigiPowerActive::parsePacket()
AndyA 19:08e6a2283d58 69 {
AndyA 19:08e6a2283d58 70 // pc.puts("FIZ parse\r\n");
AndyA 19:08e6a2283d58 71 if (inputBuffer[0] != 0x07)
AndyA 19:08e6a2283d58 72 return;
AndyA 19:08e6a2283d58 73 if (inputBuffer[1] != 0x60)
AndyA 19:08e6a2283d58 74 return;
AndyA 19:08e6a2283d58 75 // checksum is sum of all bytes mod 256 = 00
AndyA 19:08e6a2283d58 76 int cs = 0;
AndyA 19:08e6a2283d58 77 for (int i=0; i<inputPtr; i++) {
AndyA 19:08e6a2283d58 78 cs += inputBuffer[i];
AndyA 19:08e6a2283d58 79 // pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
AndyA 19:08e6a2283d58 80 }
AndyA 19:08e6a2283d58 81 if (cs & 0x00ff) { // cs & 0x000000ff should give 0
AndyA 19:08e6a2283d58 82 // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
AndyA 19:08e6a2283d58 83 return;
AndyA 19:08e6a2283d58 84 }
AndyA 19:08e6a2283d58 85 // pc.puts("FIZ good\r\n");
AndyA 19:08e6a2283d58 86 uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
AndyA 19:08e6a2283d58 87 uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
AndyA 19:08e6a2283d58 88 uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
AndyA 19:08e6a2283d58 89
AndyA 19:08e6a2283d58 90 // MAY NEED TO SCALE THESE
AndyA 19:08e6a2283d58 91 _focus = focus_Position;
AndyA 19:08e6a2283d58 92 _iris = iris_Position;
AndyA 19:08e6a2283d58 93 _zoom = zoom_Position;
AndyA 19:08e6a2283d58 94
AndyA 19:08e6a2283d58 95 newData = true;
AndyA 19:08e6a2283d58 96 }