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_digiPower.cpp@18:ad407a2ed4c9, 2021-05-17 (annotated)

Committer:
AndyA
Date:
Mon May 17 11:41:57 2021 +0000
Revision:
18:ad407a2ed4c9
Parent:
17:5ce3fe98e76d 
Fix digi-power FIZ not processing packet until after the next byte is received.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 16:a8d3a0dbe4bf 1 #include "FIZ_digiPower.h"
AndyA 16:a8d3a0dbe4bf 2 #include "LTCApp.h"
AndyA 16:a8d3a0dbe4bf 3 FIZDigiPower::FIZDigiPower(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(38400);
AndyA 16:a8d3a0dbe4bf 8 _port.attach(callback(this, &FIZDigiPower::OnRx));
AndyA 16:a8d3a0dbe4bf 9 }
AndyA 16:a8d3a0dbe4bf 10
AndyA 16:a8d3a0dbe4bf 11 void FIZDigiPower::requestCurrent(void)
AndyA 16:a8d3a0dbe4bf 12 {
AndyA 16:a8d3a0dbe4bf 13 // is listen only
AndyA 16:a8d3a0dbe4bf 14 }
AndyA 16:a8d3a0dbe4bf 15
AndyA 16:a8d3a0dbe4bf 16 // expect hex data format:
AndyA 16:a8d3a0dbe4bf 17 // [dat len] [command] [data] [cs]
AndyA 16:a8d3a0dbe4bf 18 // only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
AndyA 16:a8d3a0dbe4bf 19 // 07 60 [zoom1][zoom2] .... [cs]
AndyA 16:a8d3a0dbe4bf 20 void FIZDigiPower::OnRx(void)
AndyA 16:a8d3a0dbe4bf 21 {
AndyA 16:a8d3a0dbe4bf 22 uint8_t dIn = _port.getc();
AndyA 16:a8d3a0dbe4bf 23 inputBuffer[inputPtr] = dIn;
AndyA 16:a8d3a0dbe4bf 24 if (inputPtr==0) {
AndyA 18:ad407a2ed4c9 25 if (dIn == 0x07) { // correct length
AndyA 16:a8d3a0dbe4bf 26 inputPtr++;
AndyA 18:ad407a2ed4c9 27 }
AndyA 16:a8d3a0dbe4bf 28 } else if (inputPtr == 1) {
AndyA 18:ad407a2ed4c9 29 if (dIn == 0x60) { // correct ID
AndyA 16:a8d3a0dbe4bf 30 inputPtr++;
AndyA 18:ad407a2ed4c9 31 } else { // wrong ID
AndyA 16:a8d3a0dbe4bf 32 if (dIn == 0x07) {
AndyA 16:a8d3a0dbe4bf 33 inputBuffer[0] = 7;
AndyA 16:a8d3a0dbe4bf 34 inputPtr = 1;
AndyA 16:a8d3a0dbe4bf 35 } else {
AndyA 16:a8d3a0dbe4bf 36 inputPtr = 0;
AndyA 16:a8d3a0dbe4bf 37 }
AndyA 16:a8d3a0dbe4bf 38 }
AndyA 16:a8d3a0dbe4bf 39 } else { // waiting for data.
AndyA 16:a8d3a0dbe4bf 40 inputPtr++;
AndyA 18:ad407a2ed4c9 41 if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
AndyA 18:ad407a2ed4c9 42 parsePacket();
AndyA 18:ad407a2ed4c9 43 inputPtr = 0;
AndyA 18:ad407a2ed4c9 44 }
AndyA 16:a8d3a0dbe4bf 45 }
AndyA 16:a8d3a0dbe4bf 46 }
AndyA 16:a8d3a0dbe4bf 47
AndyA 16:a8d3a0dbe4bf 48 void FIZDigiPower::parsePacket()
AndyA 18:ad407a2ed4c9 49 {
AndyA 18:ad407a2ed4c9 50 // pc.puts("FIZ parse\r\n");
AndyA 16:a8d3a0dbe4bf 51 if (inputBuffer[0] != 0x07)
AndyA 16:a8d3a0dbe4bf 52 return;
AndyA 16:a8d3a0dbe4bf 53 if (inputBuffer[1] != 0x60)
AndyA 16:a8d3a0dbe4bf 54 return;
AndyA 18:ad407a2ed4c9 55 // checksum is sum of all bytes mod 256 = 00
AndyA 16:a8d3a0dbe4bf 56 int cs = 0;
AndyA 18:ad407a2ed4c9 57 for (int i=0; i<inputPtr; i++) {
AndyA 16:a8d3a0dbe4bf 58 cs += inputBuffer[i];
AndyA 18:ad407a2ed4c9 59 // pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
AndyA 18:ad407a2ed4c9 60 }
AndyA 18:ad407a2ed4c9 61 if (cs & 0x00ff) { // cs & 0x000000ff should give 0
AndyA 18:ad407a2ed4c9 62 // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
AndyA 18:ad407a2ed4c9 63 return;
AndyA 18:ad407a2ed4c9 64 }
AndyA 18:ad407a2ed4c9 65 // pc.puts("FIZ good\r\n");
AndyA 16:a8d3a0dbe4bf 66 uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
AndyA 16:a8d3a0dbe4bf 67 uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
AndyA 16:a8d3a0dbe4bf 68 uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
AndyA 16:a8d3a0dbe4bf 69
AndyA 17:5ce3fe98e76d 70 // MAY NEED TO SCALE THESE
AndyA 16:a8d3a0dbe4bf 71 _focus = focus_Position;
AndyA 16:a8d3a0dbe4bf 72 _iris = iris_Position;
AndyA 16:a8d3a0dbe4bf 73 _zoom = zoom_Position;
AndyA 18:ad407a2ed4c9 74
AndyA 16:a8d3a0dbe4bf 75 newData = true;
AndyA 16:a8d3a0dbe4bf 76 }