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@19:08e6a2283d58, 2021-06-29 (annotated)
- Committer:
- AndyA
- Date:
- Tue Jun 29 08:40:25 2021 +0000
- Revision:
- 19:08e6a2283d58
- Child:
- 26:7f66ac76cd5d
Added interactive version of fuji lens ;
Who changed what in which revision?
User | Revision | Line number | New 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(); |
AndyA | 19:08e6a2283d58 | 40 | inputBuffer[inputPtr] = dIn; |
AndyA | 19:08e6a2283d58 | 41 | if (inputPtr==0) { |
AndyA | 19:08e6a2283d58 | 42 | if (dIn == 0x07) { // correct length |
AndyA | 19:08e6a2283d58 | 43 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 44 | } |
AndyA | 19:08e6a2283d58 | 45 | } else if (inputPtr == 1) { |
AndyA | 19:08e6a2283d58 | 46 | if (dIn == 0x60) { // correct ID |
AndyA | 19:08e6a2283d58 | 47 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 48 | } else { // wrong ID |
AndyA | 19:08e6a2283d58 | 49 | if (dIn == 0x07) { |
AndyA | 19:08e6a2283d58 | 50 | inputBuffer[0] = 7; |
AndyA | 19:08e6a2283d58 | 51 | inputPtr = 1; |
AndyA | 19:08e6a2283d58 | 52 | } else { |
AndyA | 19:08e6a2283d58 | 53 | inputPtr = 0; |
AndyA | 19:08e6a2283d58 | 54 | } |
AndyA | 19:08e6a2283d58 | 55 | } |
AndyA | 19:08e6a2283d58 | 56 | } else { // waiting for data. |
AndyA | 19:08e6a2283d58 | 57 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 58 | if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs |
AndyA | 19:08e6a2283d58 | 59 | parsePacket(); |
AndyA | 19:08e6a2283d58 | 60 | inputPtr = 0; |
AndyA | 19:08e6a2283d58 | 61 | if (missedPackets) |
AndyA | 19:08e6a2283d58 | 62 | missedPackets--; |
AndyA | 19:08e6a2283d58 | 63 | } |
AndyA | 19:08e6a2283d58 | 64 | } |
AndyA | 19:08e6a2283d58 | 65 | } |
AndyA | 19:08e6a2283d58 | 66 | |
AndyA | 19:08e6a2283d58 | 67 | void FIZDigiPowerActive::parsePacket() |
AndyA | 19:08e6a2283d58 | 68 | { |
AndyA | 19:08e6a2283d58 | 69 | // pc.puts("FIZ parse\r\n"); |
AndyA | 19:08e6a2283d58 | 70 | if (inputBuffer[0] != 0x07) |
AndyA | 19:08e6a2283d58 | 71 | return; |
AndyA | 19:08e6a2283d58 | 72 | if (inputBuffer[1] != 0x60) |
AndyA | 19:08e6a2283d58 | 73 | return; |
AndyA | 19:08e6a2283d58 | 74 | // checksum is sum of all bytes mod 256 = 00 |
AndyA | 19:08e6a2283d58 | 75 | int cs = 0; |
AndyA | 19:08e6a2283d58 | 76 | for (int i=0; i<inputPtr; i++) { |
AndyA | 19:08e6a2283d58 | 77 | cs += inputBuffer[i]; |
AndyA | 19:08e6a2283d58 | 78 | // pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs); |
AndyA | 19:08e6a2283d58 | 79 | } |
AndyA | 19:08e6a2283d58 | 80 | if (cs & 0x00ff) { // cs & 0x000000ff should give 0 |
AndyA | 19:08e6a2283d58 | 81 | // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs); |
AndyA | 19:08e6a2283d58 | 82 | return; |
AndyA | 19:08e6a2283d58 | 83 | } |
AndyA | 19:08e6a2283d58 | 84 | // pc.puts("FIZ good\r\n"); |
AndyA | 19:08e6a2283d58 | 85 | uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; |
AndyA | 19:08e6a2283d58 | 86 | uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]; |
AndyA | 19:08e6a2283d58 | 87 | uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7]; |
AndyA | 19:08e6a2283d58 | 88 | |
AndyA | 19:08e6a2283d58 | 89 | // MAY NEED TO SCALE THESE |
AndyA | 19:08e6a2283d58 | 90 | _focus = focus_Position; |
AndyA | 19:08e6a2283d58 | 91 | _iris = iris_Position; |
AndyA | 19:08e6a2283d58 | 92 | _zoom = zoom_Position; |
AndyA | 19:08e6a2283d58 | 93 | |
AndyA | 19:08e6a2283d58 | 94 | newData = true; |
AndyA | 19:08e6a2283d58 | 95 | } |