Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial FatFileSystemCpp mbed

main.cpp@27:498cce52fe5f, 2021-07-09 (annotated)

Committer:
JamieB
Date:
Fri Jul 09 16:00:55 2021 +0000
Revision:
27:498cce52fe5f
Parent:
26:7f66ac76cd5d
Child:
29:b0eaeefa4e63
Fixed 180-degree heading wrap-around issue in FreeD output

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 23:5c237f04327d 1 /*
AndyA 23:5c237f04327d 2 Settings file options
AndyA 23:5c237f04327d 3
AndyA 23:5c237f04327d 4 On startup system will look for settings.txt on the built in mbed drive.
AndyA 23:5c237f04327d 5
AndyA 23:5c237f04327d 6 This file should be ascii text with one option per line.
AndyA 23:5c237f04327d 7 The following options supported:
AndyA 23:5c237f04327d 8
AndyA 23:5c237f04327d 9 Output_Format=n
AndyA 23:5c237f04327d 10 Sets the serial output format.
AndyA 23:5c237f04327d 11 n = 0 - VIPS
AndyA 23:5c237f04327d 12 n = 1 - FreeD
AndyA 23:5c237f04327d 13
AndyA 23:5c237f04327d 14
AndyA 23:5c237f04327d 15 FIZ_Format=n
AndyA 23:5c237f04327d 16 Sets the FIZ reader to use
AndyA 23:5c237f04327d 17 n = 0 - Preston
AndyA 23:5c237f04327d 18 n = 1 - Fuji passive listen mode (skycam)
AndyA 23:5c237f04327d 19 n = 2 - Fuji active mode
AndyA 23:5c237f04327d 20
AndyA 23:5c237f04327d 21
AndyA 23:5c237f04327d 22 FreeD_Port=pppp
AndyA 23:5c237f04327d 23 Sets the UDP port for FreeD network output.
AndyA 23:5c237f04327d 24 Data is sent as a UDP broadcast on the select port number.
AndyA 23:5c237f04327d 25 A port number of 0 disables UDP output.
AndyA 23:5c237f04327d 26
AndyA 23:5c237f04327d 27 IP_addr=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 28 Subnet=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 29 Gateway=aaa.bbb.ccc.ddd
AndyA 23:5c237f04327d 30 Set the IPv4 address to use for the ethernet interface.
AndyA 23:5c237f04327d 31 All 3 values must be set for a static address to be used otherwise DHCP will be used.
AndyA 23:5c237f04327d 32
AndyA 23:5c237f04327d 33 All settings are case sensitive.
AndyA 23:5c237f04327d 34 Do NOT include spaces in the options lines.
AndyA 23:5c237f04327d 35 All options default to a value of 0 is omitted from the file.
AndyA 23:5c237f04327d 36
AndyA 23:5c237f04327d 37 */
AndyA 23:5c237f04327d 38
AndyA 23:5c237f04327d 39
AndyA 23:5c237f04327d 40
AndyA 23:5c237f04327d 41
AndyA 0:97661408d0f9 42 #include "mbed.h"
AndyA 0:97661408d0f9 43 #include "LTCApp.h"
AndyA 22:0dd9c1b5664a 44 #include "EthernetInterface.h"
AndyA 0:97661408d0f9 45
AndyA 22:0dd9c1b5664a 46 //#define enableUSBStick
AndyA 22:0dd9c1b5664a 47
AndyA 22:0dd9c1b5664a 48 EthernetInterface eth;
AndyA 22:0dd9c1b5664a 49 UDPSocket* FreeDSocket = NULL;
AndyA 22:0dd9c1b5664a 50 Endpoint FreeDTarget;
AndyA 15:830fc953edd9 51
AndyA 15:830fc953edd9 52 //#define enableFakePPF
AndyA 15:830fc953edd9 53
AndyA 15:830fc953edd9 54 // delay transmit to n ms after the frame
AndyA 15:830fc953edd9 55 // comment out rather than set to 0 to disable
AndyA 15:830fc953edd9 56 //#define Delay_TX_By 10
AndyA 14:76083dc18b0d 57
AndyA 14:76083dc18b0d 58 #ifdef enableUSBStick
AndyA 14:76083dc18b0d 59 #include "MSCFileSystem.h"
AndyA 14:76083dc18b0d 60 MSCFileSystem msc("msc");
AndyA 14:76083dc18b0d 61 #endif
AndyA 9:7214e3c3e5f8 62
AndyA 0:97661408d0f9 63 BufferedSerial pc(USBTX, USBRX);
AndyA 0:97661408d0f9 64 VIPSSerial VIPS(p28, p27);
AndyA 16:a8d3a0dbe4bf 65 RawSerial COM1(p13, p14);
AndyA 16:a8d3a0dbe4bf 66 FIZReader *FIZPort;
AndyA 0:97661408d0f9 67
AndyA 11:ef7f6591b776 68 DigitalOut led1(LED1);
AndyA 0:97661408d0f9 69 DigitalOut led2(LED2);
AndyA 0:97661408d0f9 70 DigitalOut led3(LED3);
AndyA 0:97661408d0f9 71 DigitalOut frameToggle(LED4);
AndyA 0:97661408d0f9 72
AndyA 0:97661408d0f9 73 LTCDecode LTCInput(p7);
AndyA 0:97661408d0f9 74 InterruptIn PPFin(p29);
AndyA 0:97661408d0f9 75 InterruptIn Syncin(p8);
AndyA 0:97661408d0f9 76
AndyA 11:ef7f6591b776 77 DigitalIn logButton(p17,PullDown);
AndyA 11:ef7f6591b776 78
AndyA 15:830fc953edd9 79 DigitalOut RedLED(p18); // red
AndyA 11:ef7f6591b776 80 DigitalOut GreenLED(p19);
AndyA 11:ef7f6591b776 81 DigitalOut BlueLED(p20);
AndyA 11:ef7f6591b776 82
AndyA 11:ef7f6591b776 83 #define LED_OFF 1
AndyA 11:ef7f6591b776 84 #define LED_ON 0
AndyA 14:76083dc18b0d 85
JamieB 27:498cce52fe5f 86 #define setRED() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_ON;})
JamieB 27:498cce52fe5f 87 #define setGREEN() ({GreenLED=LED_ON;BlueLED = LED_OFF;RedLED = LED_OFF;})
JamieB 27:498cce52fe5f 88 #define setBLUE() ({GreenLED=LED_OFF;BlueLED = LED_ON;RedLED = LED_OFF;})
JamieB 27:498cce52fe5f 89 #define setOFF() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_OFF;})
AndyA 9:7214e3c3e5f8 90
AndyA 9:7214e3c3e5f8 91 frameclock movieTime;
AndyA 9:7214e3c3e5f8 92
AndyA 0:97661408d0f9 93 // clock to time everything with
AndyA 0:97661408d0f9 94 Timer inputTimer;
AndyA 12:06050debf014 95 Timeout resetTimeout;
AndyA 16:a8d3a0dbe4bf 96 Timeout filteringTimeout;
AndyA 16:a8d3a0dbe4bf 97 Timeout BypassTimeout;
AndyA 0:97661408d0f9 98
AndyA 15:830fc953edd9 99 #ifdef enableFakePPF
AndyA 15:830fc953edd9 100 Ticker FakePPF;
AndyA 15:830fc953edd9 101 #endif
AndyA 15:830fc953edd9 102
AndyA 9:7214e3c3e5f8 103 float logButtonDownTime;
AndyA 9:7214e3c3e5f8 104 float logButtonUpTime;
AndyA 9:7214e3c3e5f8 105 int logButtonLastState;
AndyA 9:7214e3c3e5f8 106
AndyA 16:a8d3a0dbe4bf 107 bool bypassMode = false;
AndyA 9:7214e3c3e5f8 108 bool logging = false;
AndyA 9:7214e3c3e5f8 109 FILE *logFile = NULL;
AndyA 9:7214e3c3e5f8 110
AndyA 9:7214e3c3e5f8 111
AndyA 0:97661408d0f9 112 // Time since last frame event, used for position output interpolation
AndyA 0:97661408d0f9 113 Timer TimeSinceLastFrame;
AndyA 0:97661408d0f9 114 uint32_t TimeSinceLastFrameWrap;
AndyA 0:97661408d0f9 115
AndyA 0:97661408d0f9 116 // used to start PPS at the correct point
AndyA 0:97661408d0f9 117 Timeout PPSsyncTimer;
AndyA 0:97661408d0f9 118
AndyA 15:830fc953edd9 119 Timeout OutputDelayTimer;
AndyA 15:830fc953edd9 120
AndyA 0:97661408d0f9 121 // used to generate PPS edges
AndyA 0:97661408d0f9 122 Ticker PPSOutputTimer;
AndyA 0:97661408d0f9 123
AndyA 0:97661408d0f9 124 frameRates detectedRate;
AndyA 0:97661408d0f9 125 bool ppsRunning = false; // set when PPS start has been scheduled
AndyA 0:97661408d0f9 126 volatile bool ppsActive = false; // set when PPS is actuallt going (up to 1 second later)
AndyA 0:97661408d0f9 127
AndyA 0:97661408d0f9 128 volatile bool PPSHigh;
AndyA 0:97661408d0f9 129 bool resync = false;
AndyA 0:97661408d0f9 130 bool resyncDone = false;
AndyA 0:97661408d0f9 131 uint32_t resyncPeriod;
AndyA 0:97661408d0f9 132 bool OKToCheckSync = false;
AndyA 0:97661408d0f9 133 volatile uint32_t VBOXTicks = 0; // time at the NEXT PPS edge
AndyA 0:97661408d0f9 134 uint32_t lastPPSSecondStart;
AndyA 0:97661408d0f9 135
AndyA 0:97661408d0f9 136 #define _longPPMTrackLen_ 20
AndyA 0:97661408d0f9 137 float PPMErrors[_longPPMTrackLen_];
AndyA 0:97661408d0f9 138 float PPMTrackTotal;
AndyA 0:97661408d0f9 139 int PPMTrackIndex;
AndyA 0:97661408d0f9 140 float PPMHighAcc;
AndyA 0:97661408d0f9 141 float remainingClockError;
AndyA 0:97661408d0f9 142 bool ppmCorrection;
AndyA 0:97661408d0f9 143
AndyA 22:0dd9c1b5664a 144 enum SerialOutput {mode_VIPS, mode_FreeD};
JamieB 20:ce1d8fbff68f 145 enum FIZFormats {formatPreston, formatFujiPassive, formatFujiActive};
JamieB 20:ce1d8fbff68f 146
AndyA 22:0dd9c1b5664a 147 typedef struct UserSettings_s {
AndyA 22:0dd9c1b5664a 148 int FIZmode;
AndyA 22:0dd9c1b5664a 149 int SerialOutMode;
AndyA 22:0dd9c1b5664a 150 int UDPPort;
AndyA 23:5c237f04327d 151 char IPAddress[32];
AndyA 23:5c237f04327d 152 char Gateway[32];
AndyA 23:5c237f04327d 153 char Subnet[32];
AndyA 22:0dd9c1b5664a 154 } UserSettings_t;
AndyA 22:0dd9c1b5664a 155
AndyA 22:0dd9c1b5664a 156 UserSettings_t UserSettings;
AndyA 9:7214e3c3e5f8 157
AndyA 1:dd1f7e162f91 158 struct outputFormat_s {
AndyA 1:dd1f7e162f91 159 uint32_t header; // 2 byte header + 2 byte length
AndyA 1:dd1f7e162f91 160 uint32_t mask;
AndyA 1:dd1f7e162f91 161 uint32_t time;
AndyA 1:dd1f7e162f91 162 double x;
AndyA 1:dd1f7e162f91 163 double y;
AndyA 1:dd1f7e162f91 164 float z;
AndyA 8:961bb15570a1 165 uint8_t beacons;
AndyA 8:961bb15570a1 166 uint8_t solutionType;
AndyA 8:961bb15570a1 167 uint16_t kfStatus;
AndyA 1:dd1f7e162f91 168 float roll;
AndyA 1:dd1f7e162f91 169 float pitch;
AndyA 1:dd1f7e162f91 170 float yaw;
AndyA 2:a79201e302d7 171 uint8_t accuracy[4];
AndyA 3:14d241e29be3 172 uint32_t focus;
AndyA 3:14d241e29be3 173 uint16_t iris;
AndyA 3:14d241e29be3 174 uint16_t zoom;
AndyA 1:dd1f7e162f91 175 uint16_t checksum;
AndyA 1:dd1f7e162f91 176 } __attribute__((packed)) ;
AndyA 0:97661408d0f9 177
AndyA 1:dd1f7e162f91 178 struct outputFormat_s packetOut;
JamieB 20:ce1d8fbff68f 179 struct D1MsgFormat_s fdPacket;
AndyA 1:dd1f7e162f91 180
AndyA 16:a8d3a0dbe4bf 181 void filterOff(void)
AndyA 16:a8d3a0dbe4bf 182 {
AndyA 16:a8d3a0dbe4bf 183 VIPS.EnableSmoothing(false);
AndyA 16:a8d3a0dbe4bf 184 pc.puts("FilterTimeout");
AndyA 16:a8d3a0dbe4bf 185 }
AndyA 16:a8d3a0dbe4bf 186
AndyA 16:a8d3a0dbe4bf 187 /***************************
AndyA 16:a8d3a0dbe4bf 188 *
AndyA 16:a8d3a0dbe4bf 189 * Input format is
AndyA 16:a8d3a0dbe4bf 190 * enable filter $RLFIZ FILTER ON\n
AndyA 16:a8d3a0dbe4bf 191 * disable filter $RLFIZ FILTER OFF\n
AndyA 16:a8d3a0dbe4bf 192 *
AndyA 16:a8d3a0dbe4bf 193 * Filter mode auto exits after 30 seconds
AndyA 16:a8d3a0dbe4bf 194 *
AndyA 16:a8d3a0dbe4bf 195 ****************************/
AndyA 16:a8d3a0dbe4bf 196
AndyA 16:a8d3a0dbe4bf 197 const int C1InputBufferSize = 20;
AndyA 16:a8d3a0dbe4bf 198 char C1InputBuffer[C1InputBufferSize];
AndyA 16:a8d3a0dbe4bf 199 int C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 200
AndyA 16:a8d3a0dbe4bf 201 void parseC1Input(void)
AndyA 16:a8d3a0dbe4bf 202 {
AndyA 16:a8d3a0dbe4bf 203 if (C1InputBuffer[1] != 'R')
AndyA 16:a8d3a0dbe4bf 204 return;
AndyA 16:a8d3a0dbe4bf 205 if (C1InputBuffer[2] != 'L')
AndyA 16:a8d3a0dbe4bf 206 return;
AndyA 16:a8d3a0dbe4bf 207 if (C1InputBuffer[3] != 'F')
AndyA 16:a8d3a0dbe4bf 208 return;
AndyA 16:a8d3a0dbe4bf 209 if (C1InputBuffer[4] != 'I')
AndyA 16:a8d3a0dbe4bf 210 return;
AndyA 16:a8d3a0dbe4bf 211 if (C1InputBuffer[5] != 'Z')
AndyA 16:a8d3a0dbe4bf 212 return;
AndyA 16:a8d3a0dbe4bf 213
AndyA 16:a8d3a0dbe4bf 214 if (C1InputBuffer[7] == 'F') {
AndyA 16:a8d3a0dbe4bf 215 if (C1InputBuffer[8] != 'I')
AndyA 16:a8d3a0dbe4bf 216 return;
AndyA 16:a8d3a0dbe4bf 217 if (C1InputBuffer[9] != 'L')
AndyA 16:a8d3a0dbe4bf 218 return;
AndyA 16:a8d3a0dbe4bf 219 // 10 = T, 11 = E, 12=R, 13= space, 14 = O, 15 = N or F
AndyA 16:a8d3a0dbe4bf 220 if (C1InputBuffer[14] != 'O')
AndyA 16:a8d3a0dbe4bf 221 return;
AndyA 16:a8d3a0dbe4bf 222
AndyA 16:a8d3a0dbe4bf 223 if (C1InputBuffer[15] == 'N') {
AndyA 16:a8d3a0dbe4bf 224 VIPS.EnableSmoothing(true);
AndyA 16:a8d3a0dbe4bf 225 filteringTimeout.attach(callback(&filterOff),30.0f);
AndyA 16:a8d3a0dbe4bf 226 pc.puts("FilterOn\n");
AndyA 16:a8d3a0dbe4bf 227 return;
AndyA 16:a8d3a0dbe4bf 228 } else {
AndyA 16:a8d3a0dbe4bf 229 VIPS.EnableSmoothing(false);
AndyA 16:a8d3a0dbe4bf 230 filteringTimeout.detach();
AndyA 16:a8d3a0dbe4bf 231 pc.puts("FilterOFF\n");
AndyA 16:a8d3a0dbe4bf 232 return;
AndyA 16:a8d3a0dbe4bf 233 }
AndyA 16:a8d3a0dbe4bf 234 } // if F
AndyA 16:a8d3a0dbe4bf 235
AndyA 16:a8d3a0dbe4bf 236 }
AndyA 16:a8d3a0dbe4bf 237
AndyA 16:a8d3a0dbe4bf 238
AndyA 16:a8d3a0dbe4bf 239 void ExitBypass(void)
AndyA 16:a8d3a0dbe4bf 240 {
AndyA 16:a8d3a0dbe4bf 241 VIPS.EnableBypass(false);
AndyA 16:a8d3a0dbe4bf 242 bypassMode = false;
AndyA 16:a8d3a0dbe4bf 243 }
AndyA 16:a8d3a0dbe4bf 244
AndyA 16:a8d3a0dbe4bf 245
AndyA 16:a8d3a0dbe4bf 246 void vipsBypassRx(char byte)
AndyA 16:a8d3a0dbe4bf 247 {
AndyA 16:a8d3a0dbe4bf 248 COM1.putc(byte);
AndyA 16:a8d3a0dbe4bf 249 }
AndyA 16:a8d3a0dbe4bf 250
AndyA 16:a8d3a0dbe4bf 251
AndyA 16:a8d3a0dbe4bf 252 void onOutputSerialRx(void)
AndyA 16:a8d3a0dbe4bf 253 {
AndyA 16:a8d3a0dbe4bf 254 static bool got0x07 = false;
AndyA 16:a8d3a0dbe4bf 255 led1=!led1;
AndyA 16:a8d3a0dbe4bf 256 while (COM1.readable()) {
AndyA 16:a8d3a0dbe4bf 257 if (bypassMode) {
AndyA 16:a8d3a0dbe4bf 258 VIPS.bypassTx(COM1.getc());
AndyA 16:a8d3a0dbe4bf 259 BypassTimeout.attach(&ExitBypass,5);
AndyA 16:a8d3a0dbe4bf 260 } else {
AndyA 16:a8d3a0dbe4bf 261 C1InputBuffer[C1InputPtr] = COM1.getc();
AndyA 16:a8d3a0dbe4bf 262 pc.putc(C1InputBuffer[C1InputPtr]);
AndyA 16:a8d3a0dbe4bf 263 if (C1InputPtr == 0) {
AndyA 16:a8d3a0dbe4bf 264 if (got0x07) {
AndyA 16:a8d3a0dbe4bf 265 got0x07 = false;
AndyA 16:a8d3a0dbe4bf 266 if ((C1InputBuffer[0] >= 5) && (C1InputBuffer[0] <=11)) {
AndyA 16:a8d3a0dbe4bf 267 VIPS.bypassTx(0x07);
AndyA 16:a8d3a0dbe4bf 268 VIPS.bypassTx(C1InputBuffer[0]);
AndyA 16:a8d3a0dbe4bf 269 bypassMode = true;
AndyA 16:a8d3a0dbe4bf 270 BypassTimeout.attach(&ExitBypass,5);
AndyA 16:a8d3a0dbe4bf 271 }
AndyA 16:a8d3a0dbe4bf 272 } else if (C1InputBuffer[0] == '$')
AndyA 16:a8d3a0dbe4bf 273 C1InputPtr = 1;
AndyA 16:a8d3a0dbe4bf 274 else if (C1InputBuffer[0] == 0x07)
AndyA 16:a8d3a0dbe4bf 275 got0x07 = true;
AndyA 16:a8d3a0dbe4bf 276 } else if (C1InputBuffer[C1InputPtr] == '\n') {
AndyA 16:a8d3a0dbe4bf 277 parseC1Input();
AndyA 16:a8d3a0dbe4bf 278 C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 279 } else {
AndyA 16:a8d3a0dbe4bf 280 C1InputPtr++;
AndyA 16:a8d3a0dbe4bf 281 if (C1InputPtr == C1InputBufferSize)
AndyA 16:a8d3a0dbe4bf 282 C1InputPtr = 0;
AndyA 16:a8d3a0dbe4bf 283 }
AndyA 16:a8d3a0dbe4bf 284 }
AndyA 16:a8d3a0dbe4bf 285 }
AndyA 16:a8d3a0dbe4bf 286 }
AndyA 16:a8d3a0dbe4bf 287
AndyA 16:a8d3a0dbe4bf 288
AndyA 16:a8d3a0dbe4bf 289
AndyA 16:a8d3a0dbe4bf 290
AndyA 1:dd1f7e162f91 291 void prepPacketOut()
AndyA 1:dd1f7e162f91 292 {
AndyA 1:dd1f7e162f91 293 uint8_t bytes[4];
AndyA 1:dd1f7e162f91 294 bytes[0]=0x24;
AndyA 3:14d241e29be3 295 bytes[1]=0xd9;
AndyA 1:dd1f7e162f91 296 *(uint16_t*)(bytes+2) = sizeof(struct outputFormat_s);
AndyA 1:dd1f7e162f91 297 packetOut.header = *(uint32_t*)bytes;
AndyA 8:961bb15570a1 298 packetOut.mask = 0x0446;
AndyA 3:14d241e29be3 299 packetOut.accuracy[0] = 0;
AndyA 3:14d241e29be3 300 packetOut.accuracy[1] = 0;
AndyA 3:14d241e29be3 301 packetOut.accuracy[2] = 0;
AndyA 3:14d241e29be3 302 packetOut.accuracy[3] = 0;
AndyA 1:dd1f7e162f91 303 }
AndyA 1:dd1f7e162f91 304
AndyA 15:830fc953edd9 305 void onOutputTxTime(void)
AndyA 15:830fc953edd9 306 {
AndyA 22:0dd9c1b5664a 307 switch (UserSettings.SerialOutMode) {
JamieB 24:16fd010a71d0 308 case mode_FreeD: {
JamieB 24:16fd010a71d0 309 char *dataPtr = (char *)&fdPacket;
AndyA 22:0dd9c1b5664a 310 for (int byte=0; byte<sizeof(struct D1MsgFormat_s); byte++)
AndyA 22:0dd9c1b5664a 311 COM1.putc(*(dataPtr+byte));
JamieB 24:16fd010a71d0 312 }
JamieB 24:16fd010a71d0 313 break;
AndyA 22:0dd9c1b5664a 314 case mode_VIPS:
JamieB 24:16fd010a71d0 315 default: {
JamieB 24:16fd010a71d0 316 char *dataPtr = (char *)&packetOut;
AndyA 22:0dd9c1b5664a 317 for (int byte=0; byte<sizeof(struct outputFormat_s); byte++)
AndyA 22:0dd9c1b5664a 318 COM1.putc(*(dataPtr+byte));
JamieB 24:16fd010a71d0 319 }
JamieB 24:16fd010a71d0 320 break;
JamieB 20:ce1d8fbff68f 321 }
JamieB 20:ce1d8fbff68f 322 }
AndyA 17:5ce3fe98e76d 323
AndyA 22:0dd9c1b5664a 324 void createFreeDPacket(position *posPtr)
JamieB 20:ce1d8fbff68f 325 {
JamieB 20:ce1d8fbff68f 326 if (posPtr) {
JamieB 20:ce1d8fbff68f 327 fdPacket.header = 0xD1;
JamieB 20:ce1d8fbff68f 328 fdPacket.id = 0xFF - posPtr->ID;
JamieB 27:498cce52fe5f 329 set24bitValue(fdPacket.yaw, (int)(((posPtr->yaw > 180)? posPtr->yaw - 360 : posPtr->yaw) *32768)); ;
JamieB 25:7002be632308 330 set24bitValue(fdPacket.pitch, (int)(posPtr->pitch *32768));
JamieB 25:7002be632308 331 set24bitValue(fdPacket.roll, (int)(posPtr->roll *32768));
JamieB 25:7002be632308 332 // fdPacket.yaw = (int)(posPtr->yaw *32768);
JamieB 25:7002be632308 333 // fdPacket.pitch = (int)(posPtr->pitch *32768);
JamieB 25:7002be632308 334 // fdPacket.roll = (int)(posPtr->roll *32768);
JamieB 25:7002be632308 335 set24bitValue(fdPacket.x, (int)(posPtr->X *64000));
JamieB 25:7002be632308 336 set24bitValue(fdPacket.y, (int)(posPtr->Y *64000));
JamieB 25:7002be632308 337 set24bitValue(fdPacket.z, (int)(posPtr->Height *64000));
JamieB 25:7002be632308 338 // fdPacket.x = (int)(posPtr->X *64000);
JamieB 25:7002be632308 339 // fdPacket.y = (int)(posPtr->Y)*64000;
JamieB 25:7002be632308 340 // fdPacket.z = (int)(posPtr->Height *64000);
AndyA 22:0dd9c1b5664a 341 if (posPtr->KFStatus < 0x200) {
AndyA 22:0dd9c1b5664a 342 led2 = 0;
AndyA 22:0dd9c1b5664a 343 RedLED = LED_ON;
AndyA 22:0dd9c1b5664a 344 } else {
AndyA 22:0dd9c1b5664a 345 led2 = 1;
AndyA 22:0dd9c1b5664a 346 RedLED = LED_OFF;
AndyA 22:0dd9c1b5664a 347 }
JamieB 20:ce1d8fbff68f 348 } else {
JamieB 20:ce1d8fbff68f 349 fdPacket.header = 0xD1;
JamieB 20:ce1d8fbff68f 350 fdPacket.id = 0xFF - posPtr->ID;
JamieB 25:7002be632308 351 set24bitValue(fdPacket.yaw, 0);
JamieB 25:7002be632308 352 set24bitValue(fdPacket.pitch, 0);
JamieB 25:7002be632308 353 set24bitValue(fdPacket.roll, 0);
JamieB 25:7002be632308 354 set24bitValue(fdPacket.x, 0);
JamieB 25:7002be632308 355 set24bitValue(fdPacket.y, 0);
JamieB 25:7002be632308 356 set24bitValue(fdPacket.z, 0);
AndyA 22:0dd9c1b5664a 357 led2 = 1;
AndyA 22:0dd9c1b5664a 358 RedLED = LED_OFF;
JamieB 20:ce1d8fbff68f 359 }
JamieB 20:ce1d8fbff68f 360 uint32_t temp_focus;
JamieB 20:ce1d8fbff68f 361 uint16_t temp_iris, temp_zoom; //can't directly address bitfield
JamieB 20:ce1d8fbff68f 362 FIZPort->getMostRecent(&temp_focus, &temp_iris, &temp_zoom);
JamieB 25:7002be632308 363 set24bitValue(fdPacket.focus, temp_focus);
JamieB 25:7002be632308 364 set24bitValue(fdPacket.zoom, temp_zoom);
JamieB 20:ce1d8fbff68f 365 fdPacket.checksum = GetFdCRC((void *)&fdPacket);
JamieB 25:7002be632308 366 //pc.puts("\r\n'");
AndyA 22:0dd9c1b5664a 367 }
JamieB 20:ce1d8fbff68f 368
AndyA 22:0dd9c1b5664a 369 void sendFreeDpacket()
AndyA 22:0dd9c1b5664a 370 {
JamieB 20:ce1d8fbff68f 371 #ifdef Delay_TX_By
JamieB 20:ce1d8fbff68f 372 OutputDelayTimer.attach_us(&onOutputTxTime,Delay_TX_By*1000);
JamieB 20:ce1d8fbff68f 373 #else
JamieB 20:ce1d8fbff68f 374 onOutputTxTime();// COM1.write(&packetOut, sizeof(struct outputFormat_s));
JamieB 20:ce1d8fbff68f 375 #endif
JamieB 20:ce1d8fbff68f 376 if (logging) {
JamieB 20:ce1d8fbff68f 377 if (!fwrite(&fdPacket, sizeof(struct D1MsgFormat_s), 1, logFile)) { // write failed
JamieB 20:ce1d8fbff68f 378 GreenLED = LED_OFF;
JamieB 20:ce1d8fbff68f 379 logging = false;
JamieB 20:ce1d8fbff68f 380 fclose(logFile);
JamieB 20:ce1d8fbff68f 381 logFile = NULL;
JamieB 20:ce1d8fbff68f 382 }
JamieB 20:ce1d8fbff68f 383 }
AndyA 15:830fc953edd9 384 }
AndyA 15:830fc953edd9 385
AndyA 15:830fc953edd9 386
JamieB 20:ce1d8fbff68f 387 void sendVIPSPacket(position *posPtr)
AndyA 1:dd1f7e162f91 388 {
AndyA 0:97661408d0f9 389 if (posPtr) {
AndyA 1:dd1f7e162f91 390 packetOut.x = posPtr->X;
AndyA 1:dd1f7e162f91 391 packetOut.y = posPtr->Y;
AndyA 1:dd1f7e162f91 392 packetOut.z = posPtr->Height;
AndyA 8:961bb15570a1 393 packetOut.beacons = posPtr->beacons;
AndyA 8:961bb15570a1 394 packetOut.solutionType = posPtr->solutionType;
AndyA 9:7214e3c3e5f8 395 packetOut.kfStatus = posPtr->KFStatus;
AndyA 1:dd1f7e162f91 396 packetOut.roll = posPtr->roll;
AndyA 1:dd1f7e162f91 397 packetOut.pitch = posPtr->pitch;
AndyA 1:dd1f7e162f91 398 packetOut.yaw = posPtr->yaw;
AndyA 2:a79201e302d7 399 packetOut.accuracy[3] = posPtr->ID;
AndyA 22:0dd9c1b5664a 400 if (packetOut.kfStatus < 0x200) {
AndyA 22:0dd9c1b5664a 401 led2 = 0;
AndyA 22:0dd9c1b5664a 402 RedLED = LED_ON;
AndyA 22:0dd9c1b5664a 403 } else {
AndyA 22:0dd9c1b5664a 404 led2 = 1;
AndyA 22:0dd9c1b5664a 405 RedLED = LED_OFF;
AndyA 22:0dd9c1b5664a 406 }
AndyA 6:61274e214f46 407 } else {
AndyA 6:61274e214f46 408 packetOut.x = 0;
AndyA 6:61274e214f46 409 packetOut.y = 0;
AndyA 6:61274e214f46 410 packetOut.z = 0;
AndyA 6:61274e214f46 411 packetOut.roll = 0;
AndyA 6:61274e214f46 412 packetOut.pitch = 0;
AndyA 6:61274e214f46 413 packetOut.yaw = 0;
AndyA 6:61274e214f46 414 packetOut.accuracy[3] = 0;
AndyA 8:961bb15570a1 415 packetOut.beacons = 0;
AndyA 8:961bb15570a1 416 packetOut.solutionType = 0;
AndyA 9:7214e3c3e5f8 417 packetOut.kfStatus = 0;
AndyA 22:0dd9c1b5664a 418 led2 = 1;
AndyA 22:0dd9c1b5664a 419 RedLED = LED_OFF;
AndyA 0:97661408d0f9 420 }
AndyA 9:7214e3c3e5f8 421 packetOut.time = movieTime.getTimeMS();
AndyA 16:a8d3a0dbe4bf 422 FIZPort->getMostRecent(&packetOut.focus, &packetOut.iris, &packetOut.zoom);
AndyA 6:61274e214f46 423 VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
AndyA 15:830fc953edd9 424
AndyA 15:830fc953edd9 425 #ifdef Delay_TX_By
AndyA 15:830fc953edd9 426 OutputDelayTimer.attach_us(&onOutputTxTime,Delay_TX_By*1000);
AndyA 15:830fc953edd9 427 #else
AndyA 17:5ce3fe98e76d 428 onOutputTxTime();// COM1.write(&packetOut, sizeof(struct outputFormat_s));
AndyA 15:830fc953edd9 429 #endif
AndyA 9:7214e3c3e5f8 430 if (logging) {
AndyA 9:7214e3c3e5f8 431 if (!fwrite(&packetOut, sizeof(struct outputFormat_s), 1, logFile)) { // write failed
AndyA 15:830fc953edd9 432 GreenLED = LED_OFF;
AndyA 9:7214e3c3e5f8 433 logging = false;
AndyA 9:7214e3c3e5f8 434 fclose(logFile);
AndyA 9:7214e3c3e5f8 435 logFile = NULL;
AndyA 0:97661408d0f9 436 }
AndyA 0:97661408d0f9 437 }
AndyA 0:97661408d0f9 438 }
AndyA 0:97661408d0f9 439
AndyA 22:0dd9c1b5664a 440 void UDPTxFreeD()
AndyA 22:0dd9c1b5664a 441 {
AndyA 22:0dd9c1b5664a 442 if (!FreeDSocket) {
AndyA 22:0dd9c1b5664a 443 pc.printf("No UDP socket\n");
AndyA 22:0dd9c1b5664a 444 return;
AndyA 22:0dd9c1b5664a 445 }
AndyA 22:0dd9c1b5664a 446 int result = FreeDSocket->sendTo(FreeDTarget,(char *)&fdPacket, sizeof(struct D1MsgFormat_s));
AndyA 22:0dd9c1b5664a 447 if (result == sizeof(struct D1MsgFormat_s))
JamieB 25:7002be632308 448 return;
JamieB 25:7002be632308 449 if (result > 0)
JamieB 25:7002be632308 450 pc.printf("UDP Tx was short???\r\n");
AndyA 22:0dd9c1b5664a 451 else if (result == 0)
JamieB 25:7002be632308 452 pc.printf("UDP Tx sent 0 bytes???\r\n");
AndyA 22:0dd9c1b5664a 453 else
JamieB 25:7002be632308 454 pc.printf("UDP Tx Failed! (%d)\r\n", result);
AndyA 22:0dd9c1b5664a 455 }
AndyA 22:0dd9c1b5664a 456
JamieB 20:ce1d8fbff68f 457 void sendPosition(position *posPtr)
JamieB 20:ce1d8fbff68f 458 {
JamieB 25:7002be632308 459 if (!posPtr) {
JamieB 25:7002be632308 460 pc.puts("No VIPS Data");
JamieB 25:7002be632308 461 }
JamieB 25:7002be632308 462
JamieB 25:7002be632308 463
AndyA 22:0dd9c1b5664a 464 // FreeD could be output by two different methods so calculate first if needed rather than at Tx.
AndyA 22:0dd9c1b5664a 465 if ((UserSettings.SerialOutMode==mode_FreeD) || UserSettings.UDPPort)
AndyA 22:0dd9c1b5664a 466 createFreeDPacket(posPtr);
AndyA 22:0dd9c1b5664a 467
AndyA 22:0dd9c1b5664a 468 // Network is faster so send that first.
AndyA 22:0dd9c1b5664a 469 if (UserSettings.UDPPort) {
AndyA 22:0dd9c1b5664a 470 UDPTxFreeD();
AndyA 22:0dd9c1b5664a 471 }
AndyA 22:0dd9c1b5664a 472
AndyA 22:0dd9c1b5664a 473 switch (UserSettings.SerialOutMode) {
AndyA 22:0dd9c1b5664a 474 case mode_FreeD:
AndyA 22:0dd9c1b5664a 475 sendFreeDpacket();
AndyA 22:0dd9c1b5664a 476 break;
AndyA 22:0dd9c1b5664a 477 case mode_VIPS:
AndyA 22:0dd9c1b5664a 478 default:
AndyA 22:0dd9c1b5664a 479 sendVIPSPacket(posPtr);
AndyA 22:0dd9c1b5664a 480 break;
JamieB 20:ce1d8fbff68f 481 }
JamieB 20:ce1d8fbff68f 482 }
JamieB 20:ce1d8fbff68f 483
JamieB 20:ce1d8fbff68f 484
AndyA 0:97661408d0f9 485 //called once per frame to output the current postition
AndyA 1:dd1f7e162f91 486 void framePositionOutput()
AndyA 1:dd1f7e162f91 487 {
AndyA 14:76083dc18b0d 488 led3 = !led3;
AndyA 9:7214e3c3e5f8 489 uint32_t outputTime = TimeSinceLastFrame.read_us();
AndyA 0:97661408d0f9 490 TimeSinceLastFrame.reset();
AndyA 9:7214e3c3e5f8 491 sendPosition(VIPS.sendPositionForTime(outputTime));
AndyA 16:a8d3a0dbe4bf 492 FIZPort->requestCurrent();
AndyA 0:97661408d0f9 493 }
AndyA 0:97661408d0f9 494
AndyA 9:7214e3c3e5f8 495 int getNextFileNumber()
AndyA 1:dd1f7e162f91 496 {
AndyA 14:76083dc18b0d 497
AndyA 14:76083dc18b0d 498 #ifdef enableUSBStick
AndyA 9:7214e3c3e5f8 499 static unsigned int fileNbr = 0;
AndyA 9:7214e3c3e5f8 500 char fileName[32];
AndyA 9:7214e3c3e5f8 501 FILE *filePtr = NULL;
JamieB 20:ce1d8fbff68f 502 pc.puts("DiskInit\r\n");
JamieB 20:ce1d8fbff68f 503 if (!msc.disk_initialize()) {
JamieB 20:ce1d8fbff68f 504 pc.puts("DiskInit failed/r/n");
JamieB 20:ce1d8fbff68f 505 return fileNbr;
JamieB 20:ce1d8fbff68f 506 }
AndyA 14:76083dc18b0d 507
AndyA 9:7214e3c3e5f8 508 do {
AndyA 9:7214e3c3e5f8 509 if (filePtr != NULL)
AndyA 9:7214e3c3e5f8 510 fclose(filePtr);
JamieB 20:ce1d8fbff68f 511 pc.printf("Looking to see if %d exists\r\n",fileNbr);
AndyA 15:830fc953edd9 512 sprintf(fileName,"/msc/VIPS%04u.bin",fileNbr);
AndyA 9:7214e3c3e5f8 513 filePtr = fopen(fileName,"rb");
AndyA 15:830fc953edd9 514 if (filePtr) fileNbr++;
AndyA 9:7214e3c3e5f8 515 } while (filePtr != NULL);
JamieB 20:ce1d8fbff68f 516 pc.puts("File not found\r\n");
AndyA 15:830fc953edd9 517 return fileNbr;
AndyA 14:76083dc18b0d 518 #else
AndyA 14:76083dc18b0d 519 return 0;
AndyA 15:830fc953edd9 520 #endif
AndyA 0:97661408d0f9 521 }
AndyA 0:97661408d0f9 522
AndyA 9:7214e3c3e5f8 523 FILE *nextBinaryFile(void)
AndyA 1:dd1f7e162f91 524 {
AndyA 14:76083dc18b0d 525 #ifdef enableUSBStick
AndyA 15:830fc953edd9 526 // pc.puts("DiskInit\r\n");
AndyA 15:830fc953edd9 527 // int initResult = msc.disk_initialize();
AndyA 15:830fc953edd9 528 // pc.printf("Init returned %d\r\n",initResult);
AndyA 14:76083dc18b0d 529
AndyA 15:830fc953edd9 530 // if (msc.disk_initialize()==) {
AndyA 15:830fc953edd9 531 // pc.puts("DiskInit failed/r/n");
AndyA 15:830fc953edd9 532 // return NULL;
AndyA 15:830fc953edd9 533 // }
AndyA 9:7214e3c3e5f8 534 char fileName[32];
AndyA 9:7214e3c3e5f8 535 int file = getNextFileNumber();
AndyA 9:7214e3c3e5f8 536 sprintf(fileName,"/msc/VIPS%04u.bin",file);
AndyA 15:830fc953edd9 537 // pc.printf("Opending output file %s\r\n",fileName);
AndyA 9:7214e3c3e5f8 538 return fopen(fileName,"wb");
AndyA 14:76083dc18b0d 539 #else
AndyA 15:830fc953edd9 540 pc.puts("Disk support disabled\r\n");
AndyA 15:830fc953edd9 541 return NULL;
AndyA 14:76083dc18b0d 542 #endif
AndyA 9:7214e3c3e5f8 543 }
AndyA 7:87aea27cc68b 544
AndyA 0:97661408d0f9 545
AndyA 0:97661408d0f9 546 volatile bool NewFramePulse= false;
AndyA 0:97661408d0f9 547 volatile int framesIn = 0;
AndyA 1:dd1f7e162f91 548 void OnPPFInputStartup(void)
AndyA 1:dd1f7e162f91 549 {
AndyA 0:97661408d0f9 550 framesIn++;
AndyA 0:97661408d0f9 551 }
AndyA 0:97661408d0f9 552
AndyA 0:97661408d0f9 553 volatile int SyncInCount = 0;
AndyA 1:dd1f7e162f91 554 void OnSyncInputStartup(void)
AndyA 1:dd1f7e162f91 555 {
AndyA 0:97661408d0f9 556 SyncInCount++;
AndyA 0:97661408d0f9 557 }
AndyA 0:97661408d0f9 558
AndyA 0:97661408d0f9 559
AndyA 1:dd1f7e162f91 560 void OnPPFInput(void)
AndyA 1:dd1f7e162f91 561 {
AndyA 14:76083dc18b0d 562 frameToggle=!frameToggle;
AndyA 9:7214e3c3e5f8 563 movieTime.nextFrame();
AndyA 0:97661408d0f9 564 NewFramePulse = true;
AndyA 0:97661408d0f9 565 }
AndyA 0:97661408d0f9 566
AndyA 14:76083dc18b0d 567 void OnResetTimeout()
AndyA 14:76083dc18b0d 568 {
AndyA 12:06050debf014 569 NVIC_SystemReset();
AndyA 14:76083dc18b0d 570 }
AndyA 12:06050debf014 571
AndyA 0:97661408d0f9 572
JamieB 20:ce1d8fbff68f 573 void readSettingsFile()
JamieB 20:ce1d8fbff68f 574 {
JamieB 20:ce1d8fbff68f 575 LocalFileSystem localFS("local");
JamieB 20:ce1d8fbff68f 576 FILE *LSFile= fopen("/local/settings.txt","r");
JamieB 20:ce1d8fbff68f 577 char lineBuffer[64];
JamieB 20:ce1d8fbff68f 578 int valueIn;
JamieB 20:ce1d8fbff68f 579 if (LSFile) {
JamieB 20:ce1d8fbff68f 580 while (!feof(LSFile)) {
JamieB 20:ce1d8fbff68f 581 if (fgets(lineBuffer, 64, LSFile)) {
JamieB 20:ce1d8fbff68f 582 if (sscanf(lineBuffer,"Output_Format=%d",&valueIn) == 1) {
JamieB 20:ce1d8fbff68f 583 pc.printf("Got Output_Format value from file of %d\r\n",valueIn);
AndyA 22:0dd9c1b5664a 584 UserSettings.SerialOutMode = valueIn;
JamieB 20:ce1d8fbff68f 585 }
JamieB 20:ce1d8fbff68f 586 if (sscanf(lineBuffer,"FIZ_Format=%d",&valueIn) == 1) {
JamieB 20:ce1d8fbff68f 587 pc.printf("Got FIZ_Format value from file of %d\r\n",valueIn);
AndyA 22:0dd9c1b5664a 588 UserSettings.FIZmode = valueIn;
AndyA 22:0dd9c1b5664a 589 }
AndyA 22:0dd9c1b5664a 590 if (sscanf(lineBuffer,"FreeD_Port=%d",&valueIn) == 1) {
AndyA 22:0dd9c1b5664a 591 pc.printf("Got FreeD_Port value from file of %d\r\n",valueIn);
AndyA 22:0dd9c1b5664a 592 UserSettings.UDPPort = valueIn;
JamieB 20:ce1d8fbff68f 593 }
AndyA 23:5c237f04327d 594 if (sscanf(lineBuffer,"IP_addr=%s",UserSettings.IPAddress) == 1) {
AndyA 23:5c237f04327d 595 pc.printf("Got IP_addr value from file of %s\r\n",UserSettings.IPAddress);
AndyA 23:5c237f04327d 596 }
AndyA 23:5c237f04327d 597 if (sscanf(lineBuffer,"Subnet=%s",UserSettings.Subnet) == 1) {
JamieB 25:7002be632308 598 pc.printf("Got Subnet mask value from file of %s\r\n",UserSettings.Subnet);
AndyA 23:5c237f04327d 599 }
JamieB 25:7002be632308 600 if (sscanf(lineBuffer,"Gateway=%s",UserSettings.Gateway) == 1) {
AndyA 23:5c237f04327d 601 pc.printf("Got gateway value from file of %s\r\n",UserSettings.Gateway);
AndyA 23:5c237f04327d 602 }
JamieB 20:ce1d8fbff68f 603 }
JamieB 20:ce1d8fbff68f 604 }
JamieB 20:ce1d8fbff68f 605 fclose(LSFile);
JamieB 20:ce1d8fbff68f 606 } else {
JamieB 20:ce1d8fbff68f 607 pc.printf("No Settings File Found \r\n");
JamieB 20:ce1d8fbff68f 608 }
JamieB 20:ce1d8fbff68f 609 }
JamieB 20:ce1d8fbff68f 610
JamieB 20:ce1d8fbff68f 611
AndyA 1:dd1f7e162f91 612 int main()
AndyA 1:dd1f7e162f91 613 {
AndyA 1:dd1f7e162f91 614 pc.baud(115200);
JamieB 26:7f66ac76cd5d 615 pc.printf("\r\n\r\nStartup - v0.7\r\n");
JamieB 27:498cce52fe5f 616 setRED();
AndyA 22:0dd9c1b5664a 617
AndyA 22:0dd9c1b5664a 618 UserSettings.FIZmode = formatPreston;
AndyA 22:0dd9c1b5664a 619 UserSettings.SerialOutMode = mode_VIPS;
AndyA 22:0dd9c1b5664a 620 UserSettings.UDPPort = 0;
AndyA 23:5c237f04327d 621 UserSettings.IPAddress[0] = 0;
AndyA 23:5c237f04327d 622 UserSettings.Gateway[0] = 0;
AndyA 23:5c237f04327d 623 UserSettings.Subnet[0] = 0;
AndyA 23:5c237f04327d 624
JamieB 20:ce1d8fbff68f 625 readSettingsFile();
AndyA 22:0dd9c1b5664a 626
AndyA 22:0dd9c1b5664a 627 switch(UserSettings.FIZmode) {
JamieB 21:0dec6dd14e10 628 case formatPreston :
JamieB 21:0dec6dd14e10 629 FIZPort = new FIZDisney(p9, p10);
JamieB 26:7f66ac76cd5d 630 pc.printf("Set Preston");
JamieB 26:7f66ac76cd5d 631 break;
JamieB 21:0dec6dd14e10 632 case formatFujiPassive :
JamieB 21:0dec6dd14e10 633 FIZPort = new FIZDigiPower(p9, p10);
JamieB 26:7f66ac76cd5d 634 pc.printf("Set FujiPassive");
JamieB 26:7f66ac76cd5d 635 break;
JamieB 21:0dec6dd14e10 636 case formatFujiActive :
JamieB 21:0dec6dd14e10 637 FIZPort = new FIZDigiPowerActive(p9, p10);
JamieB 26:7f66ac76cd5d 638 pc.printf("Set FujiActive\r\n");
JamieB 26:7f66ac76cd5d 639 break;
JamieB 21:0dec6dd14e10 640 default:
JamieB 21:0dec6dd14e10 641 FIZPort = new FIZDisney(p9, p10); //preston
JamieB 26:7f66ac76cd5d 642 pc.printf("Set Default - Preston");
JamieB 21:0dec6dd14e10 643 }
JamieB 26:7f66ac76cd5d 644 // FIZPort = new FIZDigiPowerActive(p9, p10);
AndyA 22:0dd9c1b5664a 645 COM1.baud(115200); // VIPS port
AndyA 22:0dd9c1b5664a 646
AndyA 22:0dd9c1b5664a 647 if (UserSettings.UDPPort) {
AndyA 22:0dd9c1b5664a 648 int Result = -1;
AndyA 22:0dd9c1b5664a 649 while (Result != 0) {
AndyA 23:5c237f04327d 650
AndyA 23:5c237f04327d 651 if ((UserSettings.IPAddress[0] != 0) && (UserSettings.Gateway[0] != 0) && (UserSettings.Subnet[0] != 0)) {
AndyA 23:5c237f04327d 652 Result = eth.init(UserSettings.IPAddress, UserSettings.Subnet, UserSettings.Gateway);
JamieB 25:7002be632308 653 pc.printf("Performing ethernet init with static IP address of %s\r\n",UserSettings.IPAddress);
AndyA 23:5c237f04327d 654 } else {
AndyA 23:5c237f04327d 655 Result = eth.init();
JamieB 25:7002be632308 656 pc.printf("Performing ethernet init with DHCP\r\n");
AndyA 23:5c237f04327d 657 }
AndyA 23:5c237f04327d 658
AndyA 22:0dd9c1b5664a 659 if (Result) {
JamieB 25:7002be632308 660 pc.printf("Ethernet init failed\r\n");
AndyA 22:0dd9c1b5664a 661 wait(1);
AndyA 22:0dd9c1b5664a 662 }
AndyA 22:0dd9c1b5664a 663 }
AndyA 22:0dd9c1b5664a 664 Result = -1;
AndyA 22:0dd9c1b5664a 665 while (Result != 0) {
AndyA 22:0dd9c1b5664a 666 Result = eth.connect();
AndyA 22:0dd9c1b5664a 667 if (Result) {
JamieB 25:7002be632308 668 pc.printf("Ethernet connect failed: %d\r\n", Result);
AndyA 22:0dd9c1b5664a 669 wait(1);
AndyA 22:0dd9c1b5664a 670 }
AndyA 22:0dd9c1b5664a 671 }
AndyA 22:0dd9c1b5664a 672 char *ipAddress = eth.getIPAddress();
AndyA 22:0dd9c1b5664a 673 if (ipAddress && (strlen(ipAddress) > 4)) {
JamieB 25:7002be632308 674 pc.printf("IP Address is %s\r\n",eth.getIPAddress());
AndyA 22:0dd9c1b5664a 675 }
AndyA 22:0dd9c1b5664a 676 FreeDSocket = new UDPSocket();
JamieB 25:7002be632308 677 FreeDSocket->init();
AndyA 22:0dd9c1b5664a 678 FreeDSocket->set_broadcasting(true);
JamieB 25:7002be632308 679 // FreeDSocket->bind(UserSettings.UDPPort);
JamieB 25:7002be632308 680 FreeDTarget.set_address(0xFFFFFFFF, UserSettings.UDPPort);
AndyA 22:0dd9c1b5664a 681 }
AndyA 22:0dd9c1b5664a 682
AndyA 14:76083dc18b0d 683 led1 = 0;
AndyA 1:dd1f7e162f91 684 inputTimer.reset();
AndyA 1:dd1f7e162f91 685 inputTimer.start();
AndyA 0:97661408d0f9 686
AndyA 1:dd1f7e162f91 687 prepPacketOut();
AndyA 3:14d241e29be3 688
AndyA 9:7214e3c3e5f8 689 LTCInput.enable(true);
AndyA 3:14d241e29be3 690
AndyA 1:dd1f7e162f91 691 VIPS.run();
AndyA 1:dd1f7e162f91 692
AndyA 14:76083dc18b0d 693 pc.printf("System init complete\r\n");
JamieB 27:498cce52fe5f 694 setBLUE();
AndyA 16:a8d3a0dbe4bf 695 COM1.attach(&onOutputSerialRx);
AndyA 0:97661408d0f9 696
AndyA 1:dd1f7e162f91 697 TimeSinceLastFrame.reset();
AndyA 1:dd1f7e162f91 698 TimeSinceLastFrame.start();
AndyA 9:7214e3c3e5f8 699
JamieB 27:498cce52fe5f 700
AndyA 9:7214e3c3e5f8 701 pc.printf("Waiting for sync input clock\r\n");
AndyA 16:a8d3a0dbe4bf 702
AndyA 15:830fc953edd9 703 #ifdef enableFakePPF
AndyA 15:830fc953edd9 704 FakePPF.attach(callback(&OnPPFInputStartup),1/24.0);
AndyA 15:830fc953edd9 705 #endif
AndyA 16:a8d3a0dbe4bf 706
AndyA 9:7214e3c3e5f8 707 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 9:7214e3c3e5f8 708 Syncin.rise(callback(&OnSyncInputStartup));
AndyA 9:7214e3c3e5f8 709 framesIn = 0;
AndyA 9:7214e3c3e5f8 710 SyncInCount = 0;
AndyA 9:7214e3c3e5f8 711 bool LockToSync = false;
AndyA 9:7214e3c3e5f8 712
AndyA 1:dd1f7e162f91 713 while (true) {
AndyA 10:053bac3e326b 714 if (SyncInCount == 100) {
AndyA 10:053bac3e326b 715 LockToSync= true;
AndyA 9:7214e3c3e5f8 716 break;
AndyA 9:7214e3c3e5f8 717 }
AndyA 14:76083dc18b0d 718 if ((framesIn == 100) && (SyncInCount<45)) { // prefer frame sync
AndyA 9:7214e3c3e5f8 719 break;
AndyA 9:7214e3c3e5f8 720 }
AndyA 9:7214e3c3e5f8 721 }
AndyA 1:dd1f7e162f91 722
AndyA 9:7214e3c3e5f8 723 if (LockToSync) {
AndyA 9:7214e3c3e5f8 724 pc.printf("Using Genlock sync input\r\n");
AndyA 9:7214e3c3e5f8 725 Syncin.rise(callback(&OnPPFInputStartup));
AndyA 9:7214e3c3e5f8 726 PPFin.rise(NULL);
AndyA 9:7214e3c3e5f8 727 } else {
AndyA 9:7214e3c3e5f8 728 pc.printf("Using pulse per frame input\r\n");
AndyA 9:7214e3c3e5f8 729 Syncin.rise(NULL);
AndyA 9:7214e3c3e5f8 730 }
AndyA 9:7214e3c3e5f8 731
JamieB 27:498cce52fe5f 732 setGREEN();
AndyA 9:7214e3c3e5f8 733 pc.printf("Measuring frame rate\r\n");
AndyA 0:97661408d0f9 734
AndyA 9:7214e3c3e5f8 735 int currentFrames = framesIn;
AndyA 9:7214e3c3e5f8 736 while (framesIn == currentFrames); // wait for next frame;
AndyA 9:7214e3c3e5f8 737 inputTimer.reset();
AndyA 9:7214e3c3e5f8 738 framesIn = 0;
AndyA 9:7214e3c3e5f8 739
AndyA 10:053bac3e326b 740
AndyA 9:7214e3c3e5f8 741 while (framesIn < 100); // wait for 100 frames;
AndyA 9:7214e3c3e5f8 742 uint32_t frameTime = inputTimer.read_us()/100;
AndyA 9:7214e3c3e5f8 743
AndyA 9:7214e3c3e5f8 744 int framesPerSecond = frameRates::getClosestRate(frameTime);
AndyA 9:7214e3c3e5f8 745
AndyA 9:7214e3c3e5f8 746 pc.printf("Detected frame rate %d\r\n",framesPerSecond);
AndyA 1:dd1f7e162f91 747
AndyA 9:7214e3c3e5f8 748 if (LTCInput.synced()) { // getting LTC so set the clock.
AndyA 9:7214e3c3e5f8 749 currentFrames = framesIn;
AndyA 9:7214e3c3e5f8 750 while (framesIn == currentFrames); // wait for next frame;
AndyA 9:7214e3c3e5f8 751 wait_us (frameTime/4); // ensure the LTC decode for the last frame is done.
AndyA 9:7214e3c3e5f8 752 int hour,minute,second,frame;
AndyA 9:7214e3c3e5f8 753 bool drop = LTCInput.isFrameDrop();
AndyA 9:7214e3c3e5f8 754 LTCInput.getTime(&hour,&minute,&second,&frame);
AndyA 9:7214e3c3e5f8 755 movieTime.setMode(framesPerSecond,drop);
AndyA 14:76083dc18b0d 756 movieTime.setTime(hour,minute,second,frame);
AndyA 9:7214e3c3e5f8 757 } else { // no time code so clock time doesn't matter
AndyA 9:7214e3c3e5f8 758 movieTime.setMode(framesPerSecond,false);
AndyA 9:7214e3c3e5f8 759 }
AndyA 9:7214e3c3e5f8 760
AndyA 9:7214e3c3e5f8 761 if (LockToSync)
AndyA 9:7214e3c3e5f8 762 Syncin.rise(callback(&OnPPFInput));
AndyA 9:7214e3c3e5f8 763 else
AndyA 9:7214e3c3e5f8 764 PPFin.rise(callback(&OnPPFInput));
AndyA 9:7214e3c3e5f8 765
AndyA 15:830fc953edd9 766 #ifdef enableFakePPF
AndyA 15:830fc953edd9 767 FakePPF.attach(callback(&OnPPFInput),1/24.0);
AndyA 15:830fc953edd9 768 #endif
AndyA 15:830fc953edd9 769
AndyA 9:7214e3c3e5f8 770 pc.printf("Current time is %02d:%02d:%02d %02d\r\n",movieTime.hours(),movieTime.minutes(),movieTime.seconds(),movieTime.frame());
AndyA 9:7214e3c3e5f8 771 LTCInput.enable(false);
JamieB 27:498cce52fe5f 772 setOFF();
AndyA 9:7214e3c3e5f8 773
AndyA 9:7214e3c3e5f8 774 pc.printf("Running\r\n",framesPerSecond);
AndyA 9:7214e3c3e5f8 775
AndyA 14:76083dc18b0d 776
AndyA 9:7214e3c3e5f8 777 getNextFileNumber();
AndyA 16:a8d3a0dbe4bf 778 COM1.attach(callback(onOutputSerialRx));
AndyA 9:7214e3c3e5f8 779
JamieB 27:498cce52fe5f 780
AndyA 9:7214e3c3e5f8 781 while (true) {
AndyA 9:7214e3c3e5f8 782 if (NewFramePulse) { // New frame. Output data.
AndyA 9:7214e3c3e5f8 783 //framePulse(FramePulseTime);
AndyA 15:830fc953edd9 784 NewFramePulse = false;
AndyA 9:7214e3c3e5f8 785 framePositionOutput();
AndyA 9:7214e3c3e5f8 786 }
AndyA 18:ad407a2ed4c9 787 VIPS.sendQueued(); // should ideally be called on 100Hz PPS edge but we don't have that in this code...
AndyA 9:7214e3c3e5f8 788
AndyA 9:7214e3c3e5f8 789 if (logButtonLastState != logButton) {
AndyA 15:830fc953edd9 790 logButtonLastState = logButton;
AndyA 15:830fc953edd9 791 if (logButtonLastState) { // pressed
AndyA 9:7214e3c3e5f8 792 logButtonDownTime = inputTimer.read();
AndyA 15:830fc953edd9 793 if ((logButtonDownTime - logButtonUpTime) > 0.2f) {
AndyA 15:830fc953edd9 794 // pc.puts("Down\r\n");
AndyA 15:830fc953edd9 795 // resetTimeout.attach(callback(&OnResetTimeout),10);
AndyA 15:830fc953edd9 796 if (logging) {
AndyA 15:830fc953edd9 797 // pc.puts("Logging off\r\n");
AndyA 15:830fc953edd9 798 GreenLED = LED_OFF;
AndyA 15:830fc953edd9 799 logging=false;
AndyA 15:830fc953edd9 800 fclose(logFile);
AndyA 15:830fc953edd9 801 } else {
AndyA 15:830fc953edd9 802 logFile = nextBinaryFile();
AndyA 15:830fc953edd9 803 if(logFile) {
AndyA 15:830fc953edd9 804 // pc.puts("Logging on\r\n");
AndyA 15:830fc953edd9 805 GreenLED = LED_ON;
AndyA 15:830fc953edd9 806 logging = true;
AndyA 15:830fc953edd9 807 }
AndyA 15:830fc953edd9 808 //else pc.puts("Logging failed\r\n");
AndyA 9:7214e3c3e5f8 809 }
AndyA 1:dd1f7e162f91 810 }
AndyA 9:7214e3c3e5f8 811 } else { // released
AndyA 9:7214e3c3e5f8 812 logButtonUpTime = inputTimer.read();
AndyA 15:830fc953edd9 813 // resetTimeout.attach(NULL,0);
AndyA 15:830fc953edd9 814 if ((logButtonUpTime-logButtonDownTime) > 0.05f) {
AndyA 15:830fc953edd9 815 // pc.puts("Up\r\n");
AndyA 15:830fc953edd9 816 }
AndyA 1:dd1f7e162f91 817 }
AndyA 0:97661408d0f9 818 }
AndyA 9:7214e3c3e5f8 819 }
AndyA 0:97661408d0f9 820 }