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@85:0cc5931bb9ef, 18 months ago (annotated)

Committer:
JamieB
Date:
Thu Dec 15 06:05:30 2022 +0000
Revision:
85:0cc5931bb9ef
Parent:
81:aee60dcce61b 
Push to somewhere else due to merge issue

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;
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 81:aee60dcce61b 202 uint16_t iris_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
JamieB 81:aee60dcce61b 203 uint16_t zoom_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
JamieB 81:aee60dcce61b 204 uint32_t focus_Position = ((uint32_t)inputBuffer[6])<<8 | inputBuffer[7];
AndyA 19:08e6a2283d58 205
JamieB 81:aee60dcce61b 206 if (UserSettings.absolute_focus) {
JamieB 81:aee60dcce61b 207 _focus = getAbsoluteFocus(focus_Position);
JamieB 81:aee60dcce61b 208 } else {
JamieB 81:aee60dcce61b 209 _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
JamieB 81:aee60dcce61b 210 }
JamieB 81:aee60dcce61b 211 if (UserSettings.absolute_iris) {
JamieB 81:aee60dcce61b 212 _iris = getAbsoluteIris(iris_Position);
JamieB 81:aee60dcce61b 213 } else {
JamieB 81:aee60dcce61b 214 _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
JamieB 81:aee60dcce61b 215 }
JamieB 81:aee60dcce61b 216 if (UserSettings.absolute_zoom) {
JamieB 81:aee60dcce61b 217 _zoom = getAbsoluteZoom(zoom_Position);
JamieB 81:aee60dcce61b 218 } else {
JamieB 81:aee60dcce61b 219 _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;
JamieB 81:aee60dcce61b 220 }
JamieB 81:aee60dcce61b 221 // // MAY NEED TO SCALE THESE
JamieB 81:aee60dcce61b 222 // _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
JamieB 81:aee60dcce61b 223 // _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
JamieB 81:aee60dcce61b 224 // _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;
AndyA 19:08e6a2283d58 225
AndyA 19:08e6a2283d58 226 newData = true;
AndyA 19:08e6a2283d58 227 }