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@35:7ecf25d9c414, 2021-08-05 (annotated)

Committer:
AndyA
Date:
Thu Aug 05 13:44:45 2021 +0000
Revision:
35:7ecf25d9c414
Parent:
34:c864a0c67dbf
Child:
36:fff2a71bbf86
filter working

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