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@17:5ce3fe98e76d, 2021-04-30 (annotated)
- Committer:
- AndyA
- Date:
- Fri Apr 30 12:21:01 2021 +0000
- Revision:
- 17:5ce3fe98e76d
- Parent:
- 16:a8d3a0dbe4bf
- Child:
- 18:ad407a2ed4c9
Moved iris scale into fiz specific code
Who changed what in which revision?
User | Revision | Line number | New 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 | 16:a8d3a0dbe4bf | 25 | if (dIn == 0x07) // correct length |
AndyA | 16:a8d3a0dbe4bf | 26 | inputPtr++; |
AndyA | 16:a8d3a0dbe4bf | 27 | } else if (inputPtr == 1) { |
AndyA | 16:a8d3a0dbe4bf | 28 | if (dIn == 0x60) // correct ID |
AndyA | 16:a8d3a0dbe4bf | 29 | inputPtr++; |
AndyA | 16:a8d3a0dbe4bf | 30 | else { // wrong ID |
AndyA | 16:a8d3a0dbe4bf | 31 | if (dIn == 0x07) { |
AndyA | 16:a8d3a0dbe4bf | 32 | inputBuffer[0] = 7; |
AndyA | 16:a8d3a0dbe4bf | 33 | inputPtr = 1; |
AndyA | 16:a8d3a0dbe4bf | 34 | } else { |
AndyA | 16:a8d3a0dbe4bf | 35 | inputPtr = 0; |
AndyA | 16:a8d3a0dbe4bf | 36 | } |
AndyA | 16:a8d3a0dbe4bf | 37 | } |
AndyA | 16:a8d3a0dbe4bf | 38 | } else if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs |
AndyA | 16:a8d3a0dbe4bf | 39 | parsePacket(); |
AndyA | 16:a8d3a0dbe4bf | 40 | inputPtr = 0; |
AndyA | 16:a8d3a0dbe4bf | 41 | } else { // waiting for data. |
AndyA | 16:a8d3a0dbe4bf | 42 | inputPtr++; |
AndyA | 16:a8d3a0dbe4bf | 43 | } |
AndyA | 16:a8d3a0dbe4bf | 44 | } |
AndyA | 16:a8d3a0dbe4bf | 45 | |
AndyA | 16:a8d3a0dbe4bf | 46 | void FIZDigiPower::parsePacket() |
AndyA | 16:a8d3a0dbe4bf | 47 | { |
AndyA | 16:a8d3a0dbe4bf | 48 | if (inputBuffer[0] != 0x07) |
AndyA | 16:a8d3a0dbe4bf | 49 | return; |
AndyA | 16:a8d3a0dbe4bf | 50 | if (inputBuffer[1] != 0x60) |
AndyA | 16:a8d3a0dbe4bf | 51 | return; |
AndyA | 16:a8d3a0dbe4bf | 52 | // checksum is sum of all bytes mod 256 = 00 |
AndyA | 16:a8d3a0dbe4bf | 53 | int cs = 0; |
AndyA | 16:a8d3a0dbe4bf | 54 | for (int i=0;i<inputPtr;i++) { |
AndyA | 16:a8d3a0dbe4bf | 55 | cs += inputBuffer[i]; |
AndyA | 16:a8d3a0dbe4bf | 56 | } |
AndyA | 16:a8d3a0dbe4bf | 57 | if (cs & 0x00ff) // cs & 0x000000ff should give 0 |
AndyA | 16:a8d3a0dbe4bf | 58 | return; |
AndyA | 16:a8d3a0dbe4bf | 59 | |
AndyA | 16:a8d3a0dbe4bf | 60 | uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; |
AndyA | 16:a8d3a0dbe4bf | 61 | uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]; |
AndyA | 16:a8d3a0dbe4bf | 62 | uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7]; |
AndyA | 16:a8d3a0dbe4bf | 63 | |
AndyA | 17:5ce3fe98e76d | 64 | // MAY NEED TO SCALE THESE |
AndyA | 16:a8d3a0dbe4bf | 65 | _focus = focus_Position; |
AndyA | 16:a8d3a0dbe4bf | 66 | _iris = iris_Position; |
AndyA | 16:a8d3a0dbe4bf | 67 | _zoom = zoom_Position; |
AndyA | 16:a8d3a0dbe4bf | 68 | |
AndyA | 16:a8d3a0dbe4bf | 69 | newData = true; |
AndyA | 16:a8d3a0dbe4bf | 70 | } |