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, 22 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?
User | Revision | Line number | New 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 | } |