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

main.cpp@85:0cc5931bb9ef, 17 months ago (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamieB 81:aee60dcce61b 1 #define APP_VERSION 0.32
JamieB 41:a3556d765635 2
AndyA 23:5c237f04327d 3 /*
AndyA 23:5c237f04327d 4 Settings file options
AndyA 23:5c237f04327d 5
AndyA 23:5c237f04327d 6 On startup system will look for settings.txt on the built in mbed drive.
AndyA 23:5c237f04327d 7
AndyA 23:5c237f04327d 8 This file should be ascii text with one option per line.
AndyA 23:5c237f04327d 9 The following options supported:
AndyA 23:5c237f04327d 10
AndyA 23:5c237f04327d 11 Output_Format=n
AndyA 23:5c237f04327d 12 Sets the serial output format.
AndyA 23:5c237f04327d 13 n = 0 - VIPS
AndyA 23:5c237f04327d 14 n = 1 - FreeD
AndyA 23:5c237f04327d 15
AndyA 23:5c237f04327d 16
AndyA 23:5c237f04327d 17 FIZ_Format=n
AndyA 23:5c237f04327d 18 Sets the FIZ reader to use
AndyA 23:5c237f04327d 19 n = 0 - Preston
AndyA 23:5c237f04327d 20 n = 1 - Fuji passive listen mode (skycam)
AndyA 23:5c237f04327d 21 n = 2 - Fuji active mode
AndyA 32:bfb385d35097 22 n = 3 - Canon
AndyA 62:dcb92159ac8e 23 n = 4 - Arri
AndyA 23:5c237f04327d 24
AndyA 23:5c237f04327d 25 FreeD_Port=pppp
AndyA 23:5c237f04327d 26 Sets the UDP port for FreeD network output.
AndyA 23:5c237f04327d 27 Data is sent as a UDP broadcast on the select port number.
AndyA 23:5c237f04327d 28 A port number of 0 disables UDP output.
AndyA 23:5c237f04327d 29
JamieB 42:9653486372a6 30 VIPS_UDP_Port=pppp
JamieB 42:9653486372a6 31 Same As Above
JamieB 42:9653486372a6 32
AndyA 23:5c237f04327d 33 IP_addr=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 34 Subnet=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 35 Gateway=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 36 Set the IPv4 address to use for the ethernet interface.
AndyA 23:5c237f04327d 37 All 3 values must be set for a static address to be used otherwise DHCP will be used.
AndyA 23:5c237f04327d 38
AndyA 34:c864a0c67dbf 39
AndyA 34:c864a0c67dbf 40 FilterOrder=n
AndyA 34:c864a0c67dbf 41 FilterFreq=m.mm
AndyA 34:c864a0c67dbf 42 FilterRate=r.rr
AndyA 34:c864a0c67dbf 43 Low pass filter settings for channels that have it enabled.
AndyA 34:c864a0c67dbf 44 Filter is of order n with a cut off at m Hz assuming input data rate is at r Hz
AndyA 34:c864a0c67dbf 45 Filter order must be set to enable filters.
AndyA 34:c864a0c67dbf 46 Frequency default is 10Hz
AndyA 34:c864a0c67dbf 47 Rate default is 100Hz
AndyA 34:c864a0c67dbf 48
AndyA 34:c864a0c67dbf 49 FilterXY=1
AndyA 34:c864a0c67dbf 50 FilterZ=1
AndyA 34:c864a0c67dbf 51 FilterRoll=1
AndyA 34:c864a0c67dbf 52 FilterPitch=1
AndyA 34:c864a0c67dbf 53 FilterYaw=1
AndyA 34:c864a0c67dbf 54 Enable channels to low pass filter. All filters use the settings given above.
AndyA 34:c864a0c67dbf 55 A value of 1 enables the filter. A value of 0 or skipping the line disables the filter.
AndyA 34:c864a0c67dbf 56
AndyA 34:c864a0c67dbf 57 NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
AndyA 34:c864a0c67dbf 58
AndyA 34:c864a0c67dbf 59
AndyA 44:fe7fcf62c75d 60 ExtendedOutput=1
AndyA 44:fe7fcf62c75d 61 Enable extra fields in VIPS output.
AndyA 44:fe7fcf62c75d 62
AndyA 44:fe7fcf62c75d 63
AndyA 23:5c237f04327d 64 All settings are case sensitive.
AndyA 23:5c237f04327d 65 Do NOT include spaces in the options lines.
AndyA 23:5c237f04327d 66 All options default to a value of 0 is omitted from the file.
AndyA 23:5c237f04327d 67
AndyA 23:5c237f04327d 68 */
AndyA 23:5c237f04327d 69
AndyA 0:97661408d0f9 70 #include "mbed.h"
AndyA 0:97661408d0f9 71 #include "LTCApp.h"
AndyA 22:0dd9c1b5664a 72 #include "EthernetInterface.h"
JamieB 79:1910ae03cb2e 73 #include <vector>
AndyA 0:97661408d0f9 74
AndyA 22:0dd9c1b5664a 75 //#define enableUSBStick
AndyA 15:830fc953edd9 76 //#define enableFakePPF
AndyA 15:830fc953edd9 77
AndyA 15:830fc953edd9 78 // delay transmit to n ms after the frame
AndyA 15:830fc953edd9 79 // comment out rather than set to 0 to disable
AndyA 15:830fc953edd9 80 //#define Delay_TX_By 10
AndyA 14:76083dc18b0d 81
AndyA 14:76083dc18b0d 82 #ifdef enableUSBStick
AndyA 14:76083dc18b0d 83 #include "MSCFileSystem.h"
AndyA 14:76083dc18b0d 84 MSCFileSystem msc("msc");
AndyA 14:76083dc18b0d 85 #endif
AndyA 9:7214e3c3e5f8 86
AndyA 0:97661408d0f9 87 BufferedSerial pc(USBTX, USBRX);
AndyA 0:97661408d0f9 88 VIPSSerial VIPS(p28, p27);
AndyA 50:d6c56eccf56a 89 BufferedSerial COM1(p13, p14);
AndyA 16:a8d3a0dbe4bf 90 FIZReader *FIZPort;
AndyA 68:9e68287f0ab7 91 EthernetInterface eth;
AndyA 0:97661408d0f9 92
AndyA 11:ef7f6591b776 93 DigitalOut led1(LED1);
AndyA 0:97661408d0f9 94 DigitalOut led2(LED2);
AndyA 0:97661408d0f9 95 DigitalOut led3(LED3);
AndyA 0:97661408d0f9 96 DigitalOut frameToggle(LED4);
AndyA 0:97661408d0f9 97
AndyA 0:97661408d0f9 98 LTCDecode LTCInput(p7);
AndyA 0:97661408d0f9 99 InterruptIn PPFin(p29);
AndyA 0:97661408d0f9 100 InterruptIn Syncin(p8);
AndyA 0:97661408d0f9 101
AndyA 11:ef7f6591b776 102 DigitalIn logButton(p17,PullDown);
AndyA 11:ef7f6591b776 103
AndyA 15:830fc953edd9 104 DigitalOut RedLED(p18); // red
AndyA 11:ef7f6591b776 105 DigitalOut GreenLED(p19);
AndyA 11:ef7f6591b776 106 DigitalOut BlueLED(p20);
AndyA 11:ef7f6591b776 107
AndyA 68:9e68287f0ab7 108 LocalFileSystem localFS("local");
AndyA 68:9e68287f0ab7 109
AndyA 11:ef7f6591b776 110 #define LED_OFF 1
AndyA 11:ef7f6591b776 111 #define LED_ON 0
AndyA 14:76083dc18b0d 112
JamieB 27:498cce52fe5f 113 #define setRED() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_ON;})
JamieB 27:498cce52fe5f 114 #define setGREEN() ({GreenLED=LED_ON;BlueLED = LED_OFF;RedLED = LED_OFF;})
JamieB 27:498cce52fe5f 115 #define setBLUE() ({GreenLED=LED_OFF;BlueLED = LED_ON;RedLED = LED_OFF;})
JamieB 27:498cce52fe5f 116 #define setOFF() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_OFF;})
AndyA 9:7214e3c3e5f8 117
AndyA 9:7214e3c3e5f8 118 frameclock movieTime;
AndyA 9:7214e3c3e5f8 119
AndyA 0:97661408d0f9 120 // clock to time everything with
AndyA 0:97661408d0f9 121 Timer inputTimer;
AndyA 12:06050debf014 122 Timeout resetTimeout;
AndyA 16:a8d3a0dbe4bf 123 Timeout filteringTimeout;
AndyA 16:a8d3a0dbe4bf 124 Timeout BypassTimeout;
AndyA 0:97661408d0f9 125
AndyA 15:830fc953edd9 126 #ifdef enableFakePPF
AndyA 15:830fc953edd9 127 Ticker FakePPF;
AndyA 15:830fc953edd9 128 #endif
AndyA 15:830fc953edd9 129
AndyA 9:7214e3c3e5f8 130 float logButtonDownTime;
AndyA 9:7214e3c3e5f8 131 float logButtonUpTime;
AndyA 9:7214e3c3e5f8 132 int logButtonLastState;
AndyA 9:7214e3c3e5f8 133
AndyA 16:a8d3a0dbe4bf 134 bool bypassMode = false;
AndyA 9:7214e3c3e5f8 135 bool logging = false;
AndyA 9:7214e3c3e5f8 136 FILE *logFile = NULL;
AndyA 9:7214e3c3e5f8 137
AndyA 9:7214e3c3e5f8 138
AndyA 0:97661408d0f9 139 // Time since last frame event, used for position output interpolation
AndyA 0:97661408d0f9 140 Timer TimeSinceLastFrame;
AndyA 0:97661408d0f9 141 uint32_t TimeSinceLastFrameWrap;
AndyA 0:97661408d0f9 142
JamieB 58:8cce8fcc765f 143 //Debug Timer for Ethernet
JamieB 58:8cce8fcc765f 144 Timer EthernetTimer;
JamieB 58:8cce8fcc765f 145 uint32_t TimeSinceLastEthernetPacket;
JamieB 58:8cce8fcc765f 146
AndyA 0:97661408d0f9 147 // used to start PPS at the correct point
AndyA 0:97661408d0f9 148 Timeout PPSsyncTimer;
AndyA 0:97661408d0f9 149
AndyA 15:830fc953edd9 150 Timeout OutputDelayTimer;
AndyA 50:d6c56eccf56a 151 Timeout UDPOutputDelayTimer;
AndyA 15:830fc953edd9 152
AndyA 0:97661408d0f9 153 // used to generate PPS edges
AndyA 0:97661408d0f9 154 Ticker PPSOutputTimer;
AndyA 0:97661408d0f9 155
AndyA 0:97661408d0f9 156 frameRates detectedRate;
AndyA 0:97661408d0f9 157 bool ppsRunning = false; // set when PPS start has been scheduled
AndyA 0:97661408d0f9 158 volatile bool ppsActive = false; // set when PPS is actuallt going (up to 1 second later)
AndyA 0:97661408d0f9 159
AndyA 0:97661408d0f9 160 volatile bool PPSHigh;
AndyA 0:97661408d0f9 161 bool resync = false;
AndyA 0:97661408d0f9 162 bool resyncDone = false;
AndyA 0:97661408d0f9 163 uint32_t resyncPeriod;
AndyA 0:97661408d0f9 164 bool OKToCheckSync = false;
AndyA 0:97661408d0f9 165 volatile uint32_t VBOXTicks = 0; // time at the NEXT PPS edge
AndyA 0:97661408d0f9 166 uint32_t lastPPSSecondStart;
JamieB 76:c7c795b22f63 167 volatile bool StopTCPListening = false;
AndyA 0:97661408d0f9 168
AndyA 0:97661408d0f9 169 #define _longPPMTrackLen_ 20
AndyA 0:97661408d0f9 170 float PPMErrors[_longPPMTrackLen_];
AndyA 0:97661408d0f9 171 float PPMTrackTotal;
AndyA 0:97661408d0f9 172 int PPMTrackIndex;
AndyA 0:97661408d0f9 173 float PPMHighAcc;
AndyA 0:97661408d0f9 174 float remainingClockError;
AndyA 0:97661408d0f9 175 bool ppmCorrection;
AndyA 0:97661408d0f9 176
AndyA 22:0dd9c1b5664a 177 enum SerialOutput {mode_VIPS, mode_FreeD};
AndyA 62:dcb92159ac8e 178 enum FIZFormats {formatPreston, formatFujiPassive, formatFujiActive, formatCanon, formatArri};
JamieB 20:ce1d8fbff68f 179
AndyA 22:0dd9c1b5664a 180 UserSettings_t UserSettings;
AndyA 9:7214e3c3e5f8 181
AndyA 44:fe7fcf62c75d 182 uint8_t FlexibleVIPSOut[200];
AndyA 44:fe7fcf62c75d 183 int VIPSOutSize;
AndyA 0:97661408d0f9 184
JamieB 20:ce1d8fbff68f 185 struct D1MsgFormat_s fdPacket;
AndyA 1:dd1f7e162f91 186
AndyA 32:bfb385d35097 187 volatile bool EthTxNow = false;
JamieB 58:8cce8fcc765f 188 Mutex NewData;
JamieB 58:8cce8fcc765f 189
JamieB 61:e734e86661b0 190 bool TXFrame = true;
JamieB 61:e734e86661b0 191
JamieB 80:0b7f1b85b626 192 // int pos_lower = 0;
JamieB 80:0b7f1b85b626 193 // int pos_upper = 0;
JamieB 80:0b7f1b85b626 194 // int pos_value = 0;
AndyA 32:bfb385d35097 195
AndyA 16:a8d3a0dbe4bf 196 void filterOff(void)
AndyA 16:a8d3a0dbe4bf 197 {
AndyA 16:a8d3a0dbe4bf 198 VIPS.EnableSmoothing(false);
AndyA 16:a8d3a0dbe4bf 199 pc.puts("FilterTimeout");
AndyA 16:a8d3a0dbe4bf 200 }
AndyA 16:a8d3a0dbe4bf 201
AndyA 16:a8d3a0dbe4bf 202 /***************************
AndyA 16:a8d3a0dbe4bf 203 *
AndyA 16:a8d3a0dbe4bf 204 * Input format is
AndyA 16:a8d3a0dbe4bf 205 * enable filter $RLFIZ FILTER ON\n
AndyA 16:a8d3a0dbe4bf 206 * disable filter $RLFIZ FILTER OFF\n
AndyA 16:a8d3a0dbe4bf 207 *
AndyA 16:a8d3a0dbe4bf 208 * Filter mode auto exits after 30 seconds
AndyA 16:a8d3a0dbe4bf 209 *
AndyA 16:a8d3a0dbe4bf 210 ****************************/
AndyA 16:a8d3a0dbe4bf 211
AndyA 16:a8d3a0dbe4bf 212 const int C1InputBufferSize = 20;
AndyA 16:a8d3a0dbe4bf 213 char C1InputBuffer[C1InputBufferSize];
AndyA 16:a8d3a0dbe4bf 214 int C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 215
AndyA 16:a8d3a0dbe4bf 216 void parseC1Input(void)
AndyA 16:a8d3a0dbe4bf 217 {
AndyA 16:a8d3a0dbe4bf 218 if (C1InputBuffer[1] != 'R')
AndyA 16:a8d3a0dbe4bf 219 return;
AndyA 16:a8d3a0dbe4bf 220 if (C1InputBuffer[2] != 'L')
AndyA 16:a8d3a0dbe4bf 221 return;
AndyA 16:a8d3a0dbe4bf 222 if (C1InputBuffer[3] != 'F')
AndyA 16:a8d3a0dbe4bf 223 return;
AndyA 16:a8d3a0dbe4bf 224 if (C1InputBuffer[4] != 'I')
AndyA 16:a8d3a0dbe4bf 225 return;
AndyA 16:a8d3a0dbe4bf 226 if (C1InputBuffer[5] != 'Z')
AndyA 16:a8d3a0dbe4bf 227 return;
AndyA 16:a8d3a0dbe4bf 228
AndyA 16:a8d3a0dbe4bf 229 if (C1InputBuffer[7] == 'F') {
AndyA 16:a8d3a0dbe4bf 230 if (C1InputBuffer[8] != 'I')
AndyA 16:a8d3a0dbe4bf 231 return;
AndyA 16:a8d3a0dbe4bf 232 if (C1InputBuffer[9] != 'L')
AndyA 16:a8d3a0dbe4bf 233 return;
AndyA 16:a8d3a0dbe4bf 234 // 10 = T, 11 = E, 12=R, 13= space, 14 = O, 15 = N or F
AndyA 16:a8d3a0dbe4bf 235 if (C1InputBuffer[14] != 'O')
AndyA 16:a8d3a0dbe4bf 236 return;
AndyA 16:a8d3a0dbe4bf 237
AndyA 16:a8d3a0dbe4bf 238 if (C1InputBuffer[15] == 'N') {
JamieB 66:066b16c6c34f 239 VIPS.ForceSmoothing(true);
AndyA 16:a8d3a0dbe4bf 240 filteringTimeout.attach(callback(&filterOff),30.0f);
AndyA 16:a8d3a0dbe4bf 241 pc.puts("FilterOn\n");
AndyA 16:a8d3a0dbe4bf 242 return;
AndyA 16:a8d3a0dbe4bf 243 } else {
JamieB 66:066b16c6c34f 244 VIPS.ForceSmoothing(false);
AndyA 16:a8d3a0dbe4bf 245 filteringTimeout.detach();
AndyA 16:a8d3a0dbe4bf 246 pc.puts("FilterOFF\n");
AndyA 16:a8d3a0dbe4bf 247 return;
AndyA 16:a8d3a0dbe4bf 248 }
AndyA 16:a8d3a0dbe4bf 249 } // if F
AndyA 16:a8d3a0dbe4bf 250
AndyA 16:a8d3a0dbe4bf 251 }
AndyA 16:a8d3a0dbe4bf 252
AndyA 16:a8d3a0dbe4bf 253
AndyA 16:a8d3a0dbe4bf 254 void ExitBypass(void)
AndyA 16:a8d3a0dbe4bf 255 {
AndyA 16:a8d3a0dbe4bf 256 VIPS.EnableBypass(false);
AndyA 16:a8d3a0dbe4bf 257 bypassMode = false;
AndyA 16:a8d3a0dbe4bf 258 }
AndyA 16:a8d3a0dbe4bf 259
AndyA 16:a8d3a0dbe4bf 260
AndyA 16:a8d3a0dbe4bf 261 void vipsBypassRx(char byte)
AndyA 16:a8d3a0dbe4bf 262 {
AndyA 16:a8d3a0dbe4bf 263 COM1.putc(byte);
AndyA 16:a8d3a0dbe4bf 264 }
AndyA 16:a8d3a0dbe4bf 265
AndyA 16:a8d3a0dbe4bf 266
AndyA 16:a8d3a0dbe4bf 267 void onOutputSerialRx(void)
AndyA 16:a8d3a0dbe4bf 268 {
AndyA 16:a8d3a0dbe4bf 269 static bool got0x07 = false;
AndyA 16:a8d3a0dbe4bf 270 led1=!led1;
JamieB 66:066b16c6c34f 271 while (COM1.RawSerial::readable()) {
AndyA 16:a8d3a0dbe4bf 272 if (bypassMode) {
JamieB 66:066b16c6c34f 273 VIPS.bypassTx(COM1.RawSerial::getc());
AndyA 16:a8d3a0dbe4bf 274 BypassTimeout.attach(&ExitBypass,5);
AndyA 16:a8d3a0dbe4bf 275 } else {
JamieB 66:066b16c6c34f 276 C1InputBuffer[C1InputPtr] = COM1.RawSerial::getc();
AndyA 16:a8d3a0dbe4bf 277 pc.putc(C1InputBuffer[C1InputPtr]);
AndyA 16:a8d3a0dbe4bf 278 if (C1InputPtr == 0) {
AndyA 16:a8d3a0dbe4bf 279 if (got0x07) {
AndyA 16:a8d3a0dbe4bf 280 got0x07 = false;
AndyA 16:a8d3a0dbe4bf 281 if ((C1InputBuffer[0] >= 5) && (C1InputBuffer[0] <=11)) {
AndyA 16:a8d3a0dbe4bf 282 VIPS.bypassTx(0x07);
AndyA 16:a8d3a0dbe4bf 283 VIPS.bypassTx(C1InputBuffer[0]);
AndyA 16:a8d3a0dbe4bf 284 bypassMode = true;
AndyA 16:a8d3a0dbe4bf 285 BypassTimeout.attach(&ExitBypass,5);
AndyA 16:a8d3a0dbe4bf 286 }
AndyA 16:a8d3a0dbe4bf 287 } else if (C1InputBuffer[0] == '$')
AndyA 16:a8d3a0dbe4bf 288 C1InputPtr = 1;
AndyA 16:a8d3a0dbe4bf 289 else if (C1InputBuffer[0] == 0x07)
AndyA 16:a8d3a0dbe4bf 290 got0x07 = true;
AndyA 16:a8d3a0dbe4bf 291 } else if (C1InputBuffer[C1InputPtr] == '\n') {
AndyA 16:a8d3a0dbe4bf 292 parseC1Input();
AndyA 16:a8d3a0dbe4bf 293 C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 294 } else {
AndyA 16:a8d3a0dbe4bf 295 C1InputPtr++;
AndyA 16:a8d3a0dbe4bf 296 if (C1InputPtr == C1InputBufferSize)
AndyA 16:a8d3a0dbe4bf 297 C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 298 }
AndyA 16:a8d3a0dbe4bf 299 }
AndyA 16:a8d3a0dbe4bf 300 }
AndyA 16:a8d3a0dbe4bf 301 }
AndyA 16:a8d3a0dbe4bf 302
AndyA 16:a8d3a0dbe4bf 303
AndyA 16:a8d3a0dbe4bf 304
AndyA 16:a8d3a0dbe4bf 305
AndyA 1:dd1f7e162f91 306 void prepPacketOut()
AndyA 1:dd1f7e162f91 307 {
AndyA 44:fe7fcf62c75d 308 FlexibleVIPSOut[0]=0x24;
AndyA 44:fe7fcf62c75d 309 FlexibleVIPSOut[1]=0xd9;
AndyA 1:dd1f7e162f91 310 }
AndyA 1:dd1f7e162f91 311
AndyA 15:830fc953edd9 312 void onOutputTxTime(void)
AndyA 15:830fc953edd9 313 {
AndyA 22:0dd9c1b5664a 314 switch (UserSettings.SerialOutMode) {
JamieB 24:16fd010a71d0 315 case mode_FreeD: {
JamieB 24:16fd010a71d0 316 char *dataPtr = (char *)&fdPacket;
AndyA 22:0dd9c1b5664a 317 for (int byte=0; byte<sizeof(struct D1MsgFormat_s); byte++)
AndyA 22:0dd9c1b5664a 318 COM1.putc(*(dataPtr+byte));
JamieB 24:16fd010a71d0 319 }
JamieB 24:16fd010a71d0 320 break;
AndyA 22:0dd9c1b5664a 321 case mode_VIPS:
JamieB 24:16fd010a71d0 322 default: {
AndyA 44:fe7fcf62c75d 323 char *dataPtr = (char *)FlexibleVIPSOut;
AndyA 44:fe7fcf62c75d 324 for (int byte=0; byte<VIPSOutSize; byte++)
AndyA 22:0dd9c1b5664a 325 COM1.putc(*(dataPtr+byte));
JamieB 24:16fd010a71d0 326 }
JamieB 24:16fd010a71d0 327 break;
JamieB 20:ce1d8fbff68f 328 }
JamieB 20:ce1d8fbff68f 329 }
AndyA 17:5ce3fe98e76d 330
AndyA 22:0dd9c1b5664a 331 void createFreeDPacket(position *posPtr)
JamieB 20:ce1d8fbff68f 332 {
JamieB 20:ce1d8fbff68f 333 if (posPtr) {
JamieB 20:ce1d8fbff68f 334 fdPacket.header = 0xD1;
JamieB 20:ce1d8fbff68f 335 fdPacket.id = 0xFF - posPtr->ID;
JamieB 27:498cce52fe5f 336 set24bitValue(fdPacket.yaw, (int)(((posPtr->yaw > 180)? posPtr->yaw - 360 : posPtr->yaw) *32768)); ;
JamieB 25:7002be632308 337 set24bitValue(fdPacket.pitch, (int)(posPtr->pitch *32768));
JamieB 25:7002be632308 338 set24bitValue(fdPacket.roll, (int)(posPtr->roll *32768));
JamieB 25:7002be632308 339 // fdPacket.yaw = (int)(posPtr->yaw *32768);
JamieB 25:7002be632308 340 // fdPacket.pitch = (int)(posPtr->pitch *32768);
JamieB 25:7002be632308 341 // fdPacket.roll = (int)(posPtr->roll *32768);
JamieB 25:7002be632308 342 set24bitValue(fdPacket.x, (int)(posPtr->X *64000));
JamieB 25:7002be632308 343 set24bitValue(fdPacket.y, (int)(posPtr->Y *64000));
JamieB 25:7002be632308 344 set24bitValue(fdPacket.z, (int)(posPtr->Height *64000));
JamieB 25:7002be632308 345 // fdPacket.x = (int)(posPtr->X *64000);
JamieB 25:7002be632308 346 // fdPacket.y = (int)(posPtr->Y)*64000;
JamieB 25:7002be632308 347 // fdPacket.z = (int)(posPtr->Height *64000);
AndyA 22:0dd9c1b5664a 348 if (posPtr->KFStatus < 0x200) {
AndyA 22:0dd9c1b5664a 349 led2 = 0;
AndyA 22:0dd9c1b5664a 350 RedLED = LED_ON;
AndyA 22:0dd9c1b5664a 351 } else {
AndyA 22:0dd9c1b5664a 352 led2 = 1;
AndyA 22:0dd9c1b5664a 353 RedLED = LED_OFF;
AndyA 22:0dd9c1b5664a 354 }
JamieB 20:ce1d8fbff68f 355 } else {
JamieB 20:ce1d8fbff68f 356 fdPacket.header = 0xD1;
JamieB 20:ce1d8fbff68f 357 fdPacket.id = 0xFF - posPtr->ID;
JamieB 25:7002be632308 358 set24bitValue(fdPacket.yaw, 0);
JamieB 25:7002be632308 359 set24bitValue(fdPacket.pitch, 0);
JamieB 25:7002be632308 360 set24bitValue(fdPacket.roll, 0);
JamieB 25:7002be632308 361 set24bitValue(fdPacket.x, 0);
JamieB 25:7002be632308 362 set24bitValue(fdPacket.y, 0);
JamieB 25:7002be632308 363 set24bitValue(fdPacket.z, 0);
AndyA 22:0dd9c1b5664a 364 led2 = 1;
AndyA 22:0dd9c1b5664a 365 RedLED = LED_OFF;
JamieB 20:ce1d8fbff68f 366 }
JamieB 20:ce1d8fbff68f 367 uint32_t temp_focus;
JamieB 20:ce1d8fbff68f 368 uint16_t temp_iris, temp_zoom; //can't directly address bitfield
JamieB 20:ce1d8fbff68f 369 FIZPort->getMostRecent(&temp_focus, &temp_iris, &temp_zoom);
JamieB 25:7002be632308 370 set24bitValue(fdPacket.focus, temp_focus);
JamieB 25:7002be632308 371 set24bitValue(fdPacket.zoom, temp_zoom);
JamieB 20:ce1d8fbff68f 372 fdPacket.checksum = GetFdCRC((void *)&fdPacket);
JamieB 25:7002be632308 373 //pc.puts("\r\n'");
AndyA 22:0dd9c1b5664a 374 }
JamieB 20:ce1d8fbff68f 375
AndyA 48:475b3c5f5707 376 void sendFreeDpacketSerial()
AndyA 22:0dd9c1b5664a 377 {
AndyA 50:d6c56eccf56a 378 if (UserSettings.SerialTxDelayMS > 0)
AndyA 50:d6c56eccf56a 379 OutputDelayTimer.attach_us(&onOutputTxTime,UserSettings.SerialTxDelayMS*1000);
AndyA 50:d6c56eccf56a 380 else
AndyA 50:d6c56eccf56a 381 onOutputTxTime();// COM1.write(&packetOut, sizeof(struct outputFormat_s));
AndyA 50:d6c56eccf56a 382
JamieB 20:ce1d8fbff68f 383 if (logging) {
JamieB 20:ce1d8fbff68f 384 if (!fwrite(&fdPacket, sizeof(struct D1MsgFormat_s), 1, logFile)) { // write failed
JamieB 20:ce1d8fbff68f 385 GreenLED = LED_OFF;
JamieB 20:ce1d8fbff68f 386 logging = false;
JamieB 20:ce1d8fbff68f 387 fclose(logFile);
JamieB 20:ce1d8fbff68f 388 logFile = NULL;
JamieB 20:ce1d8fbff68f 389 }
JamieB 20:ce1d8fbff68f 390 }
AndyA 15:830fc953edd9 391 }
AndyA 15:830fc953edd9 392
AndyA 15:830fc953edd9 393
AndyA 50:d6c56eccf56a 394 void createVIPSPacket(position *posPtr)
AndyA 50:d6c56eccf56a 395 {
AndyA 50:d6c56eccf56a 396
AndyA 44:fe7fcf62c75d 397 int byteCount = 4;
JamieB 79:1910ae03cb2e 398 //uint32_t mask = 0x0446; // Status,orientation,accuracy,FIZ
JamieB 79:1910ae03cb2e 399 uint32_t mask = 0x0546; // Status,orientation,accuracy,FIZ,RAW_IMU
AndyA 44:fe7fcf62c75d 400 if (posPtr && UserSettings.FlexibleVIPSOut) {
AndyA 47:8d577dfb7eb1 401 if (posPtr->UsedBeaconsValid) {
AndyA 44:fe7fcf62c75d 402 mask |= 0x1000;
AndyA 50:d6c56eccf56a 403 }
AndyA 44:fe7fcf62c75d 404 if (posPtr->LLAPosition)
AndyA 44:fe7fcf62c75d 405 mask |= 0x0001;
AndyA 44:fe7fcf62c75d 406 }
AndyA 44:fe7fcf62c75d 407
AndyA 44:fe7fcf62c75d 408 *((uint32_t*)(FlexibleVIPSOut + byteCount)) = mask;
AndyA 44:fe7fcf62c75d 409 byteCount +=4;
AndyA 44:fe7fcf62c75d 410
AndyA 44:fe7fcf62c75d 411 *((uint32_t*)(FlexibleVIPSOut + byteCount)) = movieTime.getTimeMS();
AndyA 44:fe7fcf62c75d 412 byteCount +=4;
AndyA 44:fe7fcf62c75d 413
AndyA 0:97661408d0f9 414 if (posPtr) {
AndyA 44:fe7fcf62c75d 415 *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->X;
AndyA 44:fe7fcf62c75d 416 byteCount +=8;
AndyA 44:fe7fcf62c75d 417 *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->Y;
AndyA 44:fe7fcf62c75d 418 byteCount +=8;
AndyA 44:fe7fcf62c75d 419 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->Height;
AndyA 44:fe7fcf62c75d 420 byteCount +=4;
AndyA 47:8d577dfb7eb1 421 FlexibleVIPSOut[byteCount++] = posPtr->beacons;
AndyA 47:8d577dfb7eb1 422 FlexibleVIPSOut[byteCount++] = posPtr->solutionType;
AndyA 44:fe7fcf62c75d 423 *((uint16_t*)(FlexibleVIPSOut + byteCount)) = posPtr->KFStatus;
AndyA 44:fe7fcf62c75d 424 byteCount +=2;
AndyA 44:fe7fcf62c75d 425 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->roll;
AndyA 44:fe7fcf62c75d 426 byteCount +=4;
AndyA 44:fe7fcf62c75d 427 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->pitch;
AndyA 44:fe7fcf62c75d 428 byteCount +=4;
AndyA 44:fe7fcf62c75d 429 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->yaw;
AndyA 44:fe7fcf62c75d 430 byteCount +=4;
AndyA 6:61274e214f46 431 } else {
AndyA 44:fe7fcf62c75d 432 int blankBytes = 8+8+4+1+1+2+4+4+4;
AndyA 44:fe7fcf62c75d 433 memset(FlexibleVIPSOut + byteCount,0,blankBytes);
AndyA 44:fe7fcf62c75d 434 byteCount += blankBytes;
AndyA 44:fe7fcf62c75d 435 }
AndyA 44:fe7fcf62c75d 436
AndyA 47:8d577dfb7eb1 437 FlexibleVIPSOut[byteCount++] = 0;
AndyA 47:8d577dfb7eb1 438 FlexibleVIPSOut[byteCount++] = 0;
AndyA 47:8d577dfb7eb1 439 FlexibleVIPSOut[byteCount++] = 0;
AndyA 47:8d577dfb7eb1 440 FlexibleVIPSOut[byteCount++] = posPtr?posPtr->ID:0;
AndyA 44:fe7fcf62c75d 441
JamieB 79:1910ae03cb2e 442 if (posPtr) {
JamieB 79:1910ae03cb2e 443 //RAWIMU
JamieB 79:1910ae03cb2e 444 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->x_accel;
JamieB 79:1910ae03cb2e 445 byteCount +=4;
JamieB 79:1910ae03cb2e 446 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->y_accel;
JamieB 79:1910ae03cb2e 447 byteCount +=4;
JamieB 79:1910ae03cb2e 448 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->z_accel;
JamieB 79:1910ae03cb2e 449 byteCount +=4;
JamieB 79:1910ae03cb2e 450 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->x_gyro;
JamieB 79:1910ae03cb2e 451 byteCount +=4;
JamieB 79:1910ae03cb2e 452 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->y_gyro;
JamieB 79:1910ae03cb2e 453 byteCount +=4;
JamieB 79:1910ae03cb2e 454 *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->z_gyro;
JamieB 79:1910ae03cb2e 455 byteCount +=4;
JamieB 79:1910ae03cb2e 456 }
JamieB 79:1910ae03cb2e 457
AndyA 44:fe7fcf62c75d 458 FIZPort->getMostRecent((uint32_t*)(FlexibleVIPSOut + byteCount), (uint16_t*)(FlexibleVIPSOut + byteCount+4), (uint16_t*)(FlexibleVIPSOut + byteCount+6));
AndyA 44:fe7fcf62c75d 459 byteCount+=8;
AndyA 44:fe7fcf62c75d 460
JamieB 64:e2b454ed9940 461 if (mask & 0x1000) { // pos must be valid for this to be set
AndyA 44:fe7fcf62c75d 462 memcpy(FlexibleVIPSOut + byteCount,posPtr->UsedBeacons,12);
AndyA 47:8d577dfb7eb1 463 byteCount+=12;
AndyA 47:8d577dfb7eb1 464 }
AndyA 44:fe7fcf62c75d 465
AndyA 44:fe7fcf62c75d 466 // set length to current size plus checksum.
AndyA 44:fe7fcf62c75d 467 VIPSOutSize = byteCount+2;
AndyA 44:fe7fcf62c75d 468 *((uint16_t*)(FlexibleVIPSOut + 2)) = VIPSOutSize;
AndyA 44:fe7fcf62c75d 469
AndyA 50:d6c56eccf56a 470 VIPSSerial::getCRC(FlexibleVIPSOut, byteCount, (void *)(FlexibleVIPSOut+byteCount));
AndyA 50:d6c56eccf56a 471 }
AndyA 15:830fc953edd9 472
AndyA 48:475b3c5f5707 473
AndyA 48:475b3c5f5707 474 void sendVIPSPacketSerial()
AndyA 48:475b3c5f5707 475 {
AndyA 50:d6c56eccf56a 476 if (UserSettings.SerialTxDelayMS > 0)
AndyA 50:d6c56eccf56a 477 OutputDelayTimer.attach_us(&onOutputTxTime,UserSettings.SerialTxDelayMS*1000);
AndyA 50:d6c56eccf56a 478 else
AndyA 50:d6c56eccf56a 479 onOutputTxTime();// COM1.write(&packetOut, sizeof(struct outputFormat_s));
AndyA 50:d6c56eccf56a 480
AndyA 9:7214e3c3e5f8 481 if (logging) {
AndyA 44:fe7fcf62c75d 482 if (!fwrite(FlexibleVIPSOut, VIPSOutSize, 1, logFile)) { // write failed
AndyA 15:830fc953edd9 483 GreenLED = LED_OFF;
AndyA 9:7214e3c3e5f8 484 logging = false;
AndyA 9:7214e3c3e5f8 485 fclose(logFile);
AndyA 9:7214e3c3e5f8 486 logFile = NULL;
AndyA 0:97661408d0f9 487 }
AndyA 0:97661408d0f9 488 }
AndyA 0:97661408d0f9 489 }
AndyA 0:97661408d0f9 490
JamieB 42:9653486372a6 491 void UDP_Tx_Now()
AndyA 22:0dd9c1b5664a 492 {
AndyA 32:bfb385d35097 493 EthTxNow = true;
JamieB 58:8cce8fcc765f 494 NewData.unlock();
AndyA 22:0dd9c1b5664a 495 }
AndyA 22:0dd9c1b5664a 496
JamieB 57:ba6f516d93b9 497 void applyOffsets(float *value, int offset, bool invert, int max_wrap)
AndyA 55:ee36cab19a58 498 {
JamieB 57:ba6f516d93b9 499 int min_wrap = max_wrap - 360;
JamieB 61:e734e86661b0 500
AndyA 55:ee36cab19a58 501 *value += offset;
AndyA 55:ee36cab19a58 502 if (invert)
AndyA 55:ee36cab19a58 503 *value = 360-*value;
AndyA 55:ee36cab19a58 504
JamieB 57:ba6f516d93b9 505 *value += 360-max_wrap;
JamieB 57:ba6f516d93b9 506
JamieB 57:ba6f516d93b9 507 if (*value<min_wrap)
AndyA 56:0d9684c7df36 508 *value+=((int)(*value/-360)+1)*360;
JamieB 57:ba6f516d93b9 509 if (*value>=max_wrap)
AndyA 56:0d9684c7df36 510 *value-=((int)(*value/360))*360;
JamieB 57:ba6f516d93b9 511
JamieB 57:ba6f516d93b9 512 *value -= 360-max_wrap;
AndyA 55:ee36cab19a58 513 }
AndyA 55:ee36cab19a58 514
JamieB 20:ce1d8fbff68f 515 void sendPosition(position *posPtr)
JamieB 20:ce1d8fbff68f 516 {
JamieB 58:8cce8fcc765f 517 NewData.lock();
AndyA 32:bfb385d35097 518 static bool NoDataWarn = true;
JamieB 25:7002be632308 519 if (!posPtr) {
AndyA 32:bfb385d35097 520 if (NoDataWarn) { // only warn once per dropout
AndyA 32:bfb385d35097 521 pc.puts("No VIPS Data\r\n");
AndyA 32:bfb385d35097 522 NoDataWarn = false;
AndyA 32:bfb385d35097 523 }
AndyA 32:bfb385d35097 524 } else
AndyA 32:bfb385d35097 525 NoDataWarn= true;
JamieB 25:7002be632308 526
AndyA 51:d4a5151af5af 527 if (posPtr) {
JamieB 57:ba6f516d93b9 528 applyOffsets(&posPtr->roll, UserSettings.OffsetRoll, UserSettings.InvertRoll, 180);
JamieB 57:ba6f516d93b9 529 applyOffsets(&posPtr->pitch, UserSettings.OffsetPitch, UserSettings.InvertPitch, 180);
JamieB 57:ba6f516d93b9 530 applyOffsets(&posPtr->yaw, UserSettings.OffsetYaw, UserSettings.InvertYaw, 360);
AndyA 51:d4a5151af5af 531 }
AndyA 51:d4a5151af5af 532
JamieB 25:7002be632308 533
AndyA 32:bfb385d35097 534 // FreeD could be output by two different methods so calculate first if needed rather than at Tx.
JamieB 42:9653486372a6 535 if ((UserSettings.SerialOutMode==mode_FreeD) || UserSettings.FreeDPort)
AndyA 22:0dd9c1b5664a 536 createFreeDPacket(posPtr);
AndyA 22:0dd9c1b5664a 537
JamieB 73:89f799500055 538 if ((UserSettings.SerialOutMode==mode_VIPS) || UserSettings.VipsUDPPort || (UserSettings.DataOutPort && !UserSettings.FreeDPort))
AndyA 48:475b3c5f5707 539 createVIPSPacket(posPtr);
AndyA 48:475b3c5f5707 540
AndyA 48:475b3c5f5707 541
AndyA 32:bfb385d35097 542 // Network is faster so send that first.
JamieB 73:89f799500055 543 if (UserSettings.DataOutPort || UserSettings.VipsUDPPort) {
AndyA 50:d6c56eccf56a 544 if (UserSettings.UDPTxDelayMS > 0) {
AndyA 51:d4a5151af5af 545 UDPOutputDelayTimer.attach_us(&UDP_Tx_Now,UserSettings.UDPTxDelayMS*1000);
AndyA 50:d6c56eccf56a 546 } else
AndyA 51:d4a5151af5af 547 UDP_Tx_Now();
AndyA 22:0dd9c1b5664a 548 }
JamieB 61:e734e86661b0 549 if (TXFrame) {
AndyA 22:0dd9c1b5664a 550
JamieB 61:e734e86661b0 551 switch (UserSettings.SerialOutMode) {
JamieB 61:e734e86661b0 552 case mode_FreeD:
JamieB 61:e734e86661b0 553 sendFreeDpacketSerial();
JamieB 61:e734e86661b0 554 break;
JamieB 61:e734e86661b0 555 case mode_VIPS:
JamieB 61:e734e86661b0 556 default:
JamieB 61:e734e86661b0 557 sendVIPSPacketSerial();
JamieB 61:e734e86661b0 558 break;
JamieB 61:e734e86661b0 559 }
JamieB 61:e734e86661b0 560
JamieB 61:e734e86661b0 561 if (posPtr && (posPtr->KFStatus < 0x200)) {
JamieB 61:e734e86661b0 562 led2 = 0;
JamieB 61:e734e86661b0 563 RedLED = LED_ON;
JamieB 61:e734e86661b0 564 } else {
JamieB 61:e734e86661b0 565 led2 = 1;
JamieB 61:e734e86661b0 566 RedLED = LED_OFF;
JamieB 61:e734e86661b0 567 }
JamieB 61:e734e86661b0 568
JamieB 20:ce1d8fbff68f 569 }
AndyA 68:9e68287f0ab7 570 if (UserSettings.HalfRate) {
AndyA 68:9e68287f0ab7 571 TXFrame = !TXFrame;
AndyA 68:9e68287f0ab7 572 }
JamieB 20:ce1d8fbff68f 573 }
JamieB 20:ce1d8fbff68f 574
JamieB 20:ce1d8fbff68f 575
AndyA 0:97661408d0f9 576 //called once per frame to output the current postition
AndyA 1:dd1f7e162f91 577 void framePositionOutput()
AndyA 1:dd1f7e162f91 578 {
AndyA 14:76083dc18b0d 579 led3 = !led3;
JamieB 72:da2f1df6a50d 580 uint32_t outputTime = TimeSinceLastFrame.read_us() + UserSettings.InterpolationOffset_uS;
AndyA 0:97661408d0f9 581 TimeSinceLastFrame.reset();
AndyA 9:7214e3c3e5f8 582 sendPosition(VIPS.sendPositionForTime(outputTime));
AndyA 16:a8d3a0dbe4bf 583 FIZPort->requestCurrent();
AndyA 0:97661408d0f9 584 }
AndyA 0:97661408d0f9 585
AndyA 9:7214e3c3e5f8 586 int getNextFileNumber()
AndyA 1:dd1f7e162f91 587 {
AndyA 14:76083dc18b0d 588
AndyA 14:76083dc18b0d 589 #ifdef enableUSBStick
AndyA 9:7214e3c3e5f8 590 static unsigned int fileNbr = 0;
AndyA 9:7214e3c3e5f8 591 char fileName[32];
AndyA 9:7214e3c3e5f8 592 FILE *filePtr = NULL;
JamieB 20:ce1d8fbff68f 593 pc.puts("DiskInit\r\n");
JamieB 20:ce1d8fbff68f 594 if (!msc.disk_initialize()) {
JamieB 20:ce1d8fbff68f 595 pc.puts("DiskInit failed/r/n");
JamieB 20:ce1d8fbff68f 596 return fileNbr;
JamieB 20:ce1d8fbff68f 597 }
AndyA 14:76083dc18b0d 598
AndyA 9:7214e3c3e5f8 599 do {
AndyA 9:7214e3c3e5f8 600 if (filePtr != NULL)
AndyA 9:7214e3c3e5f8 601 fclose(filePtr);
JamieB 20:ce1d8fbff68f 602 pc.printf("Looking to see if %d exists\r\n",fileNbr);
AndyA 15:830fc953edd9 603 sprintf(fileName,"/msc/VIPS%04u.bin",fileNbr);
AndyA 9:7214e3c3e5f8 604 filePtr = fopen(fileName,"rb");
AndyA 15:830fc953edd9 605 if (filePtr) fileNbr++;
AndyA 9:7214e3c3e5f8 606 } while (filePtr != NULL);
JamieB 20:ce1d8fbff68f 607 pc.puts("File not found\r\n");
AndyA 15:830fc953edd9 608 return fileNbr;
AndyA 14:76083dc18b0d 609 #else
AndyA 14:76083dc18b0d 610 return 0;
AndyA 15:830fc953edd9 611 #endif
AndyA 0:97661408d0f9 612 }
AndyA 0:97661408d0f9 613
AndyA 9:7214e3c3e5f8 614 FILE *nextBinaryFile(void)
AndyA 1:dd1f7e162f91 615 {
AndyA 14:76083dc18b0d 616 #ifdef enableUSBStick
AndyA 15:830fc953edd9 617 // pc.puts("DiskInit\r\n");
AndyA 15:830fc953edd9 618 // int initResult = msc.disk_initialize();
AndyA 15:830fc953edd9 619 // pc.printf("Init returned %d\r\n",initResult);
AndyA 14:76083dc18b0d 620
AndyA 15:830fc953edd9 621 // if (msc.disk_initialize()==) {
AndyA 15:830fc953edd9 622 // pc.puts("DiskInit failed/r/n");
AndyA 15:830fc953edd9 623 // return NULL;
AndyA 15:830fc953edd9 624 // }
AndyA 9:7214e3c3e5f8 625 char fileName[32];
AndyA 9:7214e3c3e5f8 626 int file = getNextFileNumber();
AndyA 9:7214e3c3e5f8 627 sprintf(fileName,"/msc/VIPS%04u.bin",file);
AndyA 15:830fc953edd9 628 // pc.printf("Opending output file %s\r\n",fileName);
AndyA 9:7214e3c3e5f8 629 return fopen(fileName,"wb");
AndyA 14:76083dc18b0d 630 #else
AndyA 15:830fc953edd9 631 pc.puts("Disk support disabled\r\n");
AndyA 15:830fc953edd9 632 return NULL;
AndyA 14:76083dc18b0d 633 #endif
AndyA 9:7214e3c3e5f8 634 }
AndyA 7:87aea27cc68b 635
AndyA 0:97661408d0f9 636
AndyA 0:97661408d0f9 637 volatile bool NewFramePulse= false;
AndyA 0:97661408d0f9 638 volatile int framesIn = 0;
AndyA 1:dd1f7e162f91 639 void OnPPFInputStartup(void)
AndyA 1:dd1f7e162f91 640 {
AndyA 0:97661408d0f9 641 framesIn++;
AndyA 0:97661408d0f9 642 }
AndyA 0:97661408d0f9 643
AndyA 0:97661408d0f9 644 volatile int SyncInCount = 0;
AndyA 1:dd1f7e162f91 645 void OnSyncInputStartup(void)
AndyA 1:dd1f7e162f91 646 {
AndyA 0:97661408d0f9 647 SyncInCount++;
AndyA 0:97661408d0f9 648 }
AndyA 0:97661408d0f9 649
AndyA 0:97661408d0f9 650
AndyA 1:dd1f7e162f91 651 void OnPPFInput(void)
AndyA 1:dd1f7e162f91 652 {
AndyA 14:76083dc18b0d 653 frameToggle=!frameToggle;
AndyA 9:7214e3c3e5f8 654 movieTime.nextFrame();
AndyA 0:97661408d0f9 655 NewFramePulse = true;
AndyA 0:97661408d0f9 656 }
AndyA 0:97661408d0f9 657
AndyA 14:76083dc18b0d 658 void OnResetTimeout()
AndyA 14:76083dc18b0d 659 {
JamieB 76:c7c795b22f63 660 __disable_irq();
JamieB 76:c7c795b22f63 661 led1=1;
JamieB 76:c7c795b22f63 662 led2=1;
JamieB 76:c7c795b22f63 663 led3=1;
JamieB 76:c7c795b22f63 664 frameToggle=1;
JamieB 76:c7c795b22f63 665 RedLED=1; // red
JamieB 76:c7c795b22f63 666 GreenLED=1;
JamieB 76:c7c795b22f63 667 BlueLED=1;
JamieB 76:c7c795b22f63 668 wait(1);
AndyA 12:06050debf014 669 NVIC_SystemReset();
AndyA 14:76083dc18b0d 670 }
AndyA 12:06050debf014 671
JamieB 78:8e1170e18a2d 672 void readLensFile(char* filename)
JamieB 78:8e1170e18a2d 673 {
JamieB 78:8e1170e18a2d 674 FILE *LensFile = fopen(filename,"r");
JamieB 78:8e1170e18a2d 675 if (LensFile) {
JamieB 78:8e1170e18a2d 676 char chunk[64];
JamieB 78:8e1170e18a2d 677 size_t len = sizeof(chunk);
JamieB 78:8e1170e18a2d 678 char *line = new char[len];
JamieB 78:8e1170e18a2d 679 pc.printf("Opened File %s\r\n", filename);
JamieB 85:0cc5931bb9ef 680 <<<<<<< working copy
JamieB 79:1910ae03cb2e 681 vector<int> *encoder;
JamieB 79:1910ae03cb2e 682 vector<float> *absolute;
JamieB 85:0cc5931bb9ef 683 =======
JamieB 80:0b7f1b85b626 684 vector<unsigned int> *encoder;
JamieB 80:0b7f1b85b626 685 vector<unsigned int> *absolute;
JamieB 85:0cc5931bb9ef 686 >>>>>>> merge rev
JamieB 80:0b7f1b85b626 687 int focus_datapoints;
JamieB 80:0b7f1b85b626 688 int iris_datapoints;
JamieB 80:0b7f1b85b626 689 int zoom_datapoints;
JamieB 78:8e1170e18a2d 690
JamieB 78:8e1170e18a2d 691 while(fgets(chunk, sizeof(chunk), LensFile) != NULL) {
JamieB 78:8e1170e18a2d 692 size_t len_used = strlen(line);
JamieB 78:8e1170e18a2d 693 size_t chunk_used = strlen(chunk);
JamieB 78:8e1170e18a2d 694
JamieB 78:8e1170e18a2d 695 if(len - len_used < chunk_used) {
JamieB 78:8e1170e18a2d 696 pc.printf("Line Buffer Exceeded - Failed to read file");
JamieB 78:8e1170e18a2d 697 free(line);
JamieB 78:8e1170e18a2d 698 return;
JamieB 80:0b7f1b85b626 699 }
JamieB 78:8e1170e18a2d 700
JamieB 78:8e1170e18a2d 701 strncpy(line + len_used, chunk, len - len_used);
JamieB 78:8e1170e18a2d 702 len_used += chunk_used;
JamieB 78:8e1170e18a2d 703 if(line[len_used - 1] == '\n') {
JamieB 85:0cc5931bb9ef 704 <<<<<<< working copy
JamieB 79:1910ae03cb2e 705 pc.printf("Retrieved line of length %u: ", len_used);
JamieB 79:1910ae03cb2e 706 pc.printf("%s\n", line);
JamieB 79:1910ae03cb2e 707 if (sscanf(line,"FOCUS:%d", &focus_datapoints) == 1) {
JamieB 79:1910ae03cb2e 708 encoder = &UserSettings.focus_encoder_map;
JamieB 79:1910ae03cb2e 709 absolute = &UserSettings.focus_absolute_map;
JamieB 79:1910ae03cb2e 710 printf("\tSet Focus\n");
JamieB 79:1910ae03cb2e 711 } else if (sscanf(line,"IRIS:%d", &iris_datapoints) == 1) {
JamieB 79:1910ae03cb2e 712 encoder = &UserSettings.iris_encoder_map;
JamieB 79:1910ae03cb2e 713 absolute = &UserSettings.iris_absolute_map;
JamieB 79:1910ae03cb2e 714 printf("\tSet Iris\n");
JamieB 79:1910ae03cb2e 715 } else if (sscanf(line,"ZOOM:%d", &zoom_datapoints) == 1) {
JamieB 79:1910ae03cb2e 716 encoder = &UserSettings.zoom_encoder_map;
JamieB 79:1910ae03cb2e 717 absolute = &UserSettings.zoom_absolute_map;
JamieB 79:1910ae03cb2e 718 printf("\tSet Zoom\n");
JamieB 79:1910ae03cb2e 719 } else if (strchr(line,',') != NULL) {
JamieB 79:1910ae03cb2e 720 pc.printf("\tProbably some data in this line\n");
JamieB 79:1910ae03cb2e 721 int encoder_val;
JamieB 79:1910ae03cb2e 722 float absolute_val;
JamieB 79:1910ae03cb2e 723 char *pt = strtok (line,",");
JamieB 79:1910ae03cb2e 724 if (pt) {
JamieB 79:1910ae03cb2e 725 encoder_val = atoi(pt);
JamieB 79:1910ae03cb2e 726 printf("\tENCODER: %d\n", encoder_val);
JamieB 79:1910ae03cb2e 727 }
JamieB 79:1910ae03cb2e 728 pt = strtok(line, ",");
JamieB 79:1910ae03cb2e 729 if (pt) {
JamieB 79:1910ae03cb2e 730 absolute_val = (float)atof(pt);
JamieB 79:1910ae03cb2e 731 printf("\tABSOLUTE: %f\n", absolute_val);
JamieB 79:1910ae03cb2e 732 }
JamieB 79:1910ae03cb2e 733 if (encoder_val != NULL && absolute_val != NULL) {
JamieB 79:1910ae03cb2e 734 encoder->push_back(encoder_val);
JamieB 79:1910ae03cb2e 735 absolute->push_back(absolute_val);
JamieB 79:1910ae03cb2e 736 printf("\tADDED Datapoint\n");
JamieB 79:1910ae03cb2e 737 }
JamieB 79:1910ae03cb2e 738 } else {
JamieB 79:1910ae03cb2e 739 printf("\tNot Valid matching line\n");
JamieB 79:1910ae03cb2e 740 }
JamieB 85:0cc5931bb9ef 741 =======
JamieB 80:0b7f1b85b626 742 // pc.printf("Retrieved line of length %u: ", len_used);
JamieB 80:0b7f1b85b626 743 // pc.printf("%s", line);
JamieB 80:0b7f1b85b626 744 // Thread::wait(10);
JamieB 80:0b7f1b85b626 745 if (sscanf(line,"FOCUS:%d", &focus_datapoints) == 1) {
JamieB 80:0b7f1b85b626 746 encoder = &UserSettings.focus_encoder_map;
JamieB 80:0b7f1b85b626 747 absolute = &UserSettings.focus_absolute_map;
JamieB 81:aee60dcce61b 748 UserSettings.absolute_focus = true;
JamieB 80:0b7f1b85b626 749 // pc.printf(" - Set Focus\r\n");
JamieB 80:0b7f1b85b626 750 // Thread::wait(10);
JamieB 80:0b7f1b85b626 751 } else if (sscanf(line,"IRIS:%d", &iris_datapoints) == 1) {
JamieB 80:0b7f1b85b626 752 encoder = &UserSettings.iris_encoder_map;
JamieB 80:0b7f1b85b626 753 absolute = &UserSettings.iris_absolute_map;
JamieB 81:aee60dcce61b 754 UserSettings.absolute_iris = true;
JamieB 80:0b7f1b85b626 755 // pc.printf(" - Set Iris\r\n");
JamieB 80:0b7f1b85b626 756 // Thread::wait(10);
JamieB 80:0b7f1b85b626 757 } else if (sscanf(line,"ZOOM:%d", &zoom_datapoints) == 1) {
JamieB 80:0b7f1b85b626 758 encoder = &UserSettings.zoom_encoder_map;
JamieB 80:0b7f1b85b626 759 absolute = &UserSettings.zoom_absolute_map;
JamieB 81:aee60dcce61b 760 UserSettings.absolute_zoom = true;
JamieB 80:0b7f1b85b626 761 // pc.printf(" - Set Zoom\r\n");
JamieB 80:0b7f1b85b626 762 // Thread::wait(10);
JamieB 80:0b7f1b85b626 763 } else if (strchr(line,',') != NULL) {
JamieB 80:0b7f1b85b626 764 // pc.printf("\tProbably some data in this line\n");
JamieB 80:0b7f1b85b626 765 unsigned int encoder_val;
JamieB 80:0b7f1b85b626 766 unsigned int absolute_val;
JamieB 80:0b7f1b85b626 767 if (sscanf(line, "%d,%d", &encoder_val, &absolute_val) == 2) {
JamieB 80:0b7f1b85b626 768 encoder->push_back(encoder_val);
JamieB 80:0b7f1b85b626 769 absolute->push_back(absolute_val);
JamieB 80:0b7f1b85b626 770 // pc.printf(" - ADDED Datapoint\r\n");
JamieB 80:0b7f1b85b626 771 // Thread::wait(10);
JamieB 80:0b7f1b85b626 772 }
JamieB 80:0b7f1b85b626 773 } // else { pc.printf(" - Not Valid matching line\r\n"); }
JamieB 85:0cc5931bb9ef 774 >>>>>>> merge rev
JamieB 80:0b7f1b85b626 775 line[0] = '\0';
JamieB 78:8e1170e18a2d 776 }
JamieB 78:8e1170e18a2d 777 }
JamieB 80:0b7f1b85b626 778 pc.printf("FOCUS: ");
JamieB 80:0b7f1b85b626 779 for(int i = 0; i < UserSettings.focus_encoder_map.size(); i++) {
JamieB 80:0b7f1b85b626 780 pc.printf("(%d, ", UserSettings.focus_encoder_map[i]);
JamieB 80:0b7f1b85b626 781 pc.printf("%d)", UserSettings.focus_absolute_map[i]);
JamieB 80:0b7f1b85b626 782 }
JamieB 80:0b7f1b85b626 783 pc.printf("\r\n");
JamieB 80:0b7f1b85b626 784 pc.printf("IRIS: ");
JamieB 80:0b7f1b85b626 785 for(int i = 0; i < UserSettings.iris_encoder_map.size(); i++) {
JamieB 80:0b7f1b85b626 786 pc.printf("(%d, ", UserSettings.iris_encoder_map[i]);
JamieB 80:0b7f1b85b626 787 pc.printf("%d)", UserSettings.iris_absolute_map[i]);
JamieB 80:0b7f1b85b626 788 }
JamieB 80:0b7f1b85b626 789 pc.printf("\r\n");
JamieB 80:0b7f1b85b626 790 pc.printf("ZOOM: ");
JamieB 80:0b7f1b85b626 791 for(int i = 0; i < UserSettings.zoom_encoder_map.size(); i++) {
JamieB 80:0b7f1b85b626 792 pc.printf("(%d, ", UserSettings.zoom_encoder_map[i]);
JamieB 80:0b7f1b85b626 793 pc.printf("%d)", UserSettings.zoom_absolute_map[i]);
JamieB 80:0b7f1b85b626 794 }
JamieB 80:0b7f1b85b626 795 pc.printf("\r\n");
JamieB 78:8e1170e18a2d 796 fclose(LensFile);
JamieB 78:8e1170e18a2d 797 free(line);
JamieB 80:0b7f1b85b626 798 } else {
JamieB 78:8e1170e18a2d 799 pc.printf("Unable to open File %s\r\n", filename);
JamieB 78:8e1170e18a2d 800 }
JamieB 78:8e1170e18a2d 801 }
AndyA 0:97661408d0f9 802
JamieB 20:ce1d8fbff68f 803 void readSettingsFile()
JamieB 20:ce1d8fbff68f 804 {
AndyA 34:c864a0c67dbf 805
AndyA 34:c864a0c67dbf 806 UserSettings.FIZmode = formatPreston;
AndyA 34:c864a0c67dbf 807 UserSettings.SerialOutMode = mode_VIPS;
JamieB 42:9653486372a6 808 UserSettings.FreeDPort = 0;
JamieB 73:89f799500055 809 UserSettings.DataOutPort = 0;
JamieB 42:9653486372a6 810 UserSettings.VipsUDPPort = 0;
AndyA 68:9e68287f0ab7 811 UserSettings.SettingsPort = 0;
AndyA 34:c864a0c67dbf 812 UserSettings.IPAddress[0] = 0;
AndyA 34:c864a0c67dbf 813 UserSettings.Gateway[0] = 0;
AndyA 34:c864a0c67dbf 814 UserSettings.Subnet[0] = 0;
AndyA 34:c864a0c67dbf 815 UserSettings.FilterOrder = 0;
AndyA 34:c864a0c67dbf 816 UserSettings.FilterFreq = 10;
AndyA 34:c864a0c67dbf 817 UserSettings.FilterRate = 100;
AndyA 34:c864a0c67dbf 818 UserSettings.FilterXY = false;
AndyA 34:c864a0c67dbf 819 UserSettings.FilterZ = false;
AndyA 34:c864a0c67dbf 820 UserSettings.FilterRoll = false;
AndyA 34:c864a0c67dbf 821 UserSettings.FilterPitch = false;
AndyA 34:c864a0c67dbf 822 UserSettings.FilterYaw = false;
JamieB 46:432d63e8fe64 823 UserSettings.AutoHyperSmooth = true;
AndyA 44:fe7fcf62c75d 824 UserSettings.FlexibleVIPSOut = false;
AndyA 50:d6c56eccf56a 825 UserSettings.SerialTxDelayMS = 0;
AndyA 50:d6c56eccf56a 826 UserSettings.UDPTxDelayMS = 0;
AndyA 50:d6c56eccf56a 827 UserSettings.SerialTxDelayFrame = 0;
AndyA 50:d6c56eccf56a 828 UserSettings.UDPTxDelayFrame = 0;
JamieB 72:da2f1df6a50d 829 UserSettings.InterpolationOffsetFrame = 0;
JamieB 72:da2f1df6a50d 830 UserSettings.InterpolationOffset_uS = 0;
AndyA 51:d4a5151af5af 831 UserSettings.InvertRoll = false;
AndyA 51:d4a5151af5af 832 UserSettings.InvertPitch = false;
AndyA 51:d4a5151af5af 833 UserSettings.InvertYaw = false;
AndyA 55:ee36cab19a58 834 UserSettings.OffsetRoll =0;
AndyA 55:ee36cab19a58 835 UserSettings.OffsetPitch =0;
AndyA 55:ee36cab19a58 836 UserSettings.OffsetYaw =0;
AndyA 52:adbbaadca0a9 837 UserSettings.bypassBaud = 0;
JamieB 60:899862db7f2a 838 UserSettings.ForcePPF = 0;
JamieB 61:e734e86661b0 839 UserSettings.HalfRate = 0;
JamieB 71:7305a35cee58 840 UserSettings.focus_scale= 1;
JamieB 71:7305a35cee58 841 UserSettings.focus_offset = 0;
JamieB 71:7305a35cee58 842 UserSettings.iris_scale = 1;
JamieB 71:7305a35cee58 843 UserSettings.iris_offset = 0;
JamieB 71:7305a35cee58 844 UserSettings.zoom_scale = 1;
JamieB 71:7305a35cee58 845 UserSettings.zoom_offset= 0;
JamieB 80:0b7f1b85b626 846 UserSettings.low_zoom_precision = false; //TODO: Get zoom range from lens file, if over 327mm, set this flag (and set top bit in zoom output)
JamieB 81:aee60dcce61b 847 UserSettings.absolute_focus = false;
JamieB 81:aee60dcce61b 848 UserSettings.absolute_iris = false;
JamieB 81:aee60dcce61b 849 UserSettings.absolute_zoom = false;
AndyA 51:d4a5151af5af 850
AndyA 68:9e68287f0ab7 851 // LocalFileSystem localFS("local");
JamieB 20:ce1d8fbff68f 852 FILE *LSFile= fopen("/local/settings.txt","r");
JamieB 78:8e1170e18a2d 853 char lineBuffer[128];
JamieB 20:ce1d8fbff68f 854 int valueIn;
AndyA 34:c864a0c67dbf 855 float floatIn;
JamieB 78:8e1170e18a2d 856 char lensfile[128] = "/local/";//7 characters for filepath, minus 12 chars of settings string, hence 128 shouldnt cause buffer overflow
JamieB 78:8e1170e18a2d 857 /*MAXIMUM FILENAME LENGTH IS 8 Characters*/
JamieB 78:8e1170e18a2d 858
JamieB 20:ce1d8fbff68f 859 if (LSFile) {
AndyA 52:adbbaadca0a9 860 while (!feof(LSFile) && !UserSettings.bypassBaud) {
JamieB 78:8e1170e18a2d 861 if (fgets(lineBuffer, 128, LSFile)) {
AndyA 52:adbbaadca0a9 862 if (sscanf(lineBuffer,"RadioConfigPassthrough=%d",&valueIn) == 1) {
AndyA 52:adbbaadca0a9 863 pc.printf("Switching to RADIO/USB bypass mode at %d baud\r\n",valueIn);
AndyA 52:adbbaadca0a9 864 UserSettings.bypassBaud = valueIn;
AndyA 52:adbbaadca0a9 865 }
AndyA 52:adbbaadca0a9 866
JamieB 20:ce1d8fbff68f 867 if (sscanf(lineBuffer,"Output_Format=%d",&valueIn) == 1) {
JamieB 20:ce1d8fbff68f 868 pc.printf("Got Output_Format value from file of %d\r\n",valueIn);
AndyA 22:0dd9c1b5664a 869 UserSettings.SerialOutMode = valueIn;
JamieB 20:ce1d8fbff68f 870 }
JamieB 20:ce1d8fbff68f 871 if (sscanf(lineBuffer,"FIZ_Format=%d",&valueIn) == 1) {
JamieB 20:ce1d8fbff68f 872 pc.printf("Got FIZ_Format value from file of %d\r\n",valueIn);
AndyA 22:0dd9c1b5664a 873 UserSettings.FIZmode = valueIn;
AndyA 22:0dd9c1b5664a 874 }
AndyA 22:0dd9c1b5664a 875 if (sscanf(lineBuffer,"FreeD_Port=%d",&valueIn) == 1) {
AndyA 22:0dd9c1b5664a 876 pc.printf("Got FreeD_Port value from file of %d\r\n",valueIn);
JamieB 42:9653486372a6 877 UserSettings.FreeDPort = valueIn;
JamieB 73:89f799500055 878 UserSettings.DataOutPort = valueIn;
JamieB 73:89f799500055 879 }
JamieB 73:89f799500055 880 if (sscanf(lineBuffer,"FreeD_Mode=%d",&valueIn) == 1) {
JamieB 73:89f799500055 881 pc.printf("Got FreeD_Mode value from file of %d\r\n",valueIn);
JamieB 73:89f799500055 882 UserSettings.FreeDPort = valueIn;
JamieB 73:89f799500055 883 }
JamieB 73:89f799500055 884 if (sscanf(lineBuffer,"DataOut_Port=%d",&valueIn) == 1) {
JamieB 73:89f799500055 885 pc.printf("Got DataOut_Port value from file of %d\r\n",valueIn);
JamieB 73:89f799500055 886 UserSettings.DataOutPort = valueIn;
JamieB 42:9653486372a6 887 }
JamieB 42:9653486372a6 888 if (sscanf(lineBuffer,"VIPS_UDP_Port=%d",&valueIn) == 1) {
JamieB 42:9653486372a6 889 pc.printf("Got VIPS_Port value from file of %d\r\n",valueIn);
JamieB 42:9653486372a6 890 UserSettings.VipsUDPPort = valueIn;
JamieB 20:ce1d8fbff68f 891 }
AndyA 68:9e68287f0ab7 892 if (sscanf(lineBuffer,"Settings_Port=%d",&valueIn) == 1) {
AndyA 68:9e68287f0ab7 893 pc.printf("Got Settings update port value from file of %d\r\n",valueIn);
AndyA 68:9e68287f0ab7 894 UserSettings.SettingsPort = valueIn;
AndyA 68:9e68287f0ab7 895 }
AndyA 23:5c237f04327d 896 if (sscanf(lineBuffer,"IP_addr=%s",UserSettings.IPAddress) == 1) {
AndyA 23:5c237f04327d 897 pc.printf("Got IP_addr value from file of %s\r\n",UserSettings.IPAddress);
AndyA 23:5c237f04327d 898 }
AndyA 23:5c237f04327d 899 if (sscanf(lineBuffer,"Subnet=%s",UserSettings.Subnet) == 1) {
JamieB 25:7002be632308 900 pc.printf("Got Subnet mask value from file of %s\r\n",UserSettings.Subnet);
AndyA 23:5c237f04327d 901 }
JamieB 25:7002be632308 902 if (sscanf(lineBuffer,"Gateway=%s",UserSettings.Gateway) == 1) {
AndyA 23:5c237f04327d 903 pc.printf("Got gateway value from file of %s\r\n",UserSettings.Gateway);
AndyA 23:5c237f04327d 904 }
AndyA 34:c864a0c67dbf 905 if (sscanf(lineBuffer,"FilterOrder=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 906 pc.printf("Got FilterOrder value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 907 UserSettings.FilterOrder = valueIn;
AndyA 34:c864a0c67dbf 908 }
AndyA 34:c864a0c67dbf 909 if (sscanf(lineBuffer,"FilterFreq=%f",&floatIn) == 1) {
AndyA 34:c864a0c67dbf 910 pc.printf("Got FilterFreq value from file of %.2f\r\n",floatIn);
AndyA 34:c864a0c67dbf 911 UserSettings.FilterFreq = floatIn;
AndyA 34:c864a0c67dbf 912 }
AndyA 34:c864a0c67dbf 913 if (sscanf(lineBuffer,"FilterRate=%f",&floatIn) == 1) {
AndyA 34:c864a0c67dbf 914 pc.printf("Got FilterRate value from file of %.2f\r\n",floatIn);
AndyA 34:c864a0c67dbf 915 UserSettings.FilterRate = floatIn;
AndyA 34:c864a0c67dbf 916 }
AndyA 34:c864a0c67dbf 917 if (sscanf(lineBuffer,"FilterXY=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 918 pc.printf("Got FilterXY value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 919 UserSettings.FilterXY = (valueIn==1);
AndyA 34:c864a0c67dbf 920 }
AndyA 34:c864a0c67dbf 921 if (sscanf(lineBuffer,"FilterZ=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 922 pc.printf("Got FilterZ value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 923 UserSettings.FilterZ = (valueIn==1);
AndyA 34:c864a0c67dbf 924 }
AndyA 34:c864a0c67dbf 925 if (sscanf(lineBuffer,"FilterRoll=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 926 pc.printf("Got FilterRoll value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 927 UserSettings.FilterRoll = (valueIn==1);
AndyA 34:c864a0c67dbf 928 }
AndyA 34:c864a0c67dbf 929 if (sscanf(lineBuffer,"FilterPitch=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 930 pc.printf("Got FilterPitch value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 931 UserSettings.FilterPitch = (valueIn==1);
AndyA 34:c864a0c67dbf 932 }
AndyA 34:c864a0c67dbf 933 if (sscanf(lineBuffer,"FilterYaw=%d",&valueIn) == 1) {
AndyA 34:c864a0c67dbf 934 pc.printf("Got FilterYaw value from file of %d\r\n",valueIn);
AndyA 34:c864a0c67dbf 935 UserSettings.FilterYaw = (valueIn==1);
AndyA 34:c864a0c67dbf 936 }
JamieB 46:432d63e8fe64 937 if (sscanf(lineBuffer,"AutoHyperSmooth=%d",&valueIn) == 1) {
JamieB 46:432d63e8fe64 938 pc.printf("Got AutoHyperSmooth value from file of %d\r\n",valueIn);
AndyA 56:0d9684c7df36 939 UserSettings.AutoHyperSmooth = (valueIn==1);
JamieB 46:432d63e8fe64 940 }
AndyA 44:fe7fcf62c75d 941 if (sscanf(lineBuffer,"ExtendedOutput=%d",&valueIn) == 1) {
AndyA 44:fe7fcf62c75d 942 pc.printf("Got ExtendedOutput value from file of %d\r\n",valueIn);
JamieB 61:e734e86661b0 943 UserSettings.FlexibleVIPSOut = (valueIn==1);
AndyA 44:fe7fcf62c75d 944 }
AndyA 50:d6c56eccf56a 945 if (sscanf(lineBuffer,"DelaySerial=%f",&floatIn) == 1) {
AndyA 50:d6c56eccf56a 946 pc.printf("Got serial delay value from file of %.2f ms\r\n",floatIn);
AndyA 50:d6c56eccf56a 947 UserSettings.SerialTxDelayMS = floatIn;
AndyA 50:d6c56eccf56a 948 }
AndyA 50:d6c56eccf56a 949 if (sscanf(lineBuffer,"DelaySerialFrame=%f",&floatIn) == 1) {
AndyA 50:d6c56eccf56a 950 pc.printf("Got serial delay value from file of %.2f frames\r\n",floatIn);
AndyA 50:d6c56eccf56a 951 UserSettings.SerialTxDelayFrame = floatIn;
AndyA 50:d6c56eccf56a 952 }
AndyA 50:d6c56eccf56a 953 if (sscanf(lineBuffer,"DelayUDP=%f",&floatIn) == 1) {
AndyA 50:d6c56eccf56a 954 pc.printf("Got UDP delay value from file of %.2f ms\r\n",floatIn);
AndyA 50:d6c56eccf56a 955 UserSettings.UDPTxDelayMS = floatIn;
AndyA 50:d6c56eccf56a 956 }
AndyA 50:d6c56eccf56a 957 if (sscanf(lineBuffer,"DelayUDPFrame=%f",&floatIn) == 1) {
AndyA 50:d6c56eccf56a 958 pc.printf("Got UDP delay value from file of %.2f frames\r\n",floatIn);
AndyA 50:d6c56eccf56a 959 UserSettings.UDPTxDelayFrame = floatIn;
AndyA 50:d6c56eccf56a 960 }
JamieB 72:da2f1df6a50d 961 if (sscanf(lineBuffer,"ShiftInterpolationByFrame=%f",&floatIn) == 1) {
JamieB 72:da2f1df6a50d 962 pc.printf("Got Interpolation Offset %.2f frames\r\n",floatIn);
JamieB 72:da2f1df6a50d 963 UserSettings.InterpolationOffsetFrame = floatIn;
JamieB 72:da2f1df6a50d 964 }
JamieB 72:da2f1df6a50d 965 if (sscanf(lineBuffer,"ShiftInterpolationByTimeMs=%f",&floatIn) == 1) {
JamieB 72:da2f1df6a50d 966 pc.printf("Got Interpolation offset of %.2f ms\r\n",floatIn);
JamieB 72:da2f1df6a50d 967 UserSettings.InterpolationOffset_uS = floatIn * 1000;
JamieB 72:da2f1df6a50d 968 }
AndyA 51:d4a5151af5af 969 if (sscanf(lineBuffer,"InvertRoll=%d",&valueIn) == 1) {
AndyA 51:d4a5151af5af 970 pc.printf("Got InvertRoll value from file of %d\r\n",valueIn);
AndyA 51:d4a5151af5af 971 UserSettings.InvertRoll = (valueIn==1);
AndyA 51:d4a5151af5af 972 }
AndyA 51:d4a5151af5af 973 if (sscanf(lineBuffer,"InvertPitch=%d",&valueIn) == 1) {
AndyA 51:d4a5151af5af 974 pc.printf("Got InvertPitch value from file of %d\r\n",valueIn);
AndyA 51:d4a5151af5af 975 UserSettings.InvertPitch = (valueIn==1);
AndyA 51:d4a5151af5af 976 }
AndyA 51:d4a5151af5af 977 if (sscanf(lineBuffer,"InvertYaw=%d",&valueIn) == 1) {
AndyA 51:d4a5151af5af 978 pc.printf("Got InvertYaw value from file of %d\r\n",valueIn);
AndyA 51:d4a5151af5af 979 UserSettings.InvertYaw = (valueIn==1);
AndyA 51:d4a5151af5af 980 }
AndyA 55:ee36cab19a58 981 if (sscanf(lineBuffer,"OffsetRoll=%d",&valueIn) == 1) {
AndyA 55:ee36cab19a58 982 pc.printf("Got OffsetRoll value from file of %d\r\n",valueIn);
AndyA 56:0d9684c7df36 983 UserSettings.OffsetRoll = valueIn;
AndyA 55:ee36cab19a58 984 }
AndyA 55:ee36cab19a58 985 if (sscanf(lineBuffer,"OffsetPitch=%d",&valueIn) == 1) {
AndyA 55:ee36cab19a58 986 pc.printf("Got OffsetPitch value from file of %d\r\n",valueIn);
AndyA 56:0d9684c7df36 987 UserSettings.OffsetPitch = valueIn;
AndyA 55:ee36cab19a58 988 }
AndyA 55:ee36cab19a58 989 if (sscanf(lineBuffer,"OffsetYaw=%d",&valueIn) == 1) {
AndyA 55:ee36cab19a58 990 pc.printf("Got OffsetYaw value from file of %d\r\n",valueIn);
AndyA 56:0d9684c7df36 991 UserSettings.OffsetYaw = valueIn;
AndyA 55:ee36cab19a58 992 }
JamieB 60:899862db7f2a 993 if (sscanf(lineBuffer,"ForcePPF=%d",&valueIn) == 1) {
JamieB 60:899862db7f2a 994 pc.printf("Using Forced PPF\r\n",valueIn);
JamieB 60:899862db7f2a 995 UserSettings.ForcePPF = valueIn;
JamieB 60:899862db7f2a 996 }
JamieB 61:e734e86661b0 997 if (sscanf(lineBuffer,"HalfSerialRate=%d",&valueIn) == 1) {
JamieB 61:e734e86661b0 998 pc.printf("Halving Serial rate output \r\n",valueIn);
JamieB 61:e734e86661b0 999 UserSettings.HalfRate = valueIn;
JamieB 61:e734e86661b0 1000 }
JamieB 71:7305a35cee58 1001 if (sscanf(lineBuffer,"FocusScale=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1002 pc.printf("Got Focus scale of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1003 UserSettings.focus_scale = floatIn;
JamieB 71:7305a35cee58 1004 }
JamieB 71:7305a35cee58 1005 if (sscanf(lineBuffer,"FocusOffset=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1006 pc.printf("Got Focus offset of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1007 UserSettings.focus_offset = floatIn;
JamieB 71:7305a35cee58 1008 }
JamieB 71:7305a35cee58 1009 if (sscanf(lineBuffer,"IrisScale=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1010 pc.printf("Got Iris scale of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1011 UserSettings.iris_scale = floatIn;
JamieB 71:7305a35cee58 1012 }
JamieB 71:7305a35cee58 1013 if (sscanf(lineBuffer,"IrisOffset=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1014 pc.printf("Got Iris offset of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1015 UserSettings.iris_offset = floatIn;
JamieB 71:7305a35cee58 1016 }
JamieB 71:7305a35cee58 1017 if (sscanf(lineBuffer,"ZoomScale=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1018 pc.printf("Got Zoom Scale of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1019 UserSettings.zoom_scale = floatIn;
JamieB 71:7305a35cee58 1020 }
JamieB 71:7305a35cee58 1021 if (sscanf(lineBuffer,"ZoomOffset=%f",&floatIn) == 1) {
JamieB 71:7305a35cee58 1022 pc.printf("Got Zoom offset of %f \r\n",floatIn);
JamieB 71:7305a35cee58 1023 UserSettings.zoom_offset = floatIn;
JamieB 71:7305a35cee58 1024 }
JamieB 78:8e1170e18a2d 1025 if (sscanf(lineBuffer,"LensProfile=%s", &lensfile[7]) == 1) {
JamieB 78:8e1170e18a2d 1026 pc.printf("Found filename of %s \r\n", lensfile);
JamieB 78:8e1170e18a2d 1027 }
AndyA 55:ee36cab19a58 1028
JamieB 20:ce1d8fbff68f 1029 }
JamieB 20:ce1d8fbff68f 1030 }
JamieB 20:ce1d8fbff68f 1031 fclose(LSFile);
JamieB 78:8e1170e18a2d 1032 readLensFile(lensfile);
JamieB 20:ce1d8fbff68f 1033 } else {
JamieB 20:ce1d8fbff68f 1034 pc.printf("No Settings File Found \r\n");
JamieB 20:ce1d8fbff68f 1035 }
JamieB 20:ce1d8fbff68f 1036 }
JamieB 20:ce1d8fbff68f 1037
JamieB 78:8e1170e18a2d 1038
AndyA 68:9e68287f0ab7 1039 void settingsNetUpdate(void const* netInterface)
AndyA 68:9e68287f0ab7 1040 {
JamieB 69:47f800793d00 1041 //FILE *LSFile;
JamieB 69:47f800793d00 1042 const int rxBufferSize = MaxBuffSize; // making this 1024 causes a crash. Probably out of memory somewhere.
JamieB 69:47f800793d00 1043 //char settingsOutBuffer[rxBufferSize];
AndyA 68:9e68287f0ab7 1044 char settingsInBuffer[rxBufferSize];
JamieB 69:47f800793d00 1045 int bytesToSend = 0;
JamieB 69:47f800793d00 1046 unsigned char *bufferToSend;
JamieB 69:47f800793d00 1047 bool validated_connection = false;
AndyA 68:9e68287f0ab7 1048
AndyA 68:9e68287f0ab7 1049 Thread::wait(1000);
JamieB 69:47f800793d00 1050 pc.puts("Starting TCP network listen server\r\n");
AndyA 68:9e68287f0ab7 1051 Thread::wait(50);
AndyA 68:9e68287f0ab7 1052
AndyA 68:9e68287f0ab7 1053
JamieB 69:47f800793d00 1054 // int fileSize = 0;
AndyA 68:9e68287f0ab7 1055 TCPSocketServer server;
AndyA 68:9e68287f0ab7 1056 server.bind(UserSettings.SettingsPort);
JamieB 76:c7c795b22f63 1057 server.set_blocking(false, 1000);
AndyA 68:9e68287f0ab7 1058 server.listen();
AndyA 68:9e68287f0ab7 1059
JamieB 76:c7c795b22f63 1060 TCPSocketConnection connection;
JamieB 76:c7c795b22f63 1061 pc.puts("TCP thread waiting for connection\r\n");
JamieB 76:c7c795b22f63 1062
JamieB 76:c7c795b22f63 1063 while (!StopTCPListening) {
AndyA 68:9e68287f0ab7 1064 server.accept(connection);
JamieB 76:c7c795b22f63 1065 if (connection.is_connected()) {
JamieB 76:c7c795b22f63 1066 pc.puts("TCP Connected\r\n");
JamieB 76:c7c795b22f63 1067 connection.set_blocking(false,50);
JamieB 69:47f800793d00 1068
JamieB 76:c7c795b22f63 1069 int bytesIn = 0;
JamieB 76:c7c795b22f63 1070 do {
JamieB 76:c7c795b22f63 1071 bytesIn = connection.receive(settingsInBuffer,rxBufferSize);
JamieB 76:c7c795b22f63 1072 if (bytesIn > 0) {
JamieB 76:c7c795b22f63 1073 if (!validated_connection) {
JamieB 76:c7c795b22f63 1074 printf("Recieved %d bytes on unvalidated connection\r\n",bytesIn);
JamieB 76:c7c795b22f63 1075 if ((settingsInBuffer[0] == 0x07) &&
JamieB 76:c7c795b22f63 1076 (settingsInBuffer[1] == 0x05) &&
JamieB 76:c7c795b22f63 1077 (settingsInBuffer[2] == 0x02)) {
JamieB 76:c7c795b22f63 1078 validated_connection = true;
JamieB 76:c7c795b22f63 1079 printf("Validated Connection - Sending 'set quiet' command\r\n");
JamieB 76:c7c795b22f63 1080 VIPS.sendQuiet();
JamieB 76:c7c795b22f63 1081 Thread::wait(50);
JamieB 76:c7c795b22f63 1082 VIPS.EnableDirectTX(true);
JamieB 76:c7c795b22f63 1083 GreenLED = LED_ON;
JamieB 76:c7c795b22f63 1084 VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
JamieB 76:c7c795b22f63 1085 printf("Sent first %d bytes to VIPS\r\n",bytesIn); //also a set-quiet command but good to send twice
JamieB 76:c7c795b22f63 1086 } else {
JamieB 76:c7c795b22f63 1087 printf("Invalid: %X %X %X", settingsInBuffer[0], settingsInBuffer[1], settingsInBuffer[2]);
JamieB 76:c7c795b22f63 1088 }
JamieB 76:c7c795b22f63 1089 } else if (validated_connection) {
JamieB 69:47f800793d00 1090 VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
JamieB 76:c7c795b22f63 1091 printf("Sent %d bytes to VIPS\r\n",bytesIn);
JamieB 69:47f800793d00 1092 }
JamieB 76:c7c795b22f63 1093 }
JamieB 76:c7c795b22f63 1094 if (VIPS.getWaitingBuffer(&bufferToSend, &bytesToSend)) {
JamieB 76:c7c795b22f63 1095 connection.send((char *)bufferToSend, bytesToSend);
JamieB 76:c7c795b22f63 1096 printf("Recieved %d bytes from VIPS\r\n",bytesIn);
JamieB 69:47f800793d00 1097 }
JamieB 76:c7c795b22f63 1098 } while (connection.is_connected());
AndyA 68:9e68287f0ab7 1099
JamieB 76:c7c795b22f63 1100 VIPS.EnableDirectTX(false);
JamieB 76:c7c795b22f63 1101 GreenLED = LED_OFF;
JamieB 76:c7c795b22f63 1102 validated_connection = false;
JamieB 76:c7c795b22f63 1103 pc.puts("Disconnected TCP \r\n");
JamieB 76:c7c795b22f63 1104 connection.close();
JamieB 76:c7c795b22f63 1105 Thread::wait(50);
JamieB 76:c7c795b22f63 1106 }
AndyA 68:9e68287f0ab7 1107 }
JamieB 76:c7c795b22f63 1108 VIPS.EnableDirectTX(false);
JamieB 76:c7c795b22f63 1109 GreenLED = LED_OFF;
JamieB 76:c7c795b22f63 1110 pc.puts("Ending TCP Task\r\n");
AndyA 68:9e68287f0ab7 1111 }
AndyA 68:9e68287f0ab7 1112
AndyA 68:9e68287f0ab7 1113
AndyA 32:bfb385d35097 1114 void ethernetTask(void const* settings)
AndyA 32:bfb385d35097 1115 {
AndyA 32:bfb385d35097 1116
AndyA 68:9e68287f0ab7 1117 Thread* ListenTask=NULL;
JamieB 73:89f799500055 1118 UDPSocket* PrimaryDataSocket = NULL;
JamieB 73:89f799500055 1119 Endpoint PrimaryDataTarget;
AndyA 32:bfb385d35097 1120
JamieB 42:9653486372a6 1121 UDPSocket* VIPSSocket = NULL;
JamieB 42:9653486372a6 1122 Endpoint VIPSTarget;
JamieB 42:9653486372a6 1123
AndyA 32:bfb385d35097 1124 UserSettings_t *settingsPtr = (UserSettings_t *) settings;
AndyA 32:bfb385d35097 1125
AndyA 32:bfb385d35097 1126 pc.puts("Ethernet task startup\r\n");
JamieB 73:89f799500055 1127 if (!settingsPtr->DataOutPort && !settingsPtr->VipsUDPPort && !settingsPtr->SettingsPort) {
AndyA 68:9e68287f0ab7 1128 pc.puts("No ports set. Ethernet task exiting\r\n");
AndyA 32:bfb385d35097 1129 return;
AndyA 32:bfb385d35097 1130 }
AndyA 32:bfb385d35097 1131 int Result = -1;
AndyA 32:bfb385d35097 1132 while (Result != 0) {
AndyA 50:d6c56eccf56a 1133 if ((UserSettings.IPAddress[0] != 0) && (UserSettings.Gateway[0] != 0) && (UserSettings.Subnet[0] != 0)) {
AndyA 32:bfb385d35097 1134 pc.puts("Static IP init attempt\r\n");
AndyA 32:bfb385d35097 1135 Result = eth.init(UserSettings.IPAddress, UserSettings.Subnet, UserSettings.Gateway);
AndyA 32:bfb385d35097 1136 } else {
AndyA 32:bfb385d35097 1137 pc.puts("Dynamic IP init attempt\r\n");
AndyA 32:bfb385d35097 1138 Result = eth.init();
AndyA 32:bfb385d35097 1139 }
AndyA 32:bfb385d35097 1140
AndyA 32:bfb385d35097 1141 if (Result) {
AndyA 32:bfb385d35097 1142 pc.puts("Ethernet init failed\r\n");
AndyA 32:bfb385d35097 1143 Thread::wait(500);
AndyA 32:bfb385d35097 1144 }
AndyA 32:bfb385d35097 1145 }
AndyA 32:bfb385d35097 1146 pc.puts("Ethernet init complete\r\n");
AndyA 32:bfb385d35097 1147
AndyA 32:bfb385d35097 1148 while (true) {
AndyA 32:bfb385d35097 1149
AndyA 50:d6c56eccf56a 1150 Result = -1;
AndyA 50:d6c56eccf56a 1151 while (Result != 0) {
AndyA 32:bfb385d35097 1152 Result = eth.connect();
AndyA 32:bfb385d35097 1153 if (Result) {
AndyA 32:bfb385d35097 1154 pc.puts("Ethernet connect failed\r\n");
AndyA 32:bfb385d35097 1155 Thread::wait(500);
AndyA 32:bfb385d35097 1156 }
AndyA 32:bfb385d35097 1157 }
AndyA 32:bfb385d35097 1158 pc.puts("Ethernet connect complete\r\n");
AndyA 68:9e68287f0ab7 1159 Thread::wait(100);
JamieB 69:47f800793d00 1160 printf("IP Address is %s\r\n", eth.getIPAddress());
AndyA 68:9e68287f0ab7 1161 if (ListenTask)
AndyA 68:9e68287f0ab7 1162 delete ListenTask;
AndyA 68:9e68287f0ab7 1163 if (settingsPtr->SettingsPort) {
AndyA 68:9e68287f0ab7 1164 pc.puts("Starting network update task\r\n");
AndyA 68:9e68287f0ab7 1165 Thread::wait(100);
JamieB 76:c7c795b22f63 1166 StopTCPListening = false;
AndyA 68:9e68287f0ab7 1167 ListenTask = new Thread(settingsNetUpdate, NULL, osPriorityHigh, 256 * 4);
AndyA 68:9e68287f0ab7 1168 }
JamieB 73:89f799500055 1169 if (PrimaryDataSocket)
JamieB 73:89f799500055 1170 delete PrimaryDataSocket;
JamieB 73:89f799500055 1171 if (UserSettings.DataOutPort) {
JamieB 73:89f799500055 1172 pc.printf("Starting Primary data socket - %s\r\n", (UserSettings.FreeDPort ? "FreeD Output Mode" : "VIPS Output Mode"));
AndyA 68:9e68287f0ab7 1173 Thread::wait(50);
JamieB 73:89f799500055 1174 PrimaryDataSocket = new UDPSocket();
JamieB 73:89f799500055 1175 PrimaryDataSocket->init();
JamieB 73:89f799500055 1176 PrimaryDataSocket->set_broadcasting(true);
JamieB 73:89f799500055 1177 PrimaryDataTarget.set_address(0xFFFFFFFF, UserSettings.DataOutPort);
JamieB 77:6453dcc202d5 1178 int option = (1);
JamieB 77:6453dcc202d5 1179 int tos_option = (IPTOS_LOWDELAY|IPTOS_THROUGHPUT|IPTOS_PREC_IMMEDIATE);
JamieB 77:6453dcc202d5 1180 // PrimaryDataSocket->get_option(IPPROTO_IP, IP_TOS, &option, sizeof(option))
JamieB 77:6453dcc202d5 1181 // pc.printf("Socket IPTOS Option: %d\r\n", option);
JamieB 77:6453dcc202d5 1182 PrimaryDataSocket->set_option(IPPROTO_IP, IP_TOS, &tos_option, sizeof(option));
JamieB 77:6453dcc202d5 1183 // PrimaryDataSocket->set_option(IPPROTO_TCP, TCP_NODELAY, &option, sizeof(option));
JamieB 77:6453dcc202d5 1184 // PrimaryDataSocket->set_option(SOL_SOCKET, SO_NO_CHECK, &option, sizeof(option));
JamieB 79:1910ae03cb2e 1185
AndyA 47:8d577dfb7eb1 1186 }
AndyA 32:bfb385d35097 1187
JamieB 42:9653486372a6 1188 if (VIPSSocket)
JamieB 42:9653486372a6 1189 delete VIPSSocket;
AndyA 47:8d577dfb7eb1 1190 if (UserSettings.VipsUDPPort) {
AndyA 68:9e68287f0ab7 1191 pc.puts("Starting VIPS socket\r\n");
AndyA 68:9e68287f0ab7 1192 Thread::wait(50);
AndyA 47:8d577dfb7eb1 1193 VIPSSocket = new UDPSocket();
AndyA 47:8d577dfb7eb1 1194 VIPSSocket->init();
AndyA 47:8d577dfb7eb1 1195 VIPSSocket->set_broadcasting(true);
AndyA 47:8d577dfb7eb1 1196 VIPSTarget.set_address(0xFFFFFFFF, UserSettings.VipsUDPPort);
AndyA 47:8d577dfb7eb1 1197 }
JamieB 58:8cce8fcc765f 1198 EthernetTimer.reset();
JamieB 58:8cce8fcc765f 1199 EthernetTimer.start();
AndyA 68:9e68287f0ab7 1200 pc.puts("Network ready for TX\r\n");
AndyA 68:9e68287f0ab7 1201 Thread::wait(50);
AndyA 32:bfb385d35097 1202 while (true) {
AndyA 32:bfb385d35097 1203 if (EthTxNow) {
JamieB 58:8cce8fcc765f 1204 // NewData.lock();
AndyA 32:bfb385d35097 1205 EthTxNow = false;
JamieB 73:89f799500055 1206 if (!PrimaryDataSocket && UserSettings.DataOutPort) {
JamieB 42:9653486372a6 1207 pc.puts("No FreeD UDP socket\r\n");
JamieB 42:9653486372a6 1208 break;
JamieB 42:9653486372a6 1209 }
JamieB 42:9653486372a6 1210 if (!VIPSSocket && UserSettings.VipsUDPPort) {
JamieB 42:9653486372a6 1211 pc.puts("No VIPS UDP socket\r\n");
AndyA 32:bfb385d35097 1212 break;
AndyA 32:bfb385d35097 1213 }
JamieB 73:89f799500055 1214 if (PrimaryDataSocket) {
JamieB 73:89f799500055 1215 if (UserSettings.FreeDPort)
JamieB 73:89f799500055 1216 Result = PrimaryDataSocket->sendTo(PrimaryDataTarget,(char *)&fdPacket, sizeof(struct D1MsgFormat_s));
JamieB 73:89f799500055 1217 else
JamieB 73:89f799500055 1218 Result = PrimaryDataSocket->sendTo(PrimaryDataTarget,(char *)FlexibleVIPSOut, VIPSOutSize);
JamieB 58:8cce8fcc765f 1219 //uint32_t ethtime = EthernetTimer.read_us();
JamieB 58:8cce8fcc765f 1220 //EthernetTimer.reset();
JamieB 73:89f799500055 1221 int expected_size = UserSettings.FreeDPort ? sizeof(struct D1MsgFormat_s) : VIPSOutSize;
JamieB 73:89f799500055 1222 if (Result != expected_size) {
JamieB 73:89f799500055 1223 pc.puts("Primary Port UDP Tx Failed!\r\n");
AndyA 47:8d577dfb7eb1 1224 break;
JamieB 58:8cce8fcc765f 1225 } //else
JamieB 58:8cce8fcc765f 1226 //pc.printf("%d\r\n", ethtime);
JamieB 42:9653486372a6 1227 }
AndyA 47:8d577dfb7eb1 1228 if (VIPSSocket) {
AndyA 47:8d577dfb7eb1 1229 Result = VIPSSocket->sendTo(VIPSTarget,(char *)FlexibleVIPSOut, VIPSOutSize);
AndyA 47:8d577dfb7eb1 1230 if (Result != VIPSOutSize) {
AndyA 47:8d577dfb7eb1 1231 pc.puts("VIPS UDP Tx Failed!\r\n");
AndyA 47:8d577dfb7eb1 1232 break;
AndyA 47:8d577dfb7eb1 1233 }
AndyA 32:bfb385d35097 1234 }
AndyA 32:bfb385d35097 1235 // pc.puts("UDP Tx\r\n");
AndyA 32:bfb385d35097 1236 } else
JamieB 58:8cce8fcc765f 1237 // NewData.unlock();
JamieB 58:8cce8fcc765f 1238 Thread::wait(1);
AndyA 32:bfb385d35097 1239 }
JamieB 76:c7c795b22f63 1240 StopTCPListening = true;
JamieB 76:c7c795b22f63 1241 Thread::wait(1);
JamieB 76:c7c795b22f63 1242
JamieB 76:c7c795b22f63 1243 while (ListenTask->get_state()) {
JamieB 76:c7c795b22f63 1244 Thread::wait(50);
JamieB 76:c7c795b22f63 1245 }
JamieB 76:c7c795b22f63 1246 // int ListenState = ListenTask->get_state();
JamieB 76:c7c795b22f63 1247 // pc.printf("Listen State: %d\r\n", ListenState);
JamieB 76:c7c795b22f63 1248
AndyA 32:bfb385d35097 1249 eth.disconnect();
AndyA 32:bfb385d35097 1250 pc.puts("Attempting to restart network\r\n");
AndyA 32:bfb385d35097 1251 }
AndyA 32:bfb385d35097 1252 pc.puts("Eth Task ending. Should never happen!\r\n");
AndyA 32:bfb385d35097 1253 }
AndyA 32:bfb385d35097 1254
AndyA 32:bfb385d35097 1255
AndyA 52:adbbaadca0a9 1256 void XBEEBypassmode(int baud)
AndyA 52:adbbaadca0a9 1257 {
AndyA 52:adbbaadca0a9 1258 pc.baud(baud);
AndyA 52:adbbaadca0a9 1259
AndyA 55:ee36cab19a58 1260 /*
AndyA 55:ee36cab19a58 1261 COM1.baud(9600);
AndyA 55:ee36cab19a58 1262 COM1.puts("+++");
AndyA 55:ee36cab19a58 1263 wait(1.2);
AndyA 55:ee36cab19a58 1264 switch (baud) {
AndyA 55:ee36cab19a58 1265 case 1200:
AndyA 55:ee36cab19a58 1266 COM1.puts("ATBD0\n");
AndyA 55:ee36cab19a58 1267 break;
AndyA 55:ee36cab19a58 1268 case 2400:
AndyA 55:ee36cab19a58 1269 COM1.puts("ATBD1\n");
AndyA 55:ee36cab19a58 1270 break;
AndyA 55:ee36cab19a58 1271 case 4800:
AndyA 55:ee36cab19a58 1272 COM1.puts("ATBD2\n");
AndyA 55:ee36cab19a58 1273 break;
AndyA 55:ee36cab19a58 1274 case 9600:
AndyA 55:ee36cab19a58 1275 COM1.puts("ATBD3\n");
AndyA 55:ee36cab19a58 1276 break;
AndyA 55:ee36cab19a58 1277 case 19200:
AndyA 55:ee36cab19a58 1278 COM1.puts("ATBD4\n");
AndyA 55:ee36cab19a58 1279 break;
AndyA 55:ee36cab19a58 1280 case 38400:
AndyA 55:ee36cab19a58 1281 COM1.puts("ATBD5\n");
AndyA 55:ee36cab19a58 1282 break;
AndyA 55:ee36cab19a58 1283 case 57600:
AndyA 55:ee36cab19a58 1284 COM1.puts("ATBD6\n");
AndyA 55:ee36cab19a58 1285 break;
AndyA 55:ee36cab19a58 1286 case 115200:
AndyA 55:ee36cab19a58 1287 COM1.puts("ATBD7\n");
AndyA 55:ee36cab19a58 1288 break;
AndyA 55:ee36cab19a58 1289 default:
AndyA 55:ee36cab19a58 1290 COM1.printf("ATBD%d\n",baud);
AndyA 55:ee36cab19a58 1291 break;
AndyA 55:ee36cab19a58 1292 }
AndyA 52:adbbaadca0a9 1293
AndyA 55:ee36cab19a58 1294 wait(0.1);
AndyA 55:ee36cab19a58 1295 COM1.puts("ATCN\n");
AndyA 55:ee36cab19a58 1296 wait(0.5);
AndyA 55:ee36cab19a58 1297 */
AndyA 52:adbbaadca0a9 1298
AndyA 52:adbbaadca0a9 1299 COM1.baud(baud);
AndyA 52:adbbaadca0a9 1300 while (true) {
AndyA 52:adbbaadca0a9 1301 if (pc.readable()) {
AndyA 52:adbbaadca0a9 1302 char commandIn = pc.getc();
AndyA 52:adbbaadca0a9 1303 COM1.putc(commandIn);
AndyA 52:adbbaadca0a9 1304 }
AndyA 52:adbbaadca0a9 1305 if (COM1.readable())
AndyA 52:adbbaadca0a9 1306 pc.putc(COM1.getc());
AndyA 52:adbbaadca0a9 1307 }
AndyA 52:adbbaadca0a9 1308 }
JamieB 20:ce1d8fbff68f 1309
AndyA 1:dd1f7e162f91 1310 int main()
AndyA 1:dd1f7e162f91 1311 {
AndyA 1:dd1f7e162f91 1312 pc.baud(115200);
JamieB 80:0b7f1b85b626 1313 pc.printf("\r\n\r\nStartup - v%.3f\r\n", APP_VERSION); //Change via Line 1
JamieB 27:498cce52fe5f 1314 setRED();
AndyA 22:0dd9c1b5664a 1315
JamieB 20:ce1d8fbff68f 1316 readSettingsFile();
AndyA 22:0dd9c1b5664a 1317
AndyA 52:adbbaadca0a9 1318 if (UserSettings.bypassBaud) {
AndyA 52:adbbaadca0a9 1319 XBEEBypassmode(UserSettings.bypassBaud);
AndyA 52:adbbaadca0a9 1320 }
AndyA 52:adbbaadca0a9 1321
AndyA 22:0dd9c1b5664a 1322 switch(UserSettings.FIZmode) {
JamieB 21:0dec6dd14e10 1323 case formatPreston :
JamieB 21:0dec6dd14e10 1324 FIZPort = new FIZDisney(p9, p10);
JamieB 26:7f66ac76cd5d 1325 pc.printf("Set Preston");
JamieB 26:7f66ac76cd5d 1326 break;
JamieB 21:0dec6dd14e10 1327 case formatFujiPassive :
JamieB 21:0dec6dd14e10 1328 FIZPort = new FIZDigiPower(p9, p10);
JamieB 26:7f66ac76cd5d 1329 pc.printf("Set FujiPassive");
JamieB 26:7f66ac76cd5d 1330 break;
JamieB 21:0dec6dd14e10 1331 case formatFujiActive :
JamieB 21:0dec6dd14e10 1332 FIZPort = new FIZDigiPowerActive(p9, p10);
JamieB 26:7f66ac76cd5d 1333 pc.printf("Set FujiActive\r\n");
JamieB 26:7f66ac76cd5d 1334 break;
JamieB 29:b0eaeefa4e63 1335 case formatCanon :
JamieB 29:b0eaeefa4e63 1336 FIZPort = new FIZCanon(p9, p10);
JamieB 29:b0eaeefa4e63 1337 pc.printf("Set Canon\r\n");
JamieB 29:b0eaeefa4e63 1338 break;
AndyA 68:9e68287f0ab7 1339 case formatArri:
AndyA 62:dcb92159ac8e 1340 FIZPort = new FIZ_ArriCmotion(p9, p10);
AndyA 62:dcb92159ac8e 1341 pc.printf("Set Arri\r\n");
AndyA 62:dcb92159ac8e 1342 break;
JamieB 21:0dec6dd14e10 1343 default:
JamieB 21:0dec6dd14e10 1344 FIZPort = new FIZDisney(p9, p10); //preston
JamieB 26:7f66ac76cd5d 1345 pc.printf("Set Default - Preston");
AndyA 62:dcb92159ac8e 1346
JamieB 21:0dec6dd14e10 1347 }
JamieB 26:7f66ac76cd5d 1348 // FIZPort = new FIZDigiPowerActive(p9, p10);
AndyA 52:adbbaadca0a9 1349 COM1.baud(115200); // radio port
AndyA 22:0dd9c1b5664a 1350
AndyA 68:9e68287f0ab7 1351 Thread ethernetThread(ethernetTask, &UserSettings, osPriorityHigh, 256 * 4);
AndyA 14:76083dc18b0d 1352 led1 = 0;
AndyA 1:dd1f7e162f91 1353 inputTimer.reset();
AndyA 1:dd1f7e162f91 1354 inputTimer.start();
AndyA 0:97661408d0f9 1355
AndyA 1:dd1f7e162f91 1356 prepPacketOut();
AndyA 3:14d241e29be3 1357
AndyA 9:7214e3c3e5f8 1358 LTCInput.enable(true);
AndyA 3:14d241e29be3 1359
AndyA 35:7ecf25d9c414 1360 if (!VIPS.setFilters(&UserSettings))
AndyA 35:7ecf25d9c414 1361 pc.puts("Failed to create VIPS filters\r\n");
AndyA 44:fe7fcf62c75d 1362
AndyA 1:dd1f7e162f91 1363 VIPS.run();
AndyA 1:dd1f7e162f91 1364
AndyA 14:76083dc18b0d 1365 pc.printf("System init complete\r\n");
JamieB 27:498cce52fe5f 1366 setBLUE();
AndyA 16:a8d3a0dbe4bf 1367 COM1.attach(&onOutputSerialRx);
AndyA 0:97661408d0f9 1368
AndyA 1:dd1f7e162f91 1369 TimeSinceLastFrame.reset();
AndyA 1:dd1f7e162f91 1370 TimeSinceLastFrame.start();
AndyA 9:7214e3c3e5f8 1371
JamieB 27:498cce52fe5f 1372
AndyA 9:7214e3c3e5f8 1373 pc.printf("Waiting for sync input clock\r\n");
AndyA 16:a8d3a0dbe4bf 1374
AndyA 15:830fc953edd9 1375 #ifdef enableFakePPF
JamieB 60:899862db7f2a 1376 FakePPF.attach(callback(&OnPPFInputStartup),1/50.0);
AndyA 15:830fc953edd9 1377 #endif
AndyA 16:a8d3a0dbe4bf 1378
AndyA 9:7214e3c3e5f8 1379 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 9:7214e3c3e5f8 1380 Syncin.rise(callback(&OnSyncInputStartup));
AndyA 9:7214e3c3e5f8 1381 framesIn = 0;
AndyA 9:7214e3c3e5f8 1382 SyncInCount = 0;
AndyA 9:7214e3c3e5f8 1383 bool LockToSync = false;
AndyA 9:7214e3c3e5f8 1384
AndyA 1:dd1f7e162f91 1385 while (true) {
AndyA 10:053bac3e326b 1386 if (SyncInCount == 100) {
AndyA 10:053bac3e326b 1387 LockToSync= true;
AndyA 9:7214e3c3e5f8 1388 break;
AndyA 9:7214e3c3e5f8 1389 }
AndyA 14:76083dc18b0d 1390 if ((framesIn == 100) && (SyncInCount<45)) { // prefer frame sync
AndyA 9:7214e3c3e5f8 1391 break;
AndyA 9:7214e3c3e5f8 1392 }
AndyA 9:7214e3c3e5f8 1393 }
AndyA 1:dd1f7e162f91 1394
JamieB 60:899862db7f2a 1395 if (LockToSync && !UserSettings.ForcePPF) {
AndyA 9:7214e3c3e5f8 1396 pc.printf("Using Genlock sync input\r\n");
AndyA 9:7214e3c3e5f8 1397 Syncin.rise(callback(&OnPPFInputStartup));
AndyA 9:7214e3c3e5f8 1398 PPFin.rise(NULL);
AndyA 9:7214e3c3e5f8 1399 } else {
AndyA 9:7214e3c3e5f8 1400 pc.printf("Using pulse per frame input\r\n");
AndyA 9:7214e3c3e5f8 1401 Syncin.rise(NULL);
AndyA 9:7214e3c3e5f8 1402 }
AndyA 9:7214e3c3e5f8 1403
JamieB 27:498cce52fe5f 1404 setGREEN();
AndyA 9:7214e3c3e5f8 1405 pc.printf("Measuring frame rate\r\n");
AndyA 0:97661408d0f9 1406
AndyA 9:7214e3c3e5f8 1407 int currentFrames = framesIn;
AndyA 9:7214e3c3e5f8 1408 while (framesIn == currentFrames); // wait for next frame;
AndyA 9:7214e3c3e5f8 1409 inputTimer.reset();
AndyA 9:7214e3c3e5f8 1410 framesIn = 0;
AndyA 9:7214e3c3e5f8 1411
AndyA 10:053bac3e326b 1412
AndyA 9:7214e3c3e5f8 1413 while (framesIn < 100); // wait for 100 frames;
AndyA 9:7214e3c3e5f8 1414 uint32_t frameTime = inputTimer.read_us()/100;
AndyA 9:7214e3c3e5f8 1415
AndyA 9:7214e3c3e5f8 1416 int framesPerSecond = frameRates::getClosestRate(frameTime);
AndyA 9:7214e3c3e5f8 1417
AndyA 9:7214e3c3e5f8 1418 pc.printf("Detected frame rate %d\r\n",framesPerSecond);
AndyA 50:d6c56eccf56a 1419 if (UserSettings.SerialTxDelayFrame != 0) {
AndyA 50:d6c56eccf56a 1420 UserSettings.SerialTxDelayMS = (UserSettings.SerialTxDelayFrame*1000)/framesPerSecond;
AndyA 50:d6c56eccf56a 1421 pc.printf("Serial delay of %.2f frames calculated as %.2f ms\r\n",UserSettings.SerialTxDelayFrame,UserSettings.SerialTxDelayMS);
AndyA 50:d6c56eccf56a 1422 }
AndyA 50:d6c56eccf56a 1423 if (UserSettings.UDPTxDelayFrame != 0) {
AndyA 50:d6c56eccf56a 1424 UserSettings.UDPTxDelayMS = (UserSettings.UDPTxDelayFrame*1000)/framesPerSecond;
AndyA 50:d6c56eccf56a 1425 pc.printf("Serial delay of %.2f frames calculated as %.2f ms\r\n",UserSettings.UDPTxDelayFrame,UserSettings.UDPTxDelayMS);
AndyA 50:d6c56eccf56a 1426 }
JamieB 72:da2f1df6a50d 1427 if (UserSettings.InterpolationOffsetFrame != 0) {
JamieB 72:da2f1df6a50d 1428 UserSettings.InterpolationOffset_uS = (UserSettings.InterpolationOffsetFrame*1000000)/framesPerSecond;
JamieB 72:da2f1df6a50d 1429 pc.printf("Interpolation Delay of %.2f frames calculated as %.2f ms\r\n", UserSettings.InterpolationOffsetFrame,UserSettings.InterpolationOffset_uS);
JamieB 72:da2f1df6a50d 1430 }
AndyA 1:dd1f7e162f91 1431
AndyA 9:7214e3c3e5f8 1432 if (LTCInput.synced()) { // getting LTC so set the clock.
AndyA 9:7214e3c3e5f8 1433 currentFrames = framesIn;
AndyA 9:7214e3c3e5f8 1434 while (framesIn == currentFrames); // wait for next frame;
AndyA 9:7214e3c3e5f8 1435 wait_us (frameTime/4); // ensure the LTC decode for the last frame is done.
AndyA 9:7214e3c3e5f8 1436 int hour,minute,second,frame;
AndyA 9:7214e3c3e5f8 1437 bool drop = LTCInput.isFrameDrop();
AndyA 9:7214e3c3e5f8 1438 LTCInput.getTime(&hour,&minute,&second,&frame);
AndyA 9:7214e3c3e5f8 1439 movieTime.setMode(framesPerSecond,drop);
AndyA 14:76083dc18b0d 1440 movieTime.setTime(hour,minute,second,frame);
AndyA 9:7214e3c3e5f8 1441 } else { // no time code so clock time doesn't matter
AndyA 9:7214e3c3e5f8 1442 movieTime.setMode(framesPerSecond,false);
AndyA 9:7214e3c3e5f8 1443 }
AndyA 9:7214e3c3e5f8 1444
AndyA 9:7214e3c3e5f8 1445 if (LockToSync)
AndyA 9:7214e3c3e5f8 1446 Syncin.rise(callback(&OnPPFInput));
AndyA 9:7214e3c3e5f8 1447 else
AndyA 9:7214e3c3e5f8 1448 PPFin.rise(callback(&OnPPFInput));
AndyA 9:7214e3c3e5f8 1449
AndyA 15:830fc953edd9 1450 #ifdef enableFakePPF
JamieB 60:899862db7f2a 1451 FakePPF.attach(callback(&OnPPFInput),1/50.0);
AndyA 15:830fc953edd9 1452 #endif
AndyA 15:830fc953edd9 1453
AndyA 9:7214e3c3e5f8 1454 pc.printf("Current time is %02d:%02d:%02d %02d\r\n",movieTime.hours(),movieTime.minutes(),movieTime.seconds(),movieTime.frame());
AndyA 9:7214e3c3e5f8 1455 LTCInput.enable(false);
JamieB 27:498cce52fe5f 1456 setOFF();
AndyA 9:7214e3c3e5f8 1457
AndyA 9:7214e3c3e5f8 1458 pc.printf("Running\r\n",framesPerSecond);
AndyA 9:7214e3c3e5f8 1459
AndyA 14:76083dc18b0d 1460
AndyA 9:7214e3c3e5f8 1461 getNextFileNumber();
AndyA 16:a8d3a0dbe4bf 1462 COM1.attach(callback(onOutputSerialRx));
AndyA 9:7214e3c3e5f8 1463
JamieB 27:498cce52fe5f 1464
AndyA 9:7214e3c3e5f8 1465 while (true) {
AndyA 9:7214e3c3e5f8 1466 if (NewFramePulse) { // New frame. Output data.
AndyA 9:7214e3c3e5f8 1467 //framePulse(FramePulseTime);
AndyA 15:830fc953edd9 1468 NewFramePulse = false;
AndyA 9:7214e3c3e5f8 1469 framePositionOutput();
AndyA 9:7214e3c3e5f8 1470 }
AndyA 18:ad407a2ed4c9 1471 VIPS.sendQueued(); // should ideally be called on 100Hz PPS edge but we don't have that in this code...
AndyA 9:7214e3c3e5f8 1472
JamieB 80:0b7f1b85b626 1473 // if (pos_value) {
JamieB 80:0b7f1b85b626 1474 // pc.printf("Z: %d - (%d, %d)\r\n", pos_value, pos_lower, pos_upper);
JamieB 80:0b7f1b85b626 1475 // pos_value = 0;
JamieB 80:0b7f1b85b626 1476 // }
JamieB 80:0b7f1b85b626 1477
AndyA 9:7214e3c3e5f8 1478 if (logButtonLastState != logButton) {
AndyA 15:830fc953edd9 1479 logButtonLastState = logButton;
AndyA 15:830fc953edd9 1480 if (logButtonLastState) { // pressed
AndyA 9:7214e3c3e5f8 1481 logButtonDownTime = inputTimer.read();
AndyA 15:830fc953edd9 1482 if ((logButtonDownTime - logButtonUpTime) > 0.2f) {
JamieB 76:c7c795b22f63 1483 // pc.puts("Down\r\n");
JamieB 76:c7c795b22f63 1484 resetTimeout.attach(callback(&OnResetTimeout),5);
AndyA 15:830fc953edd9 1485 if (logging) {
AndyA 15:830fc953edd9 1486 // pc.puts("Logging off\r\n");
AndyA 15:830fc953edd9 1487 GreenLED = LED_OFF;
AndyA 15:830fc953edd9 1488 logging=false;
AndyA 15:830fc953edd9 1489 fclose(logFile);
AndyA 15:830fc953edd9 1490 } else {
AndyA 15:830fc953edd9 1491 logFile = nextBinaryFile();
AndyA 15:830fc953edd9 1492 if(logFile) {
AndyA 15:830fc953edd9 1493 // pc.puts("Logging on\r\n");
AndyA 15:830fc953edd9 1494 GreenLED = LED_ON;
AndyA 15:830fc953edd9 1495 logging = true;
AndyA 15:830fc953edd9 1496 }
AndyA 15:830fc953edd9 1497 //else pc.puts("Logging failed\r\n");
AndyA 9:7214e3c3e5f8 1498 }
AndyA 1:dd1f7e162f91 1499 }
AndyA 9:7214e3c3e5f8 1500 } else { // released
AndyA 9:7214e3c3e5f8 1501 logButtonUpTime = inputTimer.read();
JamieB 76:c7c795b22f63 1502 resetTimeout.attach(NULL,0);
AndyA 15:830fc953edd9 1503 if ((logButtonUpTime-logButtonDownTime) > 0.05f) {
AndyA 15:830fc953edd9 1504 // pc.puts("Up\r\n");
AndyA 15:830fc953edd9 1505 }
AndyA 1:dd1f7e162f91 1506 }
AndyA 0:97661408d0f9 1507 }
AndyA 9:7214e3c3e5f8 1508 }
AndyA 0:97661408d0f9 1509 }