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@80:0b7f1b85b626, 2022-08-30 (annotated)
- Committer:
- JamieB
- Date:
- Tue Aug 30 09:33:44 2022 +0000
- Revision:
- 80:0b7f1b85b626
- Parent:
- 71:7305a35cee58
- Child:
- 81:aee60dcce61b
Added Lens map configuration options
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; |
JamieB | 71:7305a35cee58 | 7 | initialising = true; //not used as lens parameter reading is disabled |
AndyA | 19:08e6a2283d58 | 8 | _port.baud(38400); |
JamieB | 71:7305a35cee58 | 9 | // read_lens_parameters(); |
AndyA | 19:08e6a2283d58 | 10 | _port.attach(callback(this, &FIZDigiPowerActive::OnRx)); |
AndyA | 19:08e6a2283d58 | 11 | missedPackets=0; |
JamieB | 71:7305a35cee58 | 12 | min_zoom = 0; |
JamieB | 71:7305a35cee58 | 13 | max_zoom = 0; |
AndyA | 19:08e6a2283d58 | 14 | |
AndyA | 19:08e6a2283d58 | 15 | } |
AndyA | 19:08e6a2283d58 | 16 | |
AndyA | 19:08e6a2283d58 | 17 | void FIZDigiPowerActive::requestCurrent(void) |
AndyA | 19:08e6a2283d58 | 18 | { |
JamieB | 71:7305a35cee58 | 19 | // if (initialising) { |
JamieB | 71:7305a35cee58 | 20 | // requestLensParameters(); |
JamieB | 71:7305a35cee58 | 21 | // return; |
AndyA | 19:08e6a2283d58 | 22 | if (missedPackets>5) { |
AndyA | 19:08e6a2283d58 | 23 | // configure output format |
JamieB | 71:7305a35cee58 | 24 | _port.putc(04); //data length (number of paramters) |
JamieB | 71:7305a35cee58 | 25 | _port.putc(0x70); //multi data setting |
JamieB | 71:7305a35cee58 | 26 | _port.putc(0x30); //iris |
JamieB | 71:7305a35cee58 | 27 | _port.putc(0x31); //zoom |
JamieB | 71:7305a35cee58 | 28 | _port.putc(0x32); //focus |
JamieB | 71:7305a35cee58 | 29 | _port.putc(0x53); //Extender |
JamieB | 71:7305a35cee58 | 30 | _port.putc(0xA6); //checksum (mod 256) |
AndyA | 19:08e6a2283d58 | 31 | missedPackets = 0; |
AndyA | 19:08e6a2283d58 | 32 | } |
AndyA | 19:08e6a2283d58 | 33 | |
AndyA | 19:08e6a2283d58 | 34 | _port.putc(00); |
AndyA | 19:08e6a2283d58 | 35 | _port.putc(0x60); |
AndyA | 19:08e6a2283d58 | 36 | _port.putc(0xA0); |
AndyA | 19:08e6a2283d58 | 37 | missedPackets++; |
AndyA | 19:08e6a2283d58 | 38 | } |
AndyA | 19:08e6a2283d58 | 39 | |
JamieB | 71:7305a35cee58 | 40 | //void FIZDigiPowerActive::requestLensParameters(void) |
JamieB | 71:7305a35cee58 | 41 | //{ |
JamieB | 71:7305a35cee58 | 42 | // if (min_zoom) { |
JamieB | 71:7305a35cee58 | 43 | // _port.putc(00); |
JamieB | 71:7305a35cee58 | 44 | // _port.putc(0x15); |
JamieB | 71:7305a35cee58 | 45 | // _port.putc(0xEB); |
JamieB | 71:7305a35cee58 | 46 | // } else { |
JamieB | 71:7305a35cee58 | 47 | // _port.putc(00); |
JamieB | 71:7305a35cee58 | 48 | // _port.putc(0x14); |
JamieB | 71:7305a35cee58 | 49 | // _port.putc(0xEC); |
JamieB | 71:7305a35cee58 | 50 | // missedPackets++; |
JamieB | 71:7305a35cee58 | 51 | //} |
JamieB | 71:7305a35cee58 | 52 | |
JamieB | 71:7305a35cee58 | 53 | |
JamieB | 71:7305a35cee58 | 54 | void FIZDigiPowerActive::read_lens_parameters() |
JamieB | 71:7305a35cee58 | 55 | { |
JamieB | 71:7305a35cee58 | 56 | _port.putc(00); |
JamieB | 71:7305a35cee58 | 57 | _port.putc(0x15); //0x16 for MOD |
JamieB | 71:7305a35cee58 | 58 | _port.putc(0xEB); //checksum is EA for MOD |
JamieB | 71:7305a35cee58 | 59 | wait_ms(50); |
JamieB | 71:7305a35cee58 | 60 | uint16_t raw_val = 0; |
JamieB | 71:7305a35cee58 | 61 | uint8_t tries = 5; |
JamieB | 71:7305a35cee58 | 62 | while (tries) { |
JamieB | 71:7305a35cee58 | 63 | tries--; |
JamieB | 71:7305a35cee58 | 64 | uint8_t dIn =_port.getc(); |
JamieB | 71:7305a35cee58 | 65 | inputBuffer[inputPtr] = dIn; |
JamieB | 71:7305a35cee58 | 66 | // printf("%02x ", dIn); |
JamieB | 71:7305a35cee58 | 67 | if (inputPtr==0) { |
JamieB | 71:7305a35cee58 | 68 | if (dIn == 0x02) { // correct length |
JamieB | 71:7305a35cee58 | 69 | inputPtr++; |
JamieB | 71:7305a35cee58 | 70 | } |
JamieB | 71:7305a35cee58 | 71 | } else if (inputPtr == 1) { |
JamieB | 71:7305a35cee58 | 72 | if (dIn == 0x15) { // correct ID |
JamieB | 71:7305a35cee58 | 73 | inputPtr++; |
JamieB | 71:7305a35cee58 | 74 | } else { // wrong ID |
JamieB | 71:7305a35cee58 | 75 | if (dIn == 0x02) { |
JamieB | 71:7305a35cee58 | 76 | inputBuffer[0] = 7; |
JamieB | 71:7305a35cee58 | 77 | inputPtr = 1; |
JamieB | 71:7305a35cee58 | 78 | } else { |
JamieB | 71:7305a35cee58 | 79 | inputPtr = 0; |
JamieB | 71:7305a35cee58 | 80 | } |
JamieB | 71:7305a35cee58 | 81 | } |
JamieB | 71:7305a35cee58 | 82 | } else { // waiting for data. |
JamieB | 71:7305a35cee58 | 83 | inputPtr++; |
JamieB | 71:7305a35cee58 | 84 | if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs |
JamieB | 71:7305a35cee58 | 85 | raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; |
JamieB | 71:7305a35cee58 | 86 | min_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000); |
JamieB | 71:7305a35cee58 | 87 | pc.printf("Min Focus: %d\r\n", min_zoom); |
JamieB | 71:7305a35cee58 | 88 | break; |
JamieB | 71:7305a35cee58 | 89 | } |
JamieB | 71:7305a35cee58 | 90 | } |
JamieB | 71:7305a35cee58 | 91 | } |
JamieB | 71:7305a35cee58 | 92 | inputPtr = 0; |
JamieB | 71:7305a35cee58 | 93 | _port.putc(00); |
JamieB | 71:7305a35cee58 | 94 | _port.putc(0x14); |
JamieB | 71:7305a35cee58 | 95 | _port.putc(0xEC); //ec for 0x14 |
JamieB | 71:7305a35cee58 | 96 | wait_ms(50); |
JamieB | 71:7305a35cee58 | 97 | raw_val = 0; |
JamieB | 71:7305a35cee58 | 98 | tries = 5; |
JamieB | 71:7305a35cee58 | 99 | while (tries) { |
JamieB | 71:7305a35cee58 | 100 | tries--; |
JamieB | 71:7305a35cee58 | 101 | uint8_t dIn =_port.getc(); |
JamieB | 71:7305a35cee58 | 102 | inputBuffer[inputPtr] = dIn; |
JamieB | 71:7305a35cee58 | 103 | // printf("%02x ", dIn); |
JamieB | 71:7305a35cee58 | 104 | if (inputPtr==0) { |
JamieB | 71:7305a35cee58 | 105 | if (dIn == 0x02) { // correct length |
JamieB | 71:7305a35cee58 | 106 | inputPtr++; |
JamieB | 71:7305a35cee58 | 107 | } |
JamieB | 71:7305a35cee58 | 108 | } else if (inputPtr == 1) { |
JamieB | 71:7305a35cee58 | 109 | if (dIn == 0x14) { // correct ID |
JamieB | 71:7305a35cee58 | 110 | inputPtr++; |
JamieB | 71:7305a35cee58 | 111 | } else { // wrong ID |
JamieB | 71:7305a35cee58 | 112 | if (dIn == 0x02) { |
JamieB | 71:7305a35cee58 | 113 | inputBuffer[0] = 7; |
JamieB | 71:7305a35cee58 | 114 | inputPtr = 1; |
JamieB | 71:7305a35cee58 | 115 | } else { |
JamieB | 71:7305a35cee58 | 116 | inputPtr = 0; |
JamieB | 71:7305a35cee58 | 117 | } |
JamieB | 71:7305a35cee58 | 118 | } |
JamieB | 71:7305a35cee58 | 119 | } else { // waiting for data. |
JamieB | 71:7305a35cee58 | 120 | inputPtr++; |
JamieB | 71:7305a35cee58 | 121 | if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs |
JamieB | 71:7305a35cee58 | 122 | raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; |
JamieB | 71:7305a35cee58 | 123 | max_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000); |
JamieB | 71:7305a35cee58 | 124 | pc.printf("Max Focus %d\r\n", max_zoom); |
JamieB | 71:7305a35cee58 | 125 | break; |
JamieB | 71:7305a35cee58 | 126 | } |
JamieB | 71:7305a35cee58 | 127 | } |
JamieB | 71:7305a35cee58 | 128 | } |
JamieB | 71:7305a35cee58 | 129 | |
JamieB | 71:7305a35cee58 | 130 | //quantise it |
JamieB | 71:7305a35cee58 | 131 | |
JamieB | 71:7305a35cee58 | 132 | |
JamieB | 71:7305a35cee58 | 133 | |
JamieB | 71:7305a35cee58 | 134 | } |
JamieB | 71:7305a35cee58 | 135 | |
JamieB | 71:7305a35cee58 | 136 | float FIZDigiPowerActive::get_16bit_float(uint16_t val) |
JamieB | 71:7305a35cee58 | 137 | { |
JamieB | 71:7305a35cee58 | 138 | //Non Standard 16bit float, 4 bit Exponent, 12bit mantissa |
JamieB | 71:7305a35cee58 | 139 | int expo = val >> 12; |
JamieB | 71:7305a35cee58 | 140 | if (expo & 0x8) |
JamieB | 71:7305a35cee58 | 141 | expo|= ~0xF; |
JamieB | 71:7305a35cee58 | 142 | float ret = (float) ((val &0xFFF) * pow((double)10.0, (double)expo)); |
JamieB | 71:7305a35cee58 | 143 | return ret; |
JamieB | 71:7305a35cee58 | 144 | } |
JamieB | 71:7305a35cee58 | 145 | |
AndyA | 19:08e6a2283d58 | 146 | // expect hex data format: |
AndyA | 19:08e6a2283d58 | 147 | // [dat len] [command] [data] [cs] |
JamieB | 71:7305a35cee58 | 148 | // only command we care about is multi reply which is 7 bytes of data - iris, zoom, focus, switch 4 |
JamieB | 71:7305a35cee58 | 149 | // 07 60 [iris1][iris2] .... [cs] |
AndyA | 19:08e6a2283d58 | 150 | void FIZDigiPowerActive::OnRx(void) |
AndyA | 19:08e6a2283d58 | 151 | { |
JamieB | 71:7305a35cee58 | 152 | // if (initialising){ |
JamieB | 71:7305a35cee58 | 153 | // read_lens_parameters(); |
JamieB | 71:7305a35cee58 | 154 | // return; |
AndyA | 19:08e6a2283d58 | 155 | uint8_t dIn = _port.getc(); |
JamieB | 26:7f66ac76cd5d | 156 | // pc.printf("R"); |
AndyA | 19:08e6a2283d58 | 157 | inputBuffer[inputPtr] = dIn; |
AndyA | 19:08e6a2283d58 | 158 | if (inputPtr==0) { |
AndyA | 19:08e6a2283d58 | 159 | if (dIn == 0x07) { // correct length |
AndyA | 19:08e6a2283d58 | 160 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 161 | } |
AndyA | 19:08e6a2283d58 | 162 | } else if (inputPtr == 1) { |
AndyA | 19:08e6a2283d58 | 163 | if (dIn == 0x60) { // correct ID |
AndyA | 19:08e6a2283d58 | 164 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 165 | } else { // wrong ID |
AndyA | 19:08e6a2283d58 | 166 | if (dIn == 0x07) { |
AndyA | 19:08e6a2283d58 | 167 | inputBuffer[0] = 7; |
AndyA | 19:08e6a2283d58 | 168 | inputPtr = 1; |
AndyA | 19:08e6a2283d58 | 169 | } else { |
AndyA | 19:08e6a2283d58 | 170 | inputPtr = 0; |
AndyA | 19:08e6a2283d58 | 171 | } |
AndyA | 19:08e6a2283d58 | 172 | } |
AndyA | 19:08e6a2283d58 | 173 | } else { // waiting for data. |
AndyA | 19:08e6a2283d58 | 174 | inputPtr++; |
AndyA | 19:08e6a2283d58 | 175 | if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs |
AndyA | 19:08e6a2283d58 | 176 | parsePacket(); |
AndyA | 19:08e6a2283d58 | 177 | inputPtr = 0; |
AndyA | 19:08e6a2283d58 | 178 | if (missedPackets) |
AndyA | 19:08e6a2283d58 | 179 | missedPackets--; |
AndyA | 19:08e6a2283d58 | 180 | } |
AndyA | 19:08e6a2283d58 | 181 | } |
AndyA | 19:08e6a2283d58 | 182 | } |
AndyA | 19:08e6a2283d58 | 183 | |
AndyA | 19:08e6a2283d58 | 184 | void FIZDigiPowerActive::parsePacket() |
AndyA | 19:08e6a2283d58 | 185 | { |
AndyA | 19:08e6a2283d58 | 186 | // pc.puts("FIZ parse\r\n"); |
AndyA | 19:08e6a2283d58 | 187 | if (inputBuffer[0] != 0x07) |
AndyA | 19:08e6a2283d58 | 188 | return; |
AndyA | 19:08e6a2283d58 | 189 | if (inputBuffer[1] != 0x60) |
AndyA | 19:08e6a2283d58 | 190 | return; |
AndyA | 19:08e6a2283d58 | 191 | // checksum is sum of all bytes mod 256 = 00 |
AndyA | 19:08e6a2283d58 | 192 | int cs = 0; |
AndyA | 19:08e6a2283d58 | 193 | for (int i=0; i<inputPtr; i++) { |
AndyA | 19:08e6a2283d58 | 194 | cs += inputBuffer[i]; |
AndyA | 19:08e6a2283d58 | 195 | // pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs); |
AndyA | 19:08e6a2283d58 | 196 | } |
AndyA | 19:08e6a2283d58 | 197 | if (cs & 0x00ff) { // cs & 0x000000ff should give 0 |
AndyA | 19:08e6a2283d58 | 198 | // pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs); |
AndyA | 19:08e6a2283d58 | 199 | return; |
AndyA | 19:08e6a2283d58 | 200 | } |
AndyA | 19:08e6a2283d58 | 201 | // pc.puts("FIZ good\r\n"); |
JamieB | 80:0b7f1b85b626 | 202 | uint16_t iris_Position = getAbsoluteIris(((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]); //((uint16_t)inputBuffer[2])<<8 | inputBuffer[3]; |
JamieB | 80:0b7f1b85b626 | 203 | uint16_t zoom_Position = getAbsoluteZoom(((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]); //((uint16_t)inputBuffer[4])<<8 | inputBuffer[5]; |
JamieB | 80:0b7f1b85b626 | 204 | uint32_t focus_Position = getAbsoluteFocus(((uint32_t)inputBuffer[6])<<8 | inputBuffer[7]); //((uint32_t)inputBuffer[6])<<8 | inputBuffer[7]; |
AndyA | 19:08e6a2283d58 | 205 | |
AndyA | 19:08e6a2283d58 | 206 | // MAY NEED TO SCALE THESE |
JamieB | 71:7305a35cee58 | 207 | _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset; |
JamieB | 71:7305a35cee58 | 208 | _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset; |
JamieB | 71:7305a35cee58 | 209 | _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset; |
AndyA | 19:08e6a2283d58 | 210 | |
AndyA | 19:08e6a2283d58 | 211 | newData = true; |
AndyA | 19:08e6a2283d58 | 212 | } |