main.cpp

00001 #define APP_VERSION 0.32
00002 
00003 /*
00004 Settings file options
00005 
00006 On startup system will look for settings.txt on the built in mbed drive.
00007 
00008 This file should be ascii text with one option per line.
00009 The following options supported:
00010 
00011 Output_Format=n
00012     Sets the serial output format.
00013     n = 0 - VIPS
00014     n = 1 - FreeD
00015 
00016 
00017 FIZ_Format=n
00018     Sets the FIZ reader to use
00019     n = 0 - Preston
00020     n = 1 - Fuji passive listen mode (skycam)
00021     n = 2 - Fuji active mode
00022     n = 3 - Canon
00023     n = 4 - Arri
00024 
00025 FreeD_Port=pppp
00026     Sets the UDP port for FreeD network output.
00027     Data is sent as a UDP broadcast on the select port number.
00028     A port number of 0 disables UDP output.
00029 
00030 VIPS_UDP_Port=pppp
00031     Same As Above
00032 
00033 IP_addr=aaa.bbb.ccc.ddd
00034 Subnet=aaa.bbb.ccc.ddd
00035 Gateway=aaa.bbb.ccc.ddd
00036     Set the IPv4 address to use for the ethernet interface.
00037     All 3 values must be set for a static address to be used otherwise DHCP will be used.
00038 
00039 
00040 FilterOrder=n
00041 FilterFreq=m.mm
00042 FilterRate=r.rr
00043 Low pass filter settings for channels that have it enabled.
00044 Filter is of order n with a cut off at m Hz assuming input data rate is at r Hz
00045 Filter order must be set to enable filters.
00046 Frequency default is 10Hz
00047 Rate default is 100Hz
00048 
00049 FilterXY=1
00050 FilterZ=1
00051 FilterRoll=1
00052 FilterPitch=1
00053 FilterYaw=1
00054 Enable channels to low pass filter. All filters use the settings given above.
00055 A value of 1 enables the filter. A value of 0 or skipping the line disables the filter.
00056 
00057 NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
00058 
00059 
00060 ExtendedOutput=1
00061 Enable extra fields in VIPS output.
00062 
00063 
00064 All settings are case sensitive.
00065 Do NOT include spaces in the options lines.
00066 All options default to a value of 0 is omitted from the file.
00067 
00068 */
00069 
00070 #include "mbed.h"
00071 #include "LTCApp.h"
00072 #include "EthernetInterface.h"
00073 #include <vector>
00074 
00075 //#define enableUSBStick
00076 //#define enableFakePPF
00077 
00078 // delay transmit to n ms after the frame
00079 // comment out rather than set to 0 to disable
00080 //#define Delay_TX_By 10
00081 
00082 #ifdef enableUSBStick
00083 #include "MSCFileSystem.h"
00084 MSCFileSystem msc("msc");
00085 #endif
00086 
00087 BufferedSerial pc(USBTX, USBRX);
00088 VIPSSerial VIPS(p28, p27);
00089 BufferedSerial COM1(p13, p14);
00090 FIZReader *FIZPort;
00091 EthernetInterface eth;
00092 
00093 DigitalOut led1(LED1);
00094 DigitalOut led2(LED2);
00095 DigitalOut led3(LED3);
00096 DigitalOut frameToggle(LED4);
00097 
00098 LTCDecode LTCInput(p7);
00099 InterruptIn PPFin(p29);
00100 InterruptIn Syncin(p8);
00101 
00102 DigitalIn logButton(p17,PullDown);
00103 
00104 DigitalOut RedLED(p18); // red
00105 DigitalOut GreenLED(p19);
00106 DigitalOut BlueLED(p20);
00107 
00108 LocalFileSystem localFS("local");
00109 
00110 #define LED_OFF 1
00111 #define LED_ON 0
00112 
00113 #define setRED() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_ON;})
00114 #define setGREEN() ({GreenLED=LED_ON;BlueLED = LED_OFF;RedLED = LED_OFF;})
00115 #define setBLUE() ({GreenLED=LED_OFF;BlueLED = LED_ON;RedLED = LED_OFF;})
00116 #define setOFF() ({GreenLED=LED_OFF;BlueLED = LED_OFF;RedLED = LED_OFF;})
00117 
00118 frameclock movieTime;
00119 
00120 // clock to time everything with
00121 Timer inputTimer;
00122 Timeout resetTimeout;
00123 Timeout filteringTimeout;
00124 Timeout BypassTimeout;
00125 
00126 #ifdef enableFakePPF
00127 Ticker FakePPF;
00128 #endif
00129 
00130 float logButtonDownTime;
00131 float logButtonUpTime;
00132 int logButtonLastState;
00133 
00134 bool bypassMode = false;
00135 bool logging = false;
00136 FILE *logFile = NULL;
00137 
00138 
00139 // Time since last frame event, used for position output interpolation
00140 Timer TimeSinceLastFrame;
00141 uint32_t TimeSinceLastFrameWrap;
00142 
00143 //Debug Timer for Ethernet
00144 Timer EthernetTimer;
00145 uint32_t TimeSinceLastEthernetPacket;
00146 
00147 // used to start PPS at the correct point
00148 Timeout PPSsyncTimer;
00149 
00150 Timeout OutputDelayTimer;
00151 Timeout UDPOutputDelayTimer;
00152 
00153 // used to generate PPS edges
00154 Ticker PPSOutputTimer;
00155 
00156 frameRates detectedRate;
00157 bool ppsRunning = false; // set when PPS start has been scheduled
00158 volatile bool ppsActive = false;  // set when PPS is actuallt going (up to 1 second later)
00159 
00160 volatile bool PPSHigh;
00161 bool resync = false;
00162 bool resyncDone = false;
00163 uint32_t resyncPeriod;
00164 bool OKToCheckSync = false;
00165 volatile uint32_t VBOXTicks = 0; // time at the NEXT PPS edge
00166 uint32_t lastPPSSecondStart;
00167 volatile bool StopTCPListening = false;
00168 
00169 #define _longPPMTrackLen_ 20
00170 float PPMErrors[_longPPMTrackLen_];
00171 float PPMTrackTotal;
00172 int PPMTrackIndex;
00173 float PPMHighAcc;
00174 float remainingClockError;
00175 bool ppmCorrection;
00176 
00177 enum SerialOutput {mode_VIPS, mode_FreeD};
00178 enum FIZFormats {formatPreston, formatFujiPassive, formatFujiActive, formatCanon, formatArri};
00179 
00180 UserSettings_t UserSettings;
00181 
00182 uint8_t FlexibleVIPSOut[200];
00183 int VIPSOutSize;
00184 
00185 struct D1MsgFormat_s fdPacket;
00186 
00187 volatile bool EthTxNow = false;
00188 Mutex NewData;
00189 
00190 bool TXFrame = true;
00191 
00192 // int pos_lower = 0;
00193 // int pos_upper = 0;
00194 // int pos_value = 0;
00195 
00196 void filterOff(void)
00197 {
00198     VIPS.EnableSmoothing(false);
00199     pc.puts("FilterTimeout");
00200 }
00201 
00202 /***************************
00203 *
00204 * Input format is
00205 * enable filter $RLFIZ FILTER ON\n
00206 * disable filter $RLFIZ FILTER OFF\n
00207 *
00208 * Filter mode auto exits after 30 seconds
00209 *
00210 ****************************/
00211 
00212 const int C1InputBufferSize = 20;
00213 char C1InputBuffer[C1InputBufferSize];
00214 int C1InputPtr = 0;
00215 
00216 void parseC1Input(void)
00217 {
00218     if (C1InputBuffer[1] != 'R')
00219         return;
00220     if (C1InputBuffer[2] != 'L')
00221         return;
00222     if (C1InputBuffer[3] != 'F')
00223         return;
00224     if (C1InputBuffer[4] != 'I')
00225         return;
00226     if (C1InputBuffer[5] != 'Z')
00227         return;
00228 
00229     if (C1InputBuffer[7] == 'F') {
00230         if (C1InputBuffer[8] != 'I')
00231             return;
00232         if (C1InputBuffer[9] != 'L')
00233             return;
00234         // 10 = T, 11 = E, 12=R, 13= space, 14 = O, 15 = N or F
00235         if (C1InputBuffer[14] != 'O')
00236             return;
00237 
00238         if (C1InputBuffer[15] == 'N') {
00239             VIPS.ForceSmoothing(true);
00240             filteringTimeout.attach(callback(&filterOff),30.0f);
00241             pc.puts("FilterOn\n");
00242             return;
00243         } else {
00244             VIPS.ForceSmoothing(false);
00245             filteringTimeout.detach();
00246             pc.puts("FilterOFF\n");
00247             return;
00248         }
00249     } // if F
00250 
00251 }
00252 
00253 
00254 void ExitBypass(void)
00255 {
00256     VIPS.EnableBypass(false);
00257     bypassMode = false;
00258 }
00259 
00260 
00261 void vipsBypassRx(char byte)
00262 {
00263     COM1.putc(byte);
00264 }
00265 
00266 
00267 void onOutputSerialRx(void)
00268 {
00269     static bool got0x07 = false;
00270     led1=!led1;
00271     while (COM1.RawSerial::readable()) {
00272         if (bypassMode) {
00273             VIPS.bypassTx(COM1.RawSerial::getc());
00274             BypassTimeout.attach(&ExitBypass,5);
00275         } else {
00276             C1InputBuffer[C1InputPtr] = COM1.RawSerial::getc();
00277             pc.putc(C1InputBuffer[C1InputPtr]);
00278             if (C1InputPtr == 0) {
00279                 if (got0x07) {
00280                     got0x07 = false;
00281                     if ((C1InputBuffer[0] >= 5) && (C1InputBuffer[0] <=11)) {
00282                         VIPS.bypassTx(0x07);
00283                         VIPS.bypassTx(C1InputBuffer[0]);
00284                         bypassMode = true;
00285                         BypassTimeout.attach(&ExitBypass,5);
00286                     }
00287                 } else if (C1InputBuffer[0] == '$')
00288                     C1InputPtr = 1;
00289                 else if (C1InputBuffer[0] == 0x07)
00290                     got0x07 = true;
00291             } else if (C1InputBuffer[C1InputPtr] == '\n') {
00292                 parseC1Input();
00293                 C1InputPtr = 0;
00294             } else {
00295                 C1InputPtr++;
00296                 if (C1InputPtr == C1InputBufferSize)
00297                     C1InputPtr = 0;
00298             }
00299         }
00300     }
00301 }
00302 
00303 
00304 
00305 
00306 void prepPacketOut()
00307 {
00308     FlexibleVIPSOut[0]=0x24;
00309     FlexibleVIPSOut[1]=0xd9;
00310 }
00311 
00312 void onOutputTxTime(void)
00313 {
00314     switch (UserSettings.SerialOutMode) {
00315         case mode_FreeD: {
00316             char *dataPtr = (char *)&fdPacket;
00317             for (int byte=0; byte<sizeof(struct D1MsgFormat_s); byte++)
00318                 COM1.putc(*(dataPtr+byte));
00319         }
00320         break;
00321         case mode_VIPS:
00322         default: {
00323             char *dataPtr = (char *)FlexibleVIPSOut;
00324             for (int byte=0; byte<VIPSOutSize; byte++)
00325                 COM1.putc(*(dataPtr+byte));
00326         }
00327         break;
00328     }
00329 }
00330 
00331 void createFreeDPacket(position *posPtr)
00332 {
00333     if (posPtr) {
00334         fdPacket.header = 0xD1;
00335         fdPacket.id = 0xFF - posPtr->ID;
00336         set24bitValue(fdPacket.yaw, (int)(((posPtr->yaw > 180)? posPtr->yaw - 360 : posPtr->yaw) *32768)); ;
00337         set24bitValue(fdPacket.pitch, (int)(posPtr->pitch *32768));
00338         set24bitValue(fdPacket.roll, (int)(posPtr->roll *32768));
00339 //        fdPacket.yaw = (int)(posPtr->yaw *32768);
00340 //        fdPacket.pitch = (int)(posPtr->pitch *32768);
00341 //        fdPacket.roll = (int)(posPtr->roll *32768);
00342         set24bitValue(fdPacket.x, (int)(posPtr->X *64000));
00343         set24bitValue(fdPacket.y, (int)(posPtr->Y *64000));
00344         set24bitValue(fdPacket.z, (int)(posPtr->Height *64000));
00345 //        fdPacket.x = (int)(posPtr->X *64000);
00346 //        fdPacket.y = (int)(posPtr->Y)*64000;
00347 //        fdPacket.z = (int)(posPtr->Height *64000);
00348         if (posPtr->KFStatus < 0x200) {
00349             led2 = 0;
00350             RedLED = LED_ON;
00351         } else {
00352             led2 = 1;
00353             RedLED = LED_OFF;
00354         }
00355     } else {
00356         fdPacket.header = 0xD1;
00357         fdPacket.id = 0xFF - posPtr->ID;
00358         set24bitValue(fdPacket.yaw, 0);
00359         set24bitValue(fdPacket.pitch, 0);
00360         set24bitValue(fdPacket.roll, 0);
00361         set24bitValue(fdPacket.x, 0);
00362         set24bitValue(fdPacket.y, 0);
00363         set24bitValue(fdPacket.z, 0);
00364         led2 = 1;
00365         RedLED = LED_OFF;
00366     }
00367     uint32_t temp_focus;
00368     uint16_t temp_iris, temp_zoom; //can't directly address bitfield
00369     FIZPort->getMostRecent(&temp_focus, &temp_iris, &temp_zoom);
00370     set24bitValue(fdPacket.focus, temp_focus);
00371     set24bitValue(fdPacket.zoom, temp_zoom);
00372     fdPacket.checksum = GetFdCRC((void *)&fdPacket);
00373     //pc.puts("\r\n'");
00374 }
00375 
00376 void sendFreeDpacketSerial()
00377 {
00378     if (UserSettings.SerialTxDelayMS > 0)
00379         OutputDelayTimer.attach_us(&onOutputTxTime,UserSettings.SerialTxDelayMS*1000);
00380     else
00381         onOutputTxTime();//    COM1.write(&packetOut, sizeof(struct outputFormat_s));
00382 
00383     if (logging) {
00384         if (!fwrite(&fdPacket, sizeof(struct D1MsgFormat_s), 1, logFile)) { // write failed
00385             GreenLED = LED_OFF;
00386             logging = false;
00387             fclose(logFile);
00388             logFile = NULL;
00389         }
00390     }
00391 }
00392 
00393 
00394 void createVIPSPacket(position *posPtr)
00395 {
00396 
00397     int byteCount = 4;
00398     //uint32_t mask = 0x0446; // Status,orientation,accuracy,FIZ
00399     uint32_t mask = 0x0546; // Status,orientation,accuracy,FIZ,RAW_IMU
00400     if (posPtr && UserSettings.FlexibleVIPSOut) {
00401         if (posPtr->UsedBeaconsValid) {
00402             mask |= 0x1000;
00403         }
00404         if (posPtr->LLAPosition)
00405             mask |= 0x0001;
00406     }
00407 
00408     *((uint32_t*)(FlexibleVIPSOut + byteCount)) = mask;
00409     byteCount +=4;
00410 
00411     *((uint32_t*)(FlexibleVIPSOut + byteCount)) = movieTime.getTimeMS();
00412     byteCount +=4;
00413 
00414     if (posPtr) {
00415         *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->X;
00416         byteCount +=8;
00417         *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->Y;
00418         byteCount +=8;
00419         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->Height;
00420         byteCount +=4;
00421         FlexibleVIPSOut[byteCount++] = posPtr->beacons;
00422         FlexibleVIPSOut[byteCount++] = posPtr->solutionType;
00423         *((uint16_t*)(FlexibleVIPSOut + byteCount)) = posPtr->KFStatus;
00424         byteCount +=2;
00425         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->roll;
00426         byteCount +=4;
00427         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->pitch;
00428         byteCount +=4;
00429         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->yaw;
00430         byteCount +=4;
00431     } else {
00432         int blankBytes = 8+8+4+1+1+2+4+4+4;
00433         memset(FlexibleVIPSOut + byteCount,0,blankBytes);
00434         byteCount += blankBytes;
00435     }
00436 
00437     FlexibleVIPSOut[byteCount++] = 0;
00438     FlexibleVIPSOut[byteCount++] = 0;
00439     FlexibleVIPSOut[byteCount++] = 0;
00440     FlexibleVIPSOut[byteCount++] = posPtr?posPtr->ID:0;
00441 
00442    if (posPtr) {
00443             //RAWIMU
00444         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->x_accel;
00445         byteCount +=4;
00446         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->y_accel;
00447         byteCount +=4;
00448         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->z_accel;
00449         byteCount +=4;
00450         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->x_gyro;
00451         byteCount +=4;
00452         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->y_gyro;
00453         byteCount +=4;
00454         *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->z_gyro;
00455         byteCount +=4;
00456     }
00457 
00458     FIZPort->getMostRecent((uint32_t*)(FlexibleVIPSOut + byteCount), (uint16_t*)(FlexibleVIPSOut + byteCount+4), (uint16_t*)(FlexibleVIPSOut + byteCount+6));
00459     byteCount+=8;
00460 
00461     if (mask & 0x1000) { // pos must be valid for this to be set
00462         memcpy(FlexibleVIPSOut + byteCount,posPtr->UsedBeacons,12);
00463         byteCount+=12;
00464     }
00465 
00466     // set length to current size plus checksum.
00467     VIPSOutSize = byteCount+2;
00468     *((uint16_t*)(FlexibleVIPSOut + 2)) = VIPSOutSize;
00469 
00470     VIPSSerial::getCRC(FlexibleVIPSOut, byteCount, (void *)(FlexibleVIPSOut+byteCount));
00471 }
00472 
00473 
00474 void sendVIPSPacketSerial()
00475 {
00476     if (UserSettings.SerialTxDelayMS > 0)
00477         OutputDelayTimer.attach_us(&onOutputTxTime,UserSettings.SerialTxDelayMS*1000);
00478     else
00479         onOutputTxTime();//    COM1.write(&packetOut, sizeof(struct outputFormat_s));
00480 
00481     if (logging) {
00482         if (!fwrite(FlexibleVIPSOut, VIPSOutSize, 1, logFile)) { // write failed
00483             GreenLED = LED_OFF;
00484             logging = false;
00485             fclose(logFile);
00486             logFile = NULL;
00487         }
00488     }
00489 }
00490 
00491 void UDP_Tx_Now()
00492 {
00493     EthTxNow = true;
00494     NewData.unlock();
00495 }
00496 
00497 void applyOffsets(float *value, int offset, bool invert, int max_wrap)
00498 {
00499     int min_wrap = max_wrap - 360;
00500 
00501     *value += offset;
00502     if (invert)
00503         *value = 360-*value;
00504 
00505     *value += 360-max_wrap;
00506 
00507     if (*value<min_wrap)
00508         *value+=((int)(*value/-360)+1)*360;
00509     if (*value>=max_wrap)
00510         *value-=((int)(*value/360))*360;
00511 
00512     *value -= 360-max_wrap;
00513 }
00514 
00515 void sendPosition(position *posPtr)
00516 {
00517     NewData.lock();
00518     static bool NoDataWarn = true;
00519     if (!posPtr) {
00520         if (NoDataWarn) { // only warn once per dropout
00521             pc.puts("No VIPS Data\r\n");
00522             NoDataWarn = false;
00523         }
00524     } else
00525         NoDataWarn= true;
00526 
00527     if (posPtr) {
00528         applyOffsets(&posPtr->roll,  UserSettings.OffsetRoll,  UserSettings.InvertRoll, 180);
00529         applyOffsets(&posPtr->pitch, UserSettings.OffsetPitch, UserSettings.InvertPitch, 180);
00530         applyOffsets(&posPtr->yaw,   UserSettings.OffsetYaw,   UserSettings.InvertYaw, 360);
00531     }
00532 
00533 
00534 // FreeD could be output by two different methods so calculate first if needed rather than at Tx.
00535     if ((UserSettings.SerialOutMode==mode_FreeD) || UserSettings.FreeDPort)
00536         createFreeDPacket(posPtr);
00537 
00538     if ((UserSettings.SerialOutMode==mode_VIPS) || UserSettings.VipsUDPPort || (UserSettings.DataOutPort && !UserSettings.FreeDPort))
00539         createVIPSPacket(posPtr);
00540 
00541 
00542 // Network is faster so send that first.
00543     if (UserSettings.DataOutPort || UserSettings.VipsUDPPort) {
00544         if (UserSettings.UDPTxDelayMS > 0) {
00545             UDPOutputDelayTimer.attach_us(&UDP_Tx_Now,UserSettings.UDPTxDelayMS*1000);
00546         } else
00547             UDP_Tx_Now();
00548     }
00549     if (TXFrame) {
00550 
00551         switch (UserSettings.SerialOutMode) {
00552             case mode_FreeD:
00553                 sendFreeDpacketSerial();
00554                 break;
00555             case mode_VIPS:
00556             default:
00557                 sendVIPSPacketSerial();
00558                 break;
00559         }
00560 
00561         if (posPtr && (posPtr->KFStatus < 0x200)) {
00562             led2 = 0;
00563             RedLED = LED_ON;
00564         } else {
00565             led2 = 1;
00566             RedLED = LED_OFF;
00567         }
00568 
00569     }
00570     if (UserSettings.HalfRate) {
00571         TXFrame = !TXFrame;
00572     }
00573 }
00574 
00575 
00576 //called once per frame to output the current postition
00577 void framePositionOutput()
00578 {
00579     led3 = !led3;
00580     uint32_t outputTime = TimeSinceLastFrame.read_us() + UserSettings.InterpolationOffset_uS;
00581     TimeSinceLastFrame.reset();
00582     sendPosition(VIPS.sendPositionForTime(outputTime));
00583     FIZPort->requestCurrent();
00584 }
00585 
00586 int getNextFileNumber()
00587 {
00588 
00589 #ifdef enableUSBStick
00590     static unsigned int fileNbr = 0;
00591     char fileName[32];
00592     FILE *filePtr = NULL;
00593     pc.puts("DiskInit\r\n");
00594     if (!msc.disk_initialize()) {
00595         pc.puts("DiskInit failed/r/n");
00596         return fileNbr;
00597     }
00598 
00599     do {
00600         if (filePtr != NULL)
00601             fclose(filePtr);
00602         pc.printf("Looking to see if %d exists\r\n",fileNbr);
00603         sprintf(fileName,"/msc/VIPS%04u.bin",fileNbr);
00604         filePtr = fopen(fileName,"rb");
00605         if (filePtr) fileNbr++;
00606     } while (filePtr != NULL);
00607     pc.puts("File not found\r\n");
00608     return fileNbr;
00609 #else
00610     return 0;
00611 #endif
00612 }
00613 
00614 FILE *nextBinaryFile(void)
00615 {
00616 #ifdef enableUSBStick
00617 //   pc.puts("DiskInit\r\n");
00618 //   int initResult = msc.disk_initialize();
00619 //   pc.printf("Init returned %d\r\n",initResult);
00620 
00621 //   if (msc.disk_initialize()==) {
00622 //       pc.puts("DiskInit failed/r/n");
00623 //       return NULL;
00624 //   }
00625     char fileName[32];
00626     int file = getNextFileNumber();
00627     sprintf(fileName,"/msc/VIPS%04u.bin",file);
00628 //   pc.printf("Opending output file %s\r\n",fileName);
00629     return fopen(fileName,"wb");
00630 #else
00631     pc.puts("Disk support disabled\r\n");
00632     return NULL;
00633 #endif
00634 }
00635 
00636 
00637 volatile bool NewFramePulse= false;
00638 volatile int framesIn = 0;
00639 void OnPPFInputStartup(void)
00640 {
00641     framesIn++;
00642 }
00643 
00644 volatile int SyncInCount = 0;
00645 void OnSyncInputStartup(void)
00646 {
00647     SyncInCount++;
00648 }
00649 
00650 
00651 void OnPPFInput(void)
00652 {
00653     frameToggle=!frameToggle;
00654     movieTime.nextFrame();
00655     NewFramePulse = true;
00656 }
00657 
00658 void OnResetTimeout()
00659 {
00660     __disable_irq();
00661     led1=1;
00662     led2=1;
00663     led3=1;
00664     frameToggle=1;
00665     RedLED=1; // red
00666     GreenLED=1;
00667     BlueLED=1;
00668     wait(1);
00669     NVIC_SystemReset();
00670 }
00671 
00672 void readLensFile(char* filename)
00673 {
00674     FILE *LensFile = fopen(filename,"r");
00675     if (LensFile) {
00676         char chunk[64];
00677         size_t len = sizeof(chunk);
00678         char *line = new char[len];
00679         pc.printf("Opened File %s\r\n", filename);
00680 <<<<<<< working copy
00681         vector<int> *encoder;
00682         vector<float> *absolute;
00683 =======
00684         vector<unsigned int> *encoder;
00685         vector<unsigned int> *absolute;
00686 >>>>>>> merge rev
00687         int focus_datapoints;
00688         int iris_datapoints;
00689         int zoom_datapoints;
00690 
00691         while(fgets(chunk, sizeof(chunk), LensFile) != NULL) {
00692             size_t len_used = strlen(line);
00693             size_t chunk_used = strlen(chunk);
00694 
00695             if(len - len_used < chunk_used) {
00696                 pc.printf("Line Buffer Exceeded - Failed to read file");
00697                 free(line);
00698                 return;
00699             }
00700 
00701             strncpy(line + len_used, chunk, len - len_used);
00702             len_used += chunk_used;
00703             if(line[len_used - 1] == '\n') {
00704 <<<<<<< working copy
00705                 pc.printf("Retrieved line of length %u: ", len_used);
00706                 pc.printf("%s\n", line);
00707                 if (sscanf(line,"FOCUS:%d", &focus_datapoints) == 1) {
00708                     encoder = &UserSettings.focus_encoder_map;
00709                     absolute = &UserSettings.focus_absolute_map;
00710                     printf("\tSet Focus\n");
00711                 } else if (sscanf(line,"IRIS:%d", &iris_datapoints) == 1) {
00712                     encoder = &UserSettings.iris_encoder_map;
00713                     absolute = &UserSettings.iris_absolute_map;
00714                     printf("\tSet Iris\n");
00715                 } else if (sscanf(line,"ZOOM:%d", &zoom_datapoints) == 1) {
00716                     encoder = &UserSettings.zoom_encoder_map;
00717                     absolute = &UserSettings.zoom_absolute_map;
00718                     printf("\tSet Zoom\n");
00719                 } else if (strchr(line,',') != NULL) {
00720                     pc.printf("\tProbably some data in this line\n");
00721                     int encoder_val;
00722                     float absolute_val;
00723                     char *pt = strtok (line,",");
00724                     if (pt) {
00725                         encoder_val = atoi(pt);
00726                         printf("\tENCODER: %d\n", encoder_val);
00727                     }
00728                     pt = strtok(line, ",");
00729                     if (pt) {
00730                         absolute_val = (float)atof(pt);
00731                         printf("\tABSOLUTE: %f\n", absolute_val);
00732                     }
00733                     if (encoder_val != NULL && absolute_val != NULL) {
00734                         encoder->push_back(encoder_val);
00735                         absolute->push_back(absolute_val);
00736                         printf("\tADDED Datapoint\n");
00737                     }
00738                 } else {
00739                     printf("\tNot Valid matching line\n");
00740                     }
00741 =======
00742                 // pc.printf("Retrieved line of length %u: ", len_used);
00743                 // pc.printf("%s", line);
00744                 // Thread::wait(10);
00745                 if (sscanf(line,"FOCUS:%d", &focus_datapoints) == 1) {
00746                     encoder = &UserSettings.focus_encoder_map;
00747                     absolute = &UserSettings.focus_absolute_map;
00748                     UserSettings.absolute_focus = true;
00749                     // pc.printf(" - Set Focus\r\n");
00750                     // Thread::wait(10);
00751                 } else if (sscanf(line,"IRIS:%d", &iris_datapoints) == 1) {
00752                     encoder = &UserSettings.iris_encoder_map;
00753                     absolute = &UserSettings.iris_absolute_map;
00754                     UserSettings.absolute_iris = true;
00755                     // pc.printf(" - Set Iris\r\n");
00756                     // Thread::wait(10);
00757                 } else if (sscanf(line,"ZOOM:%d", &zoom_datapoints) == 1) {
00758                     encoder = &UserSettings.zoom_encoder_map;
00759                     absolute = &UserSettings.zoom_absolute_map;
00760                     UserSettings.absolute_zoom = true;
00761                     // pc.printf(" - Set Zoom\r\n");
00762                     // Thread::wait(10);
00763                 } else if (strchr(line,',') != NULL) {
00764                     // pc.printf("\tProbably some data in this line\n");
00765                     unsigned int encoder_val;
00766                     unsigned int absolute_val;
00767                     if (sscanf(line, "%d,%d", &encoder_val, &absolute_val) == 2) {
00768                         encoder->push_back(encoder_val);
00769                         absolute->push_back(absolute_val);
00770                         // pc.printf(" - ADDED Datapoint\r\n");
00771                         // Thread::wait(10);
00772                     }
00773                 } // else { pc.printf(" - Not Valid matching line\r\n"); }
00774 >>>>>>> merge rev
00775             line[0] = '\0';
00776             }
00777         }
00778         pc.printf("FOCUS: ");
00779         for(int i = 0; i < UserSettings.focus_encoder_map.size(); i++) {
00780             pc.printf("(%d, ", UserSettings.focus_encoder_map[i]);
00781             pc.printf("%d)", UserSettings.focus_absolute_map[i]);
00782         }
00783         pc.printf("\r\n");
00784         pc.printf("IRIS: ");
00785         for(int i = 0; i < UserSettings.iris_encoder_map.size(); i++) {
00786             pc.printf("(%d, ", UserSettings.iris_encoder_map[i]);
00787             pc.printf("%d)", UserSettings.iris_absolute_map[i]);
00788         }
00789         pc.printf("\r\n");
00790         pc.printf("ZOOM: ");
00791         for(int i = 0; i < UserSettings.zoom_encoder_map.size(); i++) {
00792             pc.printf("(%d, ", UserSettings.zoom_encoder_map[i]);
00793             pc.printf("%d)", UserSettings.zoom_absolute_map[i]);
00794         }
00795         pc.printf("\r\n");
00796         fclose(LensFile);
00797         free(line);
00798     } else {
00799         pc.printf("Unable to open File %s\r\n", filename);
00800     }
00801 }
00802 
00803 void readSettingsFile()
00804 {
00805 
00806     UserSettings.FIZmode = formatPreston;
00807     UserSettings.SerialOutMode = mode_VIPS;
00808     UserSettings.FreeDPort = 0;
00809     UserSettings.DataOutPort = 0;
00810     UserSettings.VipsUDPPort = 0;
00811     UserSettings.SettingsPort = 0;
00812     UserSettings.IPAddress[0] = 0;
00813     UserSettings.Gateway[0] = 0;
00814     UserSettings.Subnet[0] = 0;
00815     UserSettings.FilterOrder = 0;
00816     UserSettings.FilterFreq = 10;
00817     UserSettings.FilterRate = 100;
00818     UserSettings.FilterXY = false;
00819     UserSettings.FilterZ = false;
00820     UserSettings.FilterRoll = false;
00821     UserSettings.FilterPitch = false;
00822     UserSettings.FilterYaw = false;
00823     UserSettings.AutoHyperSmooth = true;
00824     UserSettings.FlexibleVIPSOut = false;
00825     UserSettings.SerialTxDelayMS = 0;
00826     UserSettings.UDPTxDelayMS = 0;
00827     UserSettings.SerialTxDelayFrame = 0;
00828     UserSettings.UDPTxDelayFrame = 0;
00829     UserSettings.InterpolationOffsetFrame = 0;
00830     UserSettings.InterpolationOffset_uS = 0;
00831     UserSettings.InvertRoll = false;
00832     UserSettings.InvertPitch = false;
00833     UserSettings.InvertYaw = false;
00834     UserSettings.OffsetRoll =0;
00835     UserSettings.OffsetPitch =0;
00836     UserSettings.OffsetYaw =0;
00837     UserSettings.bypassBaud = 0;
00838     UserSettings.ForcePPF = 0;
00839     UserSettings.HalfRate = 0;
00840     UserSettings.focus_scale= 1;
00841     UserSettings.focus_offset = 0;
00842     UserSettings.iris_scale = 1;
00843     UserSettings.iris_offset = 0;
00844     UserSettings.zoom_scale = 1;
00845     UserSettings.zoom_offset= 0;
00846     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)
00847     UserSettings.absolute_focus = false;
00848     UserSettings.absolute_iris = false;
00849     UserSettings.absolute_zoom = false;
00850 
00851 //    LocalFileSystem localFS("local");
00852     FILE *LSFile= fopen("/local/settings.txt","r");
00853     char lineBuffer[128];
00854     int valueIn;
00855     float floatIn;
00856     char lensfile[128] = "/local/";//7 characters for filepath, minus 12 chars of settings string, hence 128 shouldnt cause buffer overflow
00857     /*MAXIMUM FILENAME LENGTH IS 8 Characters*/
00858 
00859     if (LSFile) {
00860         while (!feof(LSFile) && !UserSettings.bypassBaud) {
00861             if (fgets(lineBuffer, 128, LSFile)) {
00862                 if (sscanf(lineBuffer,"RadioConfigPassthrough=%d",&valueIn) == 1) {
00863                     pc.printf("Switching to RADIO/USB bypass mode at %d baud\r\n",valueIn);
00864                     UserSettings.bypassBaud = valueIn;
00865                 }
00866 
00867                 if (sscanf(lineBuffer,"Output_Format=%d",&valueIn) == 1) {
00868                     pc.printf("Got Output_Format value from file of %d\r\n",valueIn);
00869                     UserSettings.SerialOutMode = valueIn;
00870                 }
00871                 if (sscanf(lineBuffer,"FIZ_Format=%d",&valueIn) == 1) {
00872                     pc.printf("Got FIZ_Format value from file of %d\r\n",valueIn);
00873                     UserSettings.FIZmode = valueIn;
00874                 }
00875                 if (sscanf(lineBuffer,"FreeD_Port=%d",&valueIn) == 1) {
00876                     pc.printf("Got FreeD_Port value from file of %d\r\n",valueIn);
00877                     UserSettings.FreeDPort = valueIn;
00878                     UserSettings.DataOutPort = valueIn;
00879                 }
00880                 if (sscanf(lineBuffer,"FreeD_Mode=%d",&valueIn) == 1) {
00881                     pc.printf("Got FreeD_Mode value from file of %d\r\n",valueIn);
00882                     UserSettings.FreeDPort = valueIn;
00883                 }
00884                 if (sscanf(lineBuffer,"DataOut_Port=%d",&valueIn) == 1) {
00885                     pc.printf("Got DataOut_Port value from file of %d\r\n",valueIn);
00886                     UserSettings.DataOutPort = valueIn;
00887                 }
00888                 if (sscanf(lineBuffer,"VIPS_UDP_Port=%d",&valueIn) == 1) {
00889                     pc.printf("Got VIPS_Port value from file of %d\r\n",valueIn);
00890                     UserSettings.VipsUDPPort = valueIn;
00891                 }
00892                 if (sscanf(lineBuffer,"Settings_Port=%d",&valueIn) == 1) {
00893                     pc.printf("Got Settings update port value from file of %d\r\n",valueIn);
00894                     UserSettings.SettingsPort = valueIn;
00895                 }
00896                 if (sscanf(lineBuffer,"IP_addr=%s",UserSettings.IPAddress) == 1) {
00897                     pc.printf("Got IP_addr value from file of %s\r\n",UserSettings.IPAddress);
00898                 }
00899                 if (sscanf(lineBuffer,"Subnet=%s",UserSettings.Subnet) == 1) {
00900                     pc.printf("Got Subnet mask value from file of %s\r\n",UserSettings.Subnet);
00901                 }
00902                 if (sscanf(lineBuffer,"Gateway=%s",UserSettings.Gateway) == 1) {
00903                     pc.printf("Got gateway value from file of %s\r\n",UserSettings.Gateway);
00904                 }
00905                 if (sscanf(lineBuffer,"FilterOrder=%d",&valueIn) == 1) {
00906                     pc.printf("Got FilterOrder value from file of %d\r\n",valueIn);
00907                     UserSettings.FilterOrder = valueIn;
00908                 }
00909                 if (sscanf(lineBuffer,"FilterFreq=%f",&floatIn) == 1) {
00910                     pc.printf("Got FilterFreq value from file of %.2f\r\n",floatIn);
00911                     UserSettings.FilterFreq = floatIn;
00912                 }
00913                 if (sscanf(lineBuffer,"FilterRate=%f",&floatIn) == 1) {
00914                     pc.printf("Got FilterRate value from file of %.2f\r\n",floatIn);
00915                     UserSettings.FilterRate = floatIn;
00916                 }
00917                 if (sscanf(lineBuffer,"FilterXY=%d",&valueIn) == 1) {
00918                     pc.printf("Got FilterXY value from file of %d\r\n",valueIn);
00919                     UserSettings.FilterXY = (valueIn==1);
00920                 }
00921                 if (sscanf(lineBuffer,"FilterZ=%d",&valueIn) == 1) {
00922                     pc.printf("Got FilterZ value from file of %d\r\n",valueIn);
00923                     UserSettings.FilterZ = (valueIn==1);
00924                 }
00925                 if (sscanf(lineBuffer,"FilterRoll=%d",&valueIn) == 1) {
00926                     pc.printf("Got FilterRoll value from file of %d\r\n",valueIn);
00927                     UserSettings.FilterRoll = (valueIn==1);
00928                 }
00929                 if (sscanf(lineBuffer,"FilterPitch=%d",&valueIn) == 1) {
00930                     pc.printf("Got FilterPitch value from file of %d\r\n",valueIn);
00931                     UserSettings.FilterPitch = (valueIn==1);
00932                 }
00933                 if (sscanf(lineBuffer,"FilterYaw=%d",&valueIn) == 1) {
00934                     pc.printf("Got FilterYaw value from file of %d\r\n",valueIn);
00935                     UserSettings.FilterYaw = (valueIn==1);
00936                 }
00937                 if (sscanf(lineBuffer,"AutoHyperSmooth=%d",&valueIn) == 1) {
00938                     pc.printf("Got AutoHyperSmooth value from file of %d\r\n",valueIn);
00939                     UserSettings.AutoHyperSmooth = (valueIn==1);
00940                 }
00941                 if (sscanf(lineBuffer,"ExtendedOutput=%d",&valueIn) == 1) {
00942                     pc.printf("Got ExtendedOutput value from file of %d\r\n",valueIn);
00943                     UserSettings.FlexibleVIPSOut = (valueIn==1);
00944                 }
00945                 if (sscanf(lineBuffer,"DelaySerial=%f",&floatIn) == 1) {
00946                     pc.printf("Got serial delay value from file of %.2f ms\r\n",floatIn);
00947                     UserSettings.SerialTxDelayMS = floatIn;
00948                 }
00949                 if (sscanf(lineBuffer,"DelaySerialFrame=%f",&floatIn) == 1) {
00950                     pc.printf("Got serial delay value from file of %.2f frames\r\n",floatIn);
00951                     UserSettings.SerialTxDelayFrame = floatIn;
00952                 }
00953                 if (sscanf(lineBuffer,"DelayUDP=%f",&floatIn) == 1) {
00954                     pc.printf("Got UDP delay value from file of %.2f ms\r\n",floatIn);
00955                     UserSettings.UDPTxDelayMS = floatIn;
00956                 }
00957                 if (sscanf(lineBuffer,"DelayUDPFrame=%f",&floatIn) == 1) {
00958                     pc.printf("Got UDP delay value from file of %.2f frames\r\n",floatIn);
00959                     UserSettings.UDPTxDelayFrame = floatIn;
00960                 }
00961                 if (sscanf(lineBuffer,"ShiftInterpolationByFrame=%f",&floatIn) == 1) {
00962                     pc.printf("Got Interpolation Offset %.2f frames\r\n",floatIn);
00963                     UserSettings.InterpolationOffsetFrame = floatIn;
00964                 }
00965                 if (sscanf(lineBuffer,"ShiftInterpolationByTimeMs=%f",&floatIn) == 1) {
00966                     pc.printf("Got Interpolation offset of %.2f ms\r\n",floatIn);
00967                     UserSettings.InterpolationOffset_uS = floatIn * 1000;
00968                 }
00969                 if (sscanf(lineBuffer,"InvertRoll=%d",&valueIn) == 1) {
00970                     pc.printf("Got InvertRoll value from file of %d\r\n",valueIn);
00971                     UserSettings.InvertRoll = (valueIn==1);
00972                 }
00973                 if (sscanf(lineBuffer,"InvertPitch=%d",&valueIn) == 1) {
00974                     pc.printf("Got InvertPitch value from file of %d\r\n",valueIn);
00975                     UserSettings.InvertPitch = (valueIn==1);
00976                 }
00977                 if (sscanf(lineBuffer,"InvertYaw=%d",&valueIn) == 1) {
00978                     pc.printf("Got InvertYaw value from file of %d\r\n",valueIn);
00979                     UserSettings.InvertYaw = (valueIn==1);
00980                 }
00981                 if (sscanf(lineBuffer,"OffsetRoll=%d",&valueIn) == 1) {
00982                     pc.printf("Got OffsetRoll value from file of %d\r\n",valueIn);
00983                     UserSettings.OffsetRoll = valueIn;
00984                 }
00985                 if (sscanf(lineBuffer,"OffsetPitch=%d",&valueIn) == 1) {
00986                     pc.printf("Got OffsetPitch value from file of %d\r\n",valueIn);
00987                     UserSettings.OffsetPitch = valueIn;
00988                 }
00989                 if (sscanf(lineBuffer,"OffsetYaw=%d",&valueIn) == 1) {
00990                     pc.printf("Got OffsetYaw value from file of %d\r\n",valueIn);
00991                     UserSettings.OffsetYaw = valueIn;
00992                 }
00993                 if (sscanf(lineBuffer,"ForcePPF=%d",&valueIn) == 1) {
00994                     pc.printf("Using Forced PPF\r\n",valueIn);
00995                     UserSettings.ForcePPF = valueIn;
00996                 }
00997                 if (sscanf(lineBuffer,"HalfSerialRate=%d",&valueIn) == 1) {
00998                     pc.printf("Halving Serial rate output \r\n",valueIn);
00999                     UserSettings.HalfRate = valueIn;
01000                 }
01001                 if (sscanf(lineBuffer,"FocusScale=%f",&floatIn) == 1) {
01002                     pc.printf("Got Focus scale of %f \r\n",floatIn);
01003                     UserSettings.focus_scale = floatIn;
01004                 }
01005                 if (sscanf(lineBuffer,"FocusOffset=%f",&floatIn) == 1) {
01006                     pc.printf("Got Focus offset of %f \r\n",floatIn);
01007                     UserSettings.focus_offset = floatIn;
01008                 }
01009                 if (sscanf(lineBuffer,"IrisScale=%f",&floatIn) == 1) {
01010                     pc.printf("Got Iris scale of %f \r\n",floatIn);
01011                     UserSettings.iris_scale = floatIn;
01012                 }
01013                 if (sscanf(lineBuffer,"IrisOffset=%f",&floatIn) == 1) {
01014                     pc.printf("Got Iris offset of %f \r\n",floatIn);
01015                     UserSettings.iris_offset = floatIn;
01016                 }
01017                 if (sscanf(lineBuffer,"ZoomScale=%f",&floatIn) == 1) {
01018                     pc.printf("Got Zoom Scale of %f \r\n",floatIn);
01019                     UserSettings.zoom_scale = floatIn;
01020                 }
01021                 if (sscanf(lineBuffer,"ZoomOffset=%f",&floatIn) == 1) {
01022                     pc.printf("Got Zoom offset of %f \r\n",floatIn);
01023                     UserSettings.zoom_offset = floatIn;
01024                 }
01025                 if (sscanf(lineBuffer,"LensProfile=%s", &lensfile[7]) == 1) {
01026                     pc.printf("Found filename of %s \r\n", lensfile);
01027                 }
01028 
01029             }
01030         }
01031         fclose(LSFile);
01032         readLensFile(lensfile);
01033     } else {
01034         pc.printf("No Settings File Found \r\n");
01035     }
01036 }
01037 
01038 
01039 void settingsNetUpdate(void const* netInterface)
01040 {
01041     //FILE *LSFile;
01042     const int rxBufferSize = MaxBuffSize; // making this 1024 causes a crash. Probably out of memory somewhere.
01043     //char settingsOutBuffer[rxBufferSize];
01044     char settingsInBuffer[rxBufferSize];
01045     int bytesToSend = 0;
01046     unsigned char *bufferToSend;
01047     bool validated_connection = false;
01048 
01049     Thread::wait(1000);
01050     pc.puts("Starting TCP network listen server\r\n");
01051     Thread::wait(50);
01052 
01053 
01054 //    int fileSize = 0;
01055     TCPSocketServer server;
01056     server.bind(UserSettings.SettingsPort);
01057     server.set_blocking(false, 1000);
01058     server.listen();
01059 
01060     TCPSocketConnection connection;
01061     pc.puts("TCP thread waiting for connection\r\n");
01062 
01063     while (!StopTCPListening) {
01064         server.accept(connection);
01065         if (connection.is_connected()) {
01066             pc.puts("TCP Connected\r\n");
01067             connection.set_blocking(false,50);
01068 
01069             int bytesIn = 0;
01070             do {
01071                 bytesIn = connection.receive(settingsInBuffer,rxBufferSize);
01072                 if (bytesIn > 0) {
01073                     if (!validated_connection) {
01074                         printf("Recieved %d bytes on unvalidated connection\r\n",bytesIn);
01075                         if ((settingsInBuffer[0] == 0x07) &&
01076                                 (settingsInBuffer[1] == 0x05) &&
01077                                 (settingsInBuffer[2] == 0x02)) {
01078                             validated_connection = true;
01079                             printf("Validated Connection - Sending 'set quiet' command\r\n");
01080                             VIPS.sendQuiet();
01081                             Thread::wait(50);
01082                             VIPS.EnableDirectTX(true);
01083                             GreenLED = LED_ON;
01084                             VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
01085                             printf("Sent first %d bytes to VIPS\r\n",bytesIn); //also a set-quiet command but good to send twice
01086                         } else {
01087                             printf("Invalid: %X %X %X", settingsInBuffer[0], settingsInBuffer[1], settingsInBuffer[2]);
01088                         }
01089                     } else if (validated_connection) {
01090                         VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
01091                         printf("Sent %d bytes to VIPS\r\n",bytesIn);
01092                     }
01093                 }
01094                 if (VIPS.getWaitingBuffer(&bufferToSend, &bytesToSend)) {
01095                     connection.send((char *)bufferToSend, bytesToSend);
01096                     printf("Recieved %d bytes from VIPS\r\n",bytesIn);
01097                 }
01098             } while (connection.is_connected());
01099 
01100             VIPS.EnableDirectTX(false);
01101             GreenLED = LED_OFF;
01102             validated_connection = false;
01103             pc.puts("Disconnected TCP \r\n");
01104             connection.close();
01105             Thread::wait(50);
01106         }
01107     }
01108     VIPS.EnableDirectTX(false);
01109     GreenLED = LED_OFF;
01110     pc.puts("Ending TCP Task\r\n");
01111 }
01112 
01113 
01114 void ethernetTask(void const* settings)
01115 {
01116 
01117     Thread* ListenTask=NULL;
01118     UDPSocket* PrimaryDataSocket = NULL;
01119     Endpoint PrimaryDataTarget;
01120 
01121     UDPSocket* VIPSSocket = NULL;
01122     Endpoint VIPSTarget;
01123 
01124     UserSettings_t *settingsPtr = (UserSettings_t *) settings;
01125 
01126     pc.puts("Ethernet task startup\r\n");
01127     if (!settingsPtr->DataOutPort && !settingsPtr->VipsUDPPort && !settingsPtr->SettingsPort) {
01128         pc.puts("No ports set. Ethernet task exiting\r\n");
01129         return;
01130     }
01131     int Result = -1;
01132     while (Result != 0) {
01133         if ((UserSettings.IPAddress[0] != 0) && (UserSettings.Gateway[0] != 0) && (UserSettings.Subnet[0] != 0)) {
01134             pc.puts("Static IP init attempt\r\n");
01135             Result = eth.init(UserSettings.IPAddress, UserSettings.Subnet, UserSettings.Gateway);
01136         } else {
01137             pc.puts("Dynamic IP init attempt\r\n");
01138             Result = eth.init();
01139         }
01140 
01141         if (Result) {
01142             pc.puts("Ethernet init failed\r\n");
01143             Thread::wait(500);
01144         }
01145     }
01146     pc.puts("Ethernet init complete\r\n");
01147 
01148     while (true) {
01149 
01150         Result = -1;
01151         while (Result != 0) {
01152             Result = eth.connect();
01153             if (Result) {
01154                 pc.puts("Ethernet connect failed\r\n");
01155                 Thread::wait(500);
01156             }
01157         }
01158         pc.puts("Ethernet connect complete\r\n");
01159         Thread::wait(100);
01160         printf("IP Address is %s\r\n", eth.getIPAddress());
01161         if (ListenTask)
01162             delete ListenTask;
01163         if (settingsPtr->SettingsPort) {
01164             pc.puts("Starting network update task\r\n");
01165             Thread::wait(100);
01166             StopTCPListening = false;
01167             ListenTask = new Thread(settingsNetUpdate, NULL, osPriorityHigh, 256 * 4);
01168         }
01169         if (PrimaryDataSocket)
01170             delete PrimaryDataSocket;
01171         if (UserSettings.DataOutPort) {
01172             pc.printf("Starting Primary data socket - %s\r\n", (UserSettings.FreeDPort ? "FreeD Output Mode" : "VIPS Output Mode"));
01173             Thread::wait(50);
01174             PrimaryDataSocket = new UDPSocket();
01175             PrimaryDataSocket->init();
01176             PrimaryDataSocket->set_broadcasting(true);
01177             PrimaryDataTarget.set_address(0xFFFFFFFF, UserSettings.DataOutPort);
01178             int option = (1);
01179             int tos_option = (IPTOS_LOWDELAY|IPTOS_THROUGHPUT|IPTOS_PREC_IMMEDIATE);
01180 //            PrimaryDataSocket->get_option(IPPROTO_IP, IP_TOS, &option, sizeof(option))
01181 //            pc.printf("Socket IPTOS Option: %d\r\n", option);
01182             PrimaryDataSocket->set_option(IPPROTO_IP, IP_TOS, &tos_option, sizeof(option));
01183 //            PrimaryDataSocket->set_option(IPPROTO_TCP, TCP_NODELAY, &option, sizeof(option));
01184 //            PrimaryDataSocket->set_option(SOL_SOCKET, SO_NO_CHECK, &option, sizeof(option));
01185 
01186         }
01187 
01188         if (VIPSSocket)
01189             delete VIPSSocket;
01190         if (UserSettings.VipsUDPPort) {
01191             pc.puts("Starting VIPS socket\r\n");
01192             Thread::wait(50);
01193             VIPSSocket = new UDPSocket();
01194             VIPSSocket->init();
01195             VIPSSocket->set_broadcasting(true);
01196             VIPSTarget.set_address(0xFFFFFFFF, UserSettings.VipsUDPPort);
01197         }
01198         EthernetTimer.reset();
01199         EthernetTimer.start();
01200         pc.puts("Network ready for TX\r\n");
01201         Thread::wait(50);
01202         while (true) {
01203             if (EthTxNow) {
01204 //                NewData.lock();
01205                 EthTxNow = false;
01206                 if (!PrimaryDataSocket && UserSettings.DataOutPort) {
01207                     pc.puts("No FreeD UDP socket\r\n");
01208                     break;
01209                 }
01210                 if (!VIPSSocket && UserSettings.VipsUDPPort) {
01211                     pc.puts("No VIPS UDP socket\r\n");
01212                     break;
01213                 }
01214                 if (PrimaryDataSocket) {
01215                     if (UserSettings.FreeDPort)
01216                         Result = PrimaryDataSocket->sendTo(PrimaryDataTarget,(char *)&fdPacket, sizeof(struct D1MsgFormat_s));
01217                     else
01218                         Result = PrimaryDataSocket->sendTo(PrimaryDataTarget,(char *)FlexibleVIPSOut, VIPSOutSize);
01219                     //uint32_t ethtime = EthernetTimer.read_us();
01220                     //EthernetTimer.reset();
01221                     int expected_size = UserSettings.FreeDPort ? sizeof(struct D1MsgFormat_s) :  VIPSOutSize;
01222                     if (Result != expected_size) {
01223                         pc.puts("Primary Port UDP Tx Failed!\r\n");
01224                         break;
01225                     } //else
01226                     //pc.printf("%d\r\n", ethtime);
01227                 }
01228                 if (VIPSSocket) {
01229                     Result = VIPSSocket->sendTo(VIPSTarget,(char *)FlexibleVIPSOut, VIPSOutSize);
01230                     if (Result != VIPSOutSize) {
01231                         pc.puts("VIPS UDP Tx Failed!\r\n");
01232                         break;
01233                     }
01234                 }
01235 //                pc.puts("UDP Tx\r\n");
01236             } else
01237 //                NewData.unlock();
01238                 Thread::wait(1);
01239         }
01240         StopTCPListening = true;
01241         Thread::wait(1);
01242 
01243         while (ListenTask->get_state()) {
01244             Thread::wait(50);
01245         }
01246 //        int ListenState = ListenTask->get_state();
01247 //        pc.printf("Listen State: %d\r\n", ListenState);
01248 
01249         eth.disconnect();
01250         pc.puts("Attempting to restart network\r\n");
01251     }
01252     pc.puts("Eth Task ending. Should never happen!\r\n");
01253 }
01254 
01255 
01256 void XBEEBypassmode(int baud)
01257 {
01258     pc.baud(baud);
01259 
01260     /*
01261         COM1.baud(9600);
01262         COM1.puts("+++");
01263         wait(1.2);
01264         switch (baud) {
01265             case 1200:
01266                 COM1.puts("ATBD0\n");
01267                 break;
01268             case 2400:
01269                 COM1.puts("ATBD1\n");
01270                 break;
01271             case 4800:
01272                 COM1.puts("ATBD2\n");
01273                 break;
01274             case 9600:
01275                 COM1.puts("ATBD3\n");
01276                 break;
01277             case 19200:
01278                 COM1.puts("ATBD4\n");
01279                 break;
01280             case 38400:
01281                 COM1.puts("ATBD5\n");
01282                 break;
01283             case 57600:
01284                 COM1.puts("ATBD6\n");
01285                 break;
01286             case 115200:
01287                 COM1.puts("ATBD7\n");
01288                 break;
01289             default:
01290                 COM1.printf("ATBD%d\n",baud);
01291                 break;
01292         }
01293 
01294         wait(0.1);
01295         COM1.puts("ATCN\n");
01296         wait(0.5);
01297     */
01298 
01299     COM1.baud(baud);
01300     while (true) {
01301         if (pc.readable()) {
01302             char commandIn = pc.getc();
01303             COM1.putc(commandIn);
01304         }
01305         if (COM1.readable())
01306             pc.putc(COM1.getc());
01307     }
01308 }
01309 
01310 int main()
01311 {
01312     pc.baud(115200);
01313     pc.printf("\r\n\r\nStartup - v%.3f\r\n", APP_VERSION); //Change via Line 1
01314     setRED();
01315 
01316     readSettingsFile();
01317 
01318     if (UserSettings.bypassBaud) {
01319         XBEEBypassmode(UserSettings.bypassBaud);
01320     }
01321 
01322     switch(UserSettings.FIZmode) {
01323         case formatPreston :
01324             FIZPort = new FIZDisney(p9, p10);
01325             pc.printf("Set Preston");
01326             break;
01327         case formatFujiPassive :
01328             FIZPort = new FIZDigiPower(p9, p10);
01329             pc.printf("Set FujiPassive");
01330             break;
01331         case formatFujiActive :
01332             FIZPort = new FIZDigiPowerActive(p9, p10);
01333             pc.printf("Set FujiActive\r\n");
01334             break;
01335         case formatCanon :
01336             FIZPort = new FIZCanon(p9, p10);
01337             pc.printf("Set Canon\r\n");
01338             break;
01339         case formatArri:
01340             FIZPort = new FIZ_ArriCmotion(p9, p10);
01341             pc.printf("Set Arri\r\n");
01342             break;
01343         default:
01344             FIZPort = new FIZDisney(p9, p10); //preston
01345             pc.printf("Set Default - Preston");
01346 
01347     }
01348 //    FIZPort = new FIZDigiPowerActive(p9, p10);
01349     COM1.baud(115200); // radio port
01350 
01351     Thread ethernetThread(ethernetTask, &UserSettings, osPriorityHigh, 256 * 4);
01352     led1 = 0;
01353     inputTimer.reset();
01354     inputTimer.start();
01355 
01356     prepPacketOut();
01357 
01358     LTCInput.enable(true);
01359 
01360     if (!VIPS.setFilters(&UserSettings))
01361         pc.puts("Failed to create VIPS filters\r\n");
01362 
01363     VIPS.run();
01364 
01365     pc.printf("System init complete\r\n");
01366     setBLUE();
01367     COM1.attach(&onOutputSerialRx);
01368 
01369     TimeSinceLastFrame.reset();
01370     TimeSinceLastFrame.start();
01371 
01372 
01373     pc.printf("Waiting for sync input clock\r\n");
01374 
01375 #ifdef enableFakePPF
01376     FakePPF.attach(callback(&OnPPFInputStartup),1/50.0);
01377 #endif
01378 
01379     PPFin.rise(callback(&OnPPFInputStartup));
01380     Syncin.rise(callback(&OnSyncInputStartup));
01381     framesIn = 0;
01382     SyncInCount = 0;
01383     bool LockToSync = false;
01384 
01385     while (true) {
01386         if (SyncInCount == 100) {
01387             LockToSync= true;
01388             break;
01389         }
01390         if ((framesIn == 100) && (SyncInCount<45)) { // prefer frame sync
01391             break;
01392         }
01393     }
01394 
01395     if (LockToSync && !UserSettings.ForcePPF) {
01396         pc.printf("Using Genlock sync input\r\n");
01397         Syncin.rise(callback(&OnPPFInputStartup));
01398         PPFin.rise(NULL);
01399     } else {
01400         pc.printf("Using pulse per frame input\r\n");
01401         Syncin.rise(NULL);
01402     }
01403 
01404     setGREEN();
01405     pc.printf("Measuring frame rate\r\n");
01406 
01407     int currentFrames = framesIn;
01408     while (framesIn == currentFrames);  // wait for next frame;
01409     inputTimer.reset();
01410     framesIn = 0;
01411 
01412 
01413     while (framesIn < 100);  // wait for 100 frames;
01414     uint32_t frameTime = inputTimer.read_us()/100;
01415 
01416     int framesPerSecond = frameRates::getClosestRate(frameTime);
01417 
01418     pc.printf("Detected frame rate %d\r\n",framesPerSecond);
01419     if (UserSettings.SerialTxDelayFrame != 0) {
01420         UserSettings.SerialTxDelayMS = (UserSettings.SerialTxDelayFrame*1000)/framesPerSecond;
01421         pc.printf("Serial delay of %.2f frames calculated as %.2f ms\r\n",UserSettings.SerialTxDelayFrame,UserSettings.SerialTxDelayMS);
01422     }
01423     if (UserSettings.UDPTxDelayFrame != 0) {
01424         UserSettings.UDPTxDelayMS = (UserSettings.UDPTxDelayFrame*1000)/framesPerSecond;
01425         pc.printf("Serial delay of %.2f frames calculated as %.2f ms\r\n",UserSettings.UDPTxDelayFrame,UserSettings.UDPTxDelayMS);
01426     }
01427     if (UserSettings.InterpolationOffsetFrame != 0) {
01428         UserSettings.InterpolationOffset_uS = (UserSettings.InterpolationOffsetFrame*1000000)/framesPerSecond;
01429         pc.printf("Interpolation Delay of %.2f frames calculated as %.2f ms\r\n", UserSettings.InterpolationOffsetFrame,UserSettings.InterpolationOffset_uS);
01430     }
01431 
01432     if (LTCInput.synced()) { // getting LTC so set the clock.
01433         currentFrames = framesIn;
01434         while (framesIn == currentFrames);  // wait for next frame;
01435         wait_us (frameTime/4); // ensure the LTC decode for the last frame is done.
01436         int hour,minute,second,frame;
01437         bool drop = LTCInput.isFrameDrop();
01438         LTCInput.getTime(&hour,&minute,&second,&frame);
01439         movieTime.setMode(framesPerSecond,drop);
01440         movieTime.setTime(hour,minute,second,frame);
01441     } else { // no time code so clock time doesn't matter
01442         movieTime.setMode(framesPerSecond,false);
01443     }
01444 
01445     if (LockToSync)
01446         Syncin.rise(callback(&OnPPFInput));
01447     else
01448         PPFin.rise(callback(&OnPPFInput));
01449 
01450 #ifdef enableFakePPF
01451     FakePPF.attach(callback(&OnPPFInput),1/50.0);
01452 #endif
01453 
01454     pc.printf("Current time is %02d:%02d:%02d %02d\r\n",movieTime.hours(),movieTime.minutes(),movieTime.seconds(),movieTime.frame());
01455     LTCInput.enable(false);
01456     setOFF();
01457 
01458     pc.printf("Running\r\n",framesPerSecond);
01459 
01460 
01461     getNextFileNumber();
01462     COM1.attach(callback(onOutputSerialRx));
01463 
01464 
01465     while (true) {
01466         if (NewFramePulse) { // New frame. Output data.
01467             //framePulse(FramePulseTime);
01468             NewFramePulse = false;
01469             framePositionOutput();
01470         }
01471         VIPS.sendQueued(); // should ideally be called on 100Hz PPS edge but we don't have that in this code...
01472 
01473         // if (pos_value) {
01474         //     pc.printf("Z: %d - (%d, %d)\r\n", pos_value, pos_lower, pos_upper);
01475         //     pos_value = 0;
01476         // }
01477 
01478         if (logButtonLastState != logButton) {
01479             logButtonLastState = logButton;
01480             if (logButtonLastState) { // pressed
01481                 logButtonDownTime = inputTimer.read();
01482                 if ((logButtonDownTime - logButtonUpTime) > 0.2f) {
01483 //                    pc.puts("Down\r\n");
01484                     resetTimeout.attach(callback(&OnResetTimeout),5);
01485                     if (logging) {
01486                         //           pc.puts("Logging off\r\n");
01487                         GreenLED = LED_OFF;
01488                         logging=false;
01489                         fclose(logFile);
01490                     } else {
01491                         logFile = nextBinaryFile();
01492                         if(logFile) {
01493                             //                pc.puts("Logging on\r\n");
01494                             GreenLED = LED_ON;
01495                             logging = true;
01496                         }
01497                         //else         pc.puts("Logging failed\r\n");
01498                     }
01499                 }
01500             } else { // released
01501                 logButtonUpTime = inputTimer.read();
01502                 resetTimeout.attach(NULL,0);
01503                 if ((logButtonUpTime-logButtonDownTime) > 0.05f) {
01504                     //        pc.puts("Up\r\n");
01505                 }
01506             }
01507         }
01508     }
01509 }