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@6:61274e214f46, 2021-02-09 (annotated)

Committer:
AndyA
Date:
Tue Feb 09 16:55:52 2021 +0000
Revision:
6:61274e214f46
Parent:
4:6cd904bff1bd
Child:
7:87aea27cc68b
Fix FIZ stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 0:97661408d0f9 1 #include "mbed.h"
AndyA 0:97661408d0f9 2 #include "LTCApp.h"
AndyA 0:97661408d0f9 3
AndyA 0:97661408d0f9 4 const int framesToCount = 300;
AndyA 0:97661408d0f9 5 const int MaxTimeErrorUS = 150;
AndyA 0:97661408d0f9 6 const int timerOverheadTime = 19;
AndyA 0:97661408d0f9 7
AndyA 0:97661408d0f9 8 BufferedSerial pc(USBTX, USBRX);
AndyA 0:97661408d0f9 9 VIPSSerial VIPS(p28, p27);
AndyA 0:97661408d0f9 10 BufferedSerial COM1(p13, p14);
AndyA 3:14d241e29be3 11 FIZReader FIZPort(p9, p10);
AndyA 0:97661408d0f9 12
AndyA 0:97661408d0f9 13 DigitalOut led1(LED1);
AndyA 0:97661408d0f9 14 DigitalOut PPS(p12);
AndyA 0:97661408d0f9 15
AndyA 0:97661408d0f9 16 DigitalOut led2(LED2);
AndyA 0:97661408d0f9 17 DigitalOut led3(LED3);
AndyA 0:97661408d0f9 18
AndyA 0:97661408d0f9 19 DigitalOut frameToggle(LED4);
AndyA 0:97661408d0f9 20 DigitalOut second(p23);
AndyA 0:97661408d0f9 21
AndyA 0:97661408d0f9 22 LTCDecode LTCInput(p7);
AndyA 0:97661408d0f9 23 InterruptIn PPFin(p29);
AndyA 0:97661408d0f9 24 InterruptIn Syncin(p8);
AndyA 0:97661408d0f9 25
AndyA 0:97661408d0f9 26 // clock to time everything with
AndyA 0:97661408d0f9 27 Timer inputTimer;
AndyA 0:97661408d0f9 28
AndyA 0:97661408d0f9 29 // Time since last frame event, used for position output interpolation
AndyA 0:97661408d0f9 30 Timer TimeSinceLastFrame;
AndyA 0:97661408d0f9 31 uint32_t TimeSinceLastFrameWrap;
AndyA 0:97661408d0f9 32
AndyA 0:97661408d0f9 33 // used to start PPS at the correct point
AndyA 0:97661408d0f9 34 Timeout PPSsyncTimer;
AndyA 0:97661408d0f9 35
AndyA 0:97661408d0f9 36 // used to generate PPS edges
AndyA 0:97661408d0f9 37 Ticker PPSOutputTimer;
AndyA 0:97661408d0f9 38
AndyA 0:97661408d0f9 39 frameRates detectedRate;
AndyA 0:97661408d0f9 40 bool ppsRunning = false; // set when PPS start has been scheduled
AndyA 0:97661408d0f9 41 volatile bool ppsActive = false; // set when PPS is actuallt going (up to 1 second later)
AndyA 0:97661408d0f9 42
AndyA 0:97661408d0f9 43 volatile bool PPSHigh;
AndyA 0:97661408d0f9 44 bool resync = false;
AndyA 0:97661408d0f9 45 bool resyncDone = false;
AndyA 0:97661408d0f9 46 uint32_t resyncPeriod;
AndyA 0:97661408d0f9 47 bool OKToCheckSync = false;
AndyA 0:97661408d0f9 48 volatile uint32_t VBOXTicks = 0; // time at the NEXT PPS edge
AndyA 0:97661408d0f9 49 uint32_t lastPPSSecondStart;
AndyA 0:97661408d0f9 50
AndyA 0:97661408d0f9 51 #define _longPPMTrackLen_ 20
AndyA 0:97661408d0f9 52 float PPMErrors[_longPPMTrackLen_];
AndyA 0:97661408d0f9 53 float PPMTrackTotal;
AndyA 0:97661408d0f9 54 int PPMTrackIndex;
AndyA 0:97661408d0f9 55 float PPMHighAcc;
AndyA 0:97661408d0f9 56 float remainingClockError;
AndyA 0:97661408d0f9 57 bool ppmCorrection;
AndyA 0:97661408d0f9 58
AndyA 1:dd1f7e162f91 59 struct outputFormat_s {
AndyA 1:dd1f7e162f91 60 uint32_t header; // 2 byte header + 2 byte length
AndyA 1:dd1f7e162f91 61 uint32_t mask;
AndyA 1:dd1f7e162f91 62 uint32_t time;
AndyA 1:dd1f7e162f91 63 double x;
AndyA 1:dd1f7e162f91 64 double y;
AndyA 1:dd1f7e162f91 65 float z;
AndyA 1:dd1f7e162f91 66 float roll;
AndyA 1:dd1f7e162f91 67 float pitch;
AndyA 1:dd1f7e162f91 68 float yaw;
AndyA 2:a79201e302d7 69 uint8_t accuracy[4];
AndyA 3:14d241e29be3 70 uint32_t focus;
AndyA 3:14d241e29be3 71 uint16_t iris;
AndyA 3:14d241e29be3 72 uint16_t zoom;
AndyA 1:dd1f7e162f91 73 uint16_t checksum;
AndyA 1:dd1f7e162f91 74 } __attribute__((packed)) ;
AndyA 0:97661408d0f9 75
AndyA 1:dd1f7e162f91 76 struct outputFormat_s packetOut;
AndyA 1:dd1f7e162f91 77
AndyA 1:dd1f7e162f91 78 void prepPacketOut()
AndyA 1:dd1f7e162f91 79 {
AndyA 1:dd1f7e162f91 80 uint8_t bytes[4];
AndyA 1:dd1f7e162f91 81 bytes[0]=0x24;
AndyA 3:14d241e29be3 82 bytes[1]=0xd9;
AndyA 1:dd1f7e162f91 83 *(uint16_t*)(bytes+2) = sizeof(struct outputFormat_s);
AndyA 1:dd1f7e162f91 84 packetOut.header = *(uint32_t*)bytes;
AndyA 3:14d241e29be3 85 packetOut.mask = 0x0444;
AndyA 3:14d241e29be3 86 packetOut.accuracy[0] = 0;
AndyA 3:14d241e29be3 87 packetOut.accuracy[1] = 0;
AndyA 3:14d241e29be3 88 packetOut.accuracy[2] = 0;
AndyA 3:14d241e29be3 89 packetOut.accuracy[3] = 0;
AndyA 1:dd1f7e162f91 90 }
AndyA 1:dd1f7e162f91 91
AndyA 1:dd1f7e162f91 92 void sendPosition(position *posPtr)
AndyA 1:dd1f7e162f91 93 {
AndyA 0:97661408d0f9 94 if (posPtr) {
AndyA 1:dd1f7e162f91 95 packetOut.time = posPtr->time;
AndyA 1:dd1f7e162f91 96 packetOut.x = posPtr->X;
AndyA 1:dd1f7e162f91 97 packetOut.y = posPtr->Y;
AndyA 1:dd1f7e162f91 98 packetOut.z = posPtr->Height;
AndyA 1:dd1f7e162f91 99 packetOut.roll = posPtr->roll;
AndyA 1:dd1f7e162f91 100 packetOut.pitch = posPtr->pitch;
AndyA 1:dd1f7e162f91 101 packetOut.yaw = posPtr->yaw;
AndyA 2:a79201e302d7 102 packetOut.accuracy[3] = posPtr->ID;
AndyA 6:61274e214f46 103 } else {
AndyA 6:61274e214f46 104 packetOut.time = 0;
AndyA 6:61274e214f46 105 packetOut.x = 0;
AndyA 6:61274e214f46 106 packetOut.y = 0;
AndyA 6:61274e214f46 107 packetOut.z = 0;
AndyA 6:61274e214f46 108 packetOut.roll = 0;
AndyA 6:61274e214f46 109 packetOut.pitch = 0;
AndyA 6:61274e214f46 110 packetOut.yaw = 0;
AndyA 6:61274e214f46 111 packetOut.accuracy[3] = 0;
AndyA 0:97661408d0f9 112 }
AndyA 6:61274e214f46 113 FIZPort.getMostRecent(&packetOut.focus, &packetOut.iris, &packetOut.zoom);
AndyA 6:61274e214f46 114 VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
AndyA 6:61274e214f46 115 COM1.write(&packetOut, sizeof(struct outputFormat_s));
AndyA 0:97661408d0f9 116 }
AndyA 0:97661408d0f9 117
AndyA 0:97661408d0f9 118
AndyA 1:dd1f7e162f91 119 void OnPPSEdge()
AndyA 1:dd1f7e162f91 120 {
AndyA 1:dd1f7e162f91 121 if (PPSHigh) {
AndyA 1:dd1f7e162f91 122 PPS = 0;
AndyA 1:dd1f7e162f91 123 led3=1;
AndyA 1:dd1f7e162f91 124 PPSHigh = false;
AndyA 1:dd1f7e162f91 125 } else {
AndyA 1:dd1f7e162f91 126 led3=0;
AndyA 1:dd1f7e162f91 127 PPS = 1;
AndyA 1:dd1f7e162f91 128 PPSHigh = true;
AndyA 0:97661408d0f9 129
AndyA 1:dd1f7e162f91 130 if (resync && !ppmCorrection) {
AndyA 0:97661408d0f9 131 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 132 PPSOutputTimer.attach_us(callback(OnPPSEdge), resyncPeriod);
AndyA 1:dd1f7e162f91 133 resync = false;
AndyA 1:dd1f7e162f91 134 resyncDone = true;
AndyA 1:dd1f7e162f91 135 } else if (resyncDone) {
AndyA 1:dd1f7e162f91 136 resyncDone = false;
AndyA 0:97661408d0f9 137 PPSOutputTimer.detach();
AndyA 0:97661408d0f9 138 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 139 remainingClockError = 0;
AndyA 0:97661408d0f9 140 }
AndyA 0:97661408d0f9 141
AndyA 1:dd1f7e162f91 142 if ((VBOXTicks % 100) == 0) {
AndyA 1:dd1f7e162f91 143 lastPPSSecondStart = inputTimer.read_us();
AndyA 1:dd1f7e162f91 144 second = 1;
AndyA 1:dd1f7e162f91 145 remainingClockError+=PPMHighAcc;
AndyA 1:dd1f7e162f91 146 int adjustment = (int)(remainingClockError/2);
AndyA 1:dd1f7e162f91 147 if (!resyncDone && ((adjustment > 2)|| (adjustment <-2))) {
AndyA 1:dd1f7e162f91 148 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 149 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000+adjustment-timerOverheadTime);
AndyA 1:dd1f7e162f91 150 ppmCorrection=true;
AndyA 1:dd1f7e162f91 151 remainingClockError-=2*adjustment;
AndyA 1:dd1f7e162f91 152 }
AndyA 1:dd1f7e162f91 153 } else {
AndyA 1:dd1f7e162f91 154 if ((VBOXTicks % 100) == 1) {
AndyA 1:dd1f7e162f91 155 second = 0;
AndyA 1:dd1f7e162f91 156 if (ppmCorrection) {
AndyA 1:dd1f7e162f91 157 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 158 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 159 ppmCorrection=false;
AndyA 1:dd1f7e162f91 160 }
AndyA 1:dd1f7e162f91 161 }
AndyA 1:dd1f7e162f91 162 }
AndyA 1:dd1f7e162f91 163
AndyA 1:dd1f7e162f91 164 VBOXTicks++;
AndyA 1:dd1f7e162f91 165 if (VBOXTicks >= (24 * 60 * 60 * 100))
AndyA 1:dd1f7e162f91 166 VBOXTicks -= (24 * 60 * 60 * 100);
AndyA 1:dd1f7e162f91 167 }
AndyA 0:97661408d0f9 168 }
AndyA 0:97661408d0f9 169
AndyA 1:dd1f7e162f91 170 void OnPPSCync()
AndyA 1:dd1f7e162f91 171 {
AndyA 1:dd1f7e162f91 172 PPS = 1;
AndyA 1:dd1f7e162f91 173 led3=0;
AndyA 1:dd1f7e162f91 174 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 175 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 176 PPSHigh = true;
AndyA 1:dd1f7e162f91 177 VBOXTicks++;
AndyA 1:dd1f7e162f91 178 ppsActive = true;
AndyA 1:dd1f7e162f91 179 led1 = 1;
AndyA 0:97661408d0f9 180 }
AndyA 0:97661408d0f9 181
AndyA 1:dd1f7e162f91 182 const char *ticksToTime(unsigned long vboxTickCount)
AndyA 1:dd1f7e162f91 183 {
AndyA 1:dd1f7e162f91 184 static char timeString[32];
AndyA 1:dd1f7e162f91 185 int hours = vboxTickCount / 360000;
AndyA 1:dd1f7e162f91 186 int minutes = (vboxTickCount / 6000) % 60;
AndyA 1:dd1f7e162f91 187 int seconds = (vboxTickCount / 100) % 60;
AndyA 1:dd1f7e162f91 188 int ms = 10 * (vboxTickCount % 100);
AndyA 1:dd1f7e162f91 189 sprintf(timeString, "%02d:%02d:%02d.%03d", hours, minutes, seconds, ms);
AndyA 1:dd1f7e162f91 190 return timeString;
AndyA 0:97661408d0f9 191 }
AndyA 0:97661408d0f9 192
AndyA 1:dd1f7e162f91 193 bool isStartOfSecond(int minutes, int seconds, int frame, bool dropFrame)
AndyA 1:dd1f7e162f91 194 {
AndyA 1:dd1f7e162f91 195 if (frame == 0)
AndyA 1:dd1f7e162f91 196 return true;
AndyA 1:dd1f7e162f91 197 if (dropFrame && (2 == frame) && (0 == seconds) && (minutes % 10 != 0))
AndyA 1:dd1f7e162f91 198 return true;
AndyA 1:dd1f7e162f91 199 return false;
AndyA 0:97661408d0f9 200 }
AndyA 0:97661408d0f9 201
AndyA 0:97661408d0f9 202
AndyA 1:dd1f7e162f91 203 int getClosestRateIndex(long int framePeriodUS)
AndyA 1:dd1f7e162f91 204 {
AndyA 1:dd1f7e162f91 205 int indexOver = 1;
AndyA 1:dd1f7e162f91 206 while (framePeriodUS <= frameRateInfo::FramePeriods[indexOver])
AndyA 1:dd1f7e162f91 207 indexOver++;
AndyA 1:dd1f7e162f91 208 float amountOver = framePeriodUS - frameRateInfo::FramePeriods[indexOver];
AndyA 1:dd1f7e162f91 209 float amountUnder = frameRateInfo::FramePeriods[indexOver - 1] - framePeriodUS;
AndyA 1:dd1f7e162f91 210 if (amountOver > amountUnder)
AndyA 1:dd1f7e162f91 211 return indexOver - 1;
AndyA 1:dd1f7e162f91 212 return indexOver;
AndyA 0:97661408d0f9 213 }
AndyA 0:97661408d0f9 214
AndyA 0:97661408d0f9 215 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 216 void frameRateMeasure(uint32_t frameStartTime, bool dropFrame=false)
AndyA 1:dd1f7e162f91 217 {
AndyA 1:dd1f7e162f91 218 frameToggle =!frameToggle;
AndyA 1:dd1f7e162f91 219 static int frameRateCount = 0;
AndyA 1:dd1f7e162f91 220 static uint32_t rateCalcStartTime;
AndyA 1:dd1f7e162f91 221 if (frameRateCount == 0) {
AndyA 1:dd1f7e162f91 222 rateCalcStartTime = frameStartTime;
AndyA 1:dd1f7e162f91 223 frameRateCount = 1;
AndyA 1:dd1f7e162f91 224 } else {
AndyA 1:dd1f7e162f91 225 if (frameRateCount == framesToCount) {
AndyA 1:dd1f7e162f91 226 uint32_t timeTaken = frameStartTime - rateCalcStartTime;
AndyA 1:dd1f7e162f91 227 long int timePerFrame = timeTaken / framesToCount;
AndyA 1:dd1f7e162f91 228 detectedRate.setRate(getClosestRateIndex(timePerFrame));
AndyA 1:dd1f7e162f91 229 float TrueSeconds = framesToCount / detectedRate.currentRate();
AndyA 1:dd1f7e162f91 230 // +ve PPM means I counted more than real time = my clock is fast.
AndyA 1:dd1f7e162f91 231 float ppmError = (timeTaken - (1000000 * TrueSeconds)) / TrueSeconds;
AndyA 1:dd1f7e162f91 232 if ((ppmError<250) && (ppmError>-250)) { // something went very wrong with ppm calculation.
AndyA 1:dd1f7e162f91 233 if (PPMHighAcc!=0) {
AndyA 1:dd1f7e162f91 234 PPMTrackTotal -= PPMErrors[PPMTrackIndex];
AndyA 1:dd1f7e162f91 235 PPMTrackTotal += ppmError;
AndyA 1:dd1f7e162f91 236 PPMErrors[PPMTrackIndex++] = ppmError;
AndyA 1:dd1f7e162f91 237 PPMHighAcc = PPMTrackTotal / _longPPMTrackLen_;
AndyA 1:dd1f7e162f91 238 } else { // first time
AndyA 1:dd1f7e162f91 239 for (int i=0; i<_longPPMTrackLen_; i++)
AndyA 1:dd1f7e162f91 240 PPMErrors[i] = ppmError;
AndyA 1:dd1f7e162f91 241 PPMTrackTotal = ppmError*_longPPMTrackLen_;
AndyA 1:dd1f7e162f91 242 PPMHighAcc = ppmError;
AndyA 1:dd1f7e162f91 243 }
AndyA 1:dd1f7e162f91 244 if (PPMTrackIndex == _longPPMTrackLen_)
AndyA 1:dd1f7e162f91 245 PPMTrackIndex = 0;
AndyA 1:dd1f7e162f91 246 printf("Frame rate detected as %s. My clock is %.4f ppm fast\r\n", detectedRate.frameRateString(), PPMHighAcc);
AndyA 1:dd1f7e162f91 247 } else {
AndyA 1:dd1f7e162f91 248 printf("Frame rate unclear\r\n");
AndyA 1:dd1f7e162f91 249 PPMHighAcc=0;
AndyA 1:dd1f7e162f91 250 detectedRate.setRate(0);
AndyA 1:dd1f7e162f91 251 }
AndyA 1:dd1f7e162f91 252 frameRateCount = 0;
AndyA 1:dd1f7e162f91 253 } else {
AndyA 1:dd1f7e162f91 254 frameRateCount++;
AndyA 0:97661408d0f9 255 }
AndyA 0:97661408d0f9 256 }
AndyA 0:97661408d0f9 257 }
AndyA 0:97661408d0f9 258
AndyA 0:97661408d0f9 259
AndyA 0:97661408d0f9 260 //called once per frame to output the current postition
AndyA 1:dd1f7e162f91 261 void framePositionOutput()
AndyA 1:dd1f7e162f91 262 {
AndyA 0:97661408d0f9 263 sendPosition(VIPS.sendPositionForTime(TimeSinceLastFrame.read_us()));
AndyA 0:97661408d0f9 264 TimeSinceLastFrame.reset();
AndyA 3:14d241e29be3 265 FIZPort.requestCurrent();
AndyA 0:97661408d0f9 266 }
AndyA 0:97661408d0f9 267
AndyA 0:97661408d0f9 268 // called by background loop at the end of each frame
AndyA 0:97661408d0f9 269 // frameStartTime = time of inputTimer at the start of the first bit of the
AndyA 0:97661408d0f9 270 // frame. dropFrame = true if the protocol includes frameDrops
AndyA 0:97661408d0f9 271 void frameComplete(int hours, int minutes, int seconds, int frame,
AndyA 1:dd1f7e162f91 272 bool dropFrame, uint32_t frameStartTime)
AndyA 1:dd1f7e162f91 273 {
AndyA 1:dd1f7e162f91 274 static int lastFrame = 0;
AndyA 1:dd1f7e162f91 275 static float ppmError;
AndyA 1:dd1f7e162f91 276 static int outCount = 0;
AndyA 1:dd1f7e162f91 277 if ((frame != (lastFrame + 1)) && detectedRate.currentRate()) {
AndyA 1:dd1f7e162f91 278 long timeSinceSecondStart = detectedRate.getOffsetFromSecondStart(minutes, seconds, frame);
AndyA 0:97661408d0f9 279 // printf("Time Code %02d:%02d:%02d:%02d - VBOX time %s\r", hours, minutes, seconds, frame, ticksToTime(VBOXTicks));
AndyA 1:dd1f7e162f91 280 uint32_t ThisSecondStart = frameStartTime - timeSinceSecondStart;
AndyA 1:dd1f7e162f91 281 if (!ppsRunning) {
AndyA 1:dd1f7e162f91 282 uint32_t nextSecondStart = ThisSecondStart + 1000000 + (int)(ppmError + 0.5);
AndyA 1:dd1f7e162f91 283 ppsRunning = true;
AndyA 1:dd1f7e162f91 284 __disable_irq();
AndyA 1:dd1f7e162f91 285 uint32_t nextSec = nextSecondStart - inputTimer.read_us();
AndyA 1:dd1f7e162f91 286 PPSsyncTimer.attach_us(callback(OnPPSCync), nextSec);
AndyA 1:dd1f7e162f91 287 __enable_irq();
AndyA 1:dd1f7e162f91 288 printf("PPS start scheduled for %0.3f seconds.\r\n", nextSec / 1000000.0f);
AndyA 1:dd1f7e162f91 289 VBOXTicks = (hours * 3600 + minutes * 60 + (seconds + 1)) * 100;
AndyA 1:dd1f7e162f91 290 } else if (detectedRate.isSyncable()) { // PPS running and from a syncable source
AndyA 1:dd1f7e162f91 291 long timeError = (lastPPSSecondStart - ThisSecondStart);
AndyA 1:dd1f7e162f91 292 if (timeError < -500000)
AndyA 1:dd1f7e162f91 293 timeError += 1000000;
AndyA 1:dd1f7e162f91 294 timeError = timeError % 1000000;
AndyA 0:97661408d0f9 295 // printf("PPS error measured as %ld us.\r", timeError);
AndyA 0:97661408d0f9 296
AndyA 1:dd1f7e162f91 297 if ((timeError > MaxTimeErrorUS) || (timeError < -MaxTimeErrorUS)) {
AndyA 1:dd1f7e162f91 298 outCount++;
AndyA 1:dd1f7e162f91 299 if (outCount > 3) {
AndyA 1:dd1f7e162f91 300 int newPeriod = 5000 - timeError / 2;
AndyA 1:dd1f7e162f91 301 if (newPeriod < 4500)
AndyA 1:dd1f7e162f91 302 newPeriod = 4500;
AndyA 1:dd1f7e162f91 303 if (newPeriod > 5500)
AndyA 1:dd1f7e162f91 304 newPeriod = 5500;
AndyA 0:97661408d0f9 305
AndyA 1:dd1f7e162f91 306 // printf("Resync attempt. Error was %ld. 1 cycle at %d us\r\n", timeError, newPeriod);
AndyA 1:dd1f7e162f91 307 __disable_irq();
AndyA 1:dd1f7e162f91 308 resyncPeriod = newPeriod - timerOverheadTime;
AndyA 1:dd1f7e162f91 309 resync = true;
AndyA 1:dd1f7e162f91 310 __enable_irq();
AndyA 1:dd1f7e162f91 311 }
AndyA 1:dd1f7e162f91 312 } else
AndyA 1:dd1f7e162f91 313 outCount = 0;
AndyA 1:dd1f7e162f91 314 } // either no PPS and can't start it yet or running from unsyncable source
AndyA 1:dd1f7e162f91 315 // so just free run.
AndyA 1:dd1f7e162f91 316 }
AndyA 1:dd1f7e162f91 317 lastFrame = frame;
AndyA 0:97661408d0f9 318
AndyA 0:97661408d0f9 319 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 320 frameRateMeasure(frameStartTime, dropFrame);
AndyA 1:dd1f7e162f91 321 framePositionOutput();
AndyA 0:97661408d0f9 322 }
AndyA 0:97661408d0f9 323
AndyA 0:97661408d0f9 324 // called by background loop at the end of each frame when running from a sync signal not timecode
AndyA 1:dd1f7e162f91 325 void framePulse(uint32_t frameStartTime)
AndyA 1:dd1f7e162f91 326 {
AndyA 1:dd1f7e162f91 327 static int outCount = 0;
AndyA 1:dd1f7e162f91 328 uint32_t patternStartTime;
AndyA 1:dd1f7e162f91 329 if (detectedRate.isValid()) {
AndyA 1:dd1f7e162f91 330 uint32_t ThisSecondStart = frameStartTime ;
AndyA 1:dd1f7e162f91 331 if (!ppsRunning) {
AndyA 1:dd1f7e162f91 332 uint32_t pulseStartTime = frameStartTime + (int)(detectedRate.currentPeriodUS()*5+0.5f);
AndyA 1:dd1f7e162f91 333 uint32_t timeTillStart = pulseStartTime - inputTimer.read_us();
AndyA 1:dd1f7e162f91 334 ppsRunning = true;
AndyA 1:dd1f7e162f91 335 __disable_irq();
AndyA 1:dd1f7e162f91 336 PPSsyncTimer.attach_us(callback(OnPPSCync), timeTillStart);
AndyA 1:dd1f7e162f91 337 __enable_irq();
AndyA 1:dd1f7e162f91 338 printf("PPS start scheduled for %0.3f seconds.\r\n",
AndyA 1:dd1f7e162f91 339 timeTillStart / 1000000.0f);
AndyA 1:dd1f7e162f91 340 VBOXTicks = 0;
AndyA 1:dd1f7e162f91 341 outCount = 0;
AndyA 1:dd1f7e162f91 342 } else {
AndyA 0:97661408d0f9 343 // if (outCount == 0)
AndyA 0:97661408d0f9 344 // patternStartTime = inputTimer.elapsed_time();
AndyA 0:97661408d0f9 345 // else {
AndyA 0:97661408d0f9 346 // expectedTotalUS = outCount
AndyA 0:97661408d0f9 347 // }
AndyA 1:dd1f7e162f91 348 }
AndyA 0:97661408d0f9 349 }
AndyA 1:dd1f7e162f91 350 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 351 frameRateMeasure(frameStartTime, false);
AndyA 1:dd1f7e162f91 352 framePositionOutput();
AndyA 1:dd1f7e162f91 353 }
AndyA 0:97661408d0f9 354
AndyA 0:97661408d0f9 355 volatile bool NewFramePulse= false;
AndyA 0:97661408d0f9 356 uint32_t FramePulseTime;
AndyA 0:97661408d0f9 357 volatile int framesIn = 0;
AndyA 1:dd1f7e162f91 358 void OnPPFInputStartup(void)
AndyA 1:dd1f7e162f91 359 {
AndyA 0:97661408d0f9 360 framesIn++;
AndyA 0:97661408d0f9 361 }
AndyA 0:97661408d0f9 362
AndyA 0:97661408d0f9 363 volatile int SyncInCount = 0;
AndyA 1:dd1f7e162f91 364 void OnSyncInputStartup(void)
AndyA 1:dd1f7e162f91 365 {
AndyA 0:97661408d0f9 366 SyncInCount++;
AndyA 0:97661408d0f9 367 }
AndyA 0:97661408d0f9 368
AndyA 0:97661408d0f9 369
AndyA 1:dd1f7e162f91 370 void OnPPFInput(void)
AndyA 1:dd1f7e162f91 371 {
AndyA 0:97661408d0f9 372 FramePulseTime = inputTimer.read_us();
AndyA 0:97661408d0f9 373 NewFramePulse = true;
AndyA 0:97661408d0f9 374 }
AndyA 0:97661408d0f9 375
AndyA 0:97661408d0f9 376
AndyA 1:dd1f7e162f91 377 int main()
AndyA 1:dd1f7e162f91 378 {
AndyA 1:dd1f7e162f91 379 pc.baud(115200);
AndyA 1:dd1f7e162f91 380 COM1.baud(115200);
AndyA 1:dd1f7e162f91 381 inputTimer.reset();
AndyA 1:dd1f7e162f91 382 inputTimer.start();
AndyA 0:97661408d0f9 383
AndyA 1:dd1f7e162f91 384 pc.printf("Startup\r\n");
AndyA 1:dd1f7e162f91 385 led1 = 1;
AndyA 0:97661408d0f9 386
AndyA 1:dd1f7e162f91 387 prepPacketOut();
AndyA 3:14d241e29be3 388
AndyA 1:dd1f7e162f91 389 LTCInput.setInputTimer(&inputTimer);
AndyA 3:14d241e29be3 390
AndyA 1:dd1f7e162f91 391 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 3:14d241e29be3 392
AndyA 1:dd1f7e162f91 393 VIPS.run();
AndyA 1:dd1f7e162f91 394
AndyA 1:dd1f7e162f91 395 pc.printf("armed\r\n");
AndyA 1:dd1f7e162f91 396 led2 = 1;
AndyA 0:97661408d0f9 397
AndyA 1:dd1f7e162f91 398 for (int i = 0; i < _longPPMTrackLen_; i++)
AndyA 1:dd1f7e162f91 399 PPMErrors[i] = 0;
AndyA 1:dd1f7e162f91 400 pc.printf("Startup\r\n");
AndyA 1:dd1f7e162f91 401 PPMTrackIndex = 0;
AndyA 1:dd1f7e162f91 402 PPMTrackTotal = 0;
AndyA 1:dd1f7e162f91 403 remainingClockError = 0;
AndyA 1:dd1f7e162f91 404 bool GenLockOnlyMode = false;
AndyA 1:dd1f7e162f91 405 bool GenLockToSync = false;
AndyA 0:97661408d0f9 406
AndyA 1:dd1f7e162f91 407 TimeSinceLastFrame.reset();
AndyA 1:dd1f7e162f91 408 TimeSinceLastFrame.start();
AndyA 1:dd1f7e162f91 409 while (true) {
AndyA 1:dd1f7e162f91 410 pc.printf("Waiting for sync\r\n");
AndyA 1:dd1f7e162f91 411 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 1:dd1f7e162f91 412 Syncin.rise(callback(&OnSyncInputStartup));
AndyA 1:dd1f7e162f91 413 framesIn = 0;
AndyA 1:dd1f7e162f91 414 SyncInCount = 0;
AndyA 1:dd1f7e162f91 415 GenLockOnlyMode = false;
AndyA 1:dd1f7e162f91 416 GenLockToSync = false;
AndyA 1:dd1f7e162f91 417 while (!LTCInput.searchForSync()) {
AndyA 1:dd1f7e162f91 418 if (framesIn == 100) {
AndyA 1:dd1f7e162f91 419 GenLockOnlyMode = true;
AndyA 1:dd1f7e162f91 420 break;
AndyA 1:dd1f7e162f91 421 }
AndyA 1:dd1f7e162f91 422 if ((SyncInCount == 100) && (framesIn<45)) { // prefer frame input pin, sync may be twice as high for interlaced systems.
AndyA 1:dd1f7e162f91 423 GenLockOnlyMode = true;
AndyA 1:dd1f7e162f91 424 GenLockToSync= true;
AndyA 1:dd1f7e162f91 425 break;
AndyA 1:dd1f7e162f91 426 }
AndyA 1:dd1f7e162f91 427
AndyA 1:dd1f7e162f91 428 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 429 if (message)
AndyA 1:dd1f7e162f91 430 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 0:97661408d0f9 431
AndyA 0:97661408d0f9 432 }
AndyA 1:dd1f7e162f91 433 if (GenLockOnlyMode) {
AndyA 1:dd1f7e162f91 434 if (GenLockToSync) {
AndyA 1:dd1f7e162f91 435 pc.printf("No LTC detected for 100 frames. Falling back to genlock on sync input\r\n");
AndyA 1:dd1f7e162f91 436 Syncin.rise(callback(&OnPPFInput));
AndyA 1:dd1f7e162f91 437 PPFin.rise(NULL);
AndyA 1:dd1f7e162f91 438 } else {
AndyA 1:dd1f7e162f91 439 pc.printf("No LTC detected for 100 frames. Falling back to genlock on frame input\r\n");
AndyA 1:dd1f7e162f91 440 PPFin.rise(callback(&OnPPFInput));
AndyA 1:dd1f7e162f91 441 Syncin.rise(NULL);
AndyA 1:dd1f7e162f91 442 }
AndyA 1:dd1f7e162f91 443 uint32_t lastframeTime = inputTimer.read_us();
AndyA 1:dd1f7e162f91 444
AndyA 1:dd1f7e162f91 445 while (true) {
AndyA 1:dd1f7e162f91 446 if (NewFramePulse) { // running on a frame
AndyA 1:dd1f7e162f91 447 // printf("frame\r");
AndyA 1:dd1f7e162f91 448 lastframeTime = FramePulseTime;
AndyA 1:dd1f7e162f91 449 framePulse(lastframeTime);
AndyA 1:dd1f7e162f91 450 NewFramePulse = false;
AndyA 1:dd1f7e162f91 451 }
AndyA 1:dd1f7e162f91 452 if ((inputTimer.read_us()-lastframeTime) > 1000000) {
AndyA 1:dd1f7e162f91 453 pc.printf("No frames for > 1 second\r\n");
AndyA 1:dd1f7e162f91 454 break;
AndyA 1:dd1f7e162f91 455 }
AndyA 1:dd1f7e162f91 456 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 457 if (message)
AndyA 1:dd1f7e162f91 458 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 1:dd1f7e162f91 459 }
AndyA 1:dd1f7e162f91 460 } else { // not genlock
AndyA 1:dd1f7e162f91 461 pc.printf("Got LTC sync\r\n");
AndyA 1:dd1f7e162f91 462 PPFin.rise(NULL);
AndyA 1:dd1f7e162f91 463 while (LTCInput.synced()) {
AndyA 1:dd1f7e162f91 464 position *posPtr = VIPS.getWaitingPostion();
AndyA 1:dd1f7e162f91 465 if (posPtr) {
AndyA 1:dd1f7e162f91 466 sendPosition(posPtr);
AndyA 1:dd1f7e162f91 467 }
AndyA 1:dd1f7e162f91 468 if (LTCInput.readWaitingData()) {
AndyA 1:dd1f7e162f91 469 if (LTCInput.synced()) {
AndyA 1:dd1f7e162f91 470 const LTCDecode::LTCData_t *frameData = LTCInput.getLastFrame();
AndyA 1:dd1f7e162f91 471 detectedRate.setDrop(frameData->frameDrop);
AndyA 1:dd1f7e162f91 472 frameComplete(frameData->hours, frameData->minutes,
AndyA 1:dd1f7e162f91 473 frameData->seconds, frameData->frame,
AndyA 1:dd1f7e162f91 474 frameData->frameDrop, frameData->frameStartTime);
AndyA 1:dd1f7e162f91 475 } // end if good frame
AndyA 1:dd1f7e162f91 476 } // end if new data
AndyA 1:dd1f7e162f91 477 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 478 if (message)
AndyA 1:dd1f7e162f91 479 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 1:dd1f7e162f91 480 } // end while synced
AndyA 1:dd1f7e162f91 481 pc.printf("Sync lost\r");
AndyA 0:97661408d0f9 482 }
AndyA 1:dd1f7e162f91 483 } // end while true
AndyA 0:97661408d0f9 484 }