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@4:6cd904bff1bd, 2021-01-29 (annotated)

Committer:
AndyA
Date:
Fri Jan 29 14:50:21 2021 +0000
Revision:
4:6cd904bff1bd
Parent:
3:14d241e29be3
Child:
6:61274e214f46
add fiz;

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 4:6cd904bff1bd 103 FIZPort.getMostRecent(&packetOut.focus, &packetOut.iris, &packetOut.zoom);
AndyA 1:dd1f7e162f91 104 VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
AndyA 1:dd1f7e162f91 105 COM1.write(&packetOut, sizeof(struct outputFormat_s));
AndyA 0:97661408d0f9 106 }
AndyA 0:97661408d0f9 107 }
AndyA 0:97661408d0f9 108
AndyA 0:97661408d0f9 109
AndyA 1:dd1f7e162f91 110 void OnPPSEdge()
AndyA 1:dd1f7e162f91 111 {
AndyA 1:dd1f7e162f91 112 if (PPSHigh) {
AndyA 1:dd1f7e162f91 113 PPS = 0;
AndyA 1:dd1f7e162f91 114 led3=1;
AndyA 1:dd1f7e162f91 115 PPSHigh = false;
AndyA 1:dd1f7e162f91 116 } else {
AndyA 1:dd1f7e162f91 117 led3=0;
AndyA 1:dd1f7e162f91 118 PPS = 1;
AndyA 1:dd1f7e162f91 119 PPSHigh = true;
AndyA 0:97661408d0f9 120
AndyA 1:dd1f7e162f91 121 if (resync && !ppmCorrection) {
AndyA 0:97661408d0f9 122 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 123 PPSOutputTimer.attach_us(callback(OnPPSEdge), resyncPeriod);
AndyA 1:dd1f7e162f91 124 resync = false;
AndyA 1:dd1f7e162f91 125 resyncDone = true;
AndyA 1:dd1f7e162f91 126 } else if (resyncDone) {
AndyA 1:dd1f7e162f91 127 resyncDone = false;
AndyA 0:97661408d0f9 128 PPSOutputTimer.detach();
AndyA 0:97661408d0f9 129 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 130 remainingClockError = 0;
AndyA 0:97661408d0f9 131 }
AndyA 0:97661408d0f9 132
AndyA 1:dd1f7e162f91 133 if ((VBOXTicks % 100) == 0) {
AndyA 1:dd1f7e162f91 134 lastPPSSecondStart = inputTimer.read_us();
AndyA 1:dd1f7e162f91 135 second = 1;
AndyA 1:dd1f7e162f91 136 remainingClockError+=PPMHighAcc;
AndyA 1:dd1f7e162f91 137 int adjustment = (int)(remainingClockError/2);
AndyA 1:dd1f7e162f91 138 if (!resyncDone && ((adjustment > 2)|| (adjustment <-2))) {
AndyA 1:dd1f7e162f91 139 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 140 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000+adjustment-timerOverheadTime);
AndyA 1:dd1f7e162f91 141 ppmCorrection=true;
AndyA 1:dd1f7e162f91 142 remainingClockError-=2*adjustment;
AndyA 1:dd1f7e162f91 143 }
AndyA 1:dd1f7e162f91 144 } else {
AndyA 1:dd1f7e162f91 145 if ((VBOXTicks % 100) == 1) {
AndyA 1:dd1f7e162f91 146 second = 0;
AndyA 1:dd1f7e162f91 147 if (ppmCorrection) {
AndyA 1:dd1f7e162f91 148 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 149 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 150 ppmCorrection=false;
AndyA 1:dd1f7e162f91 151 }
AndyA 1:dd1f7e162f91 152 }
AndyA 1:dd1f7e162f91 153 }
AndyA 1:dd1f7e162f91 154
AndyA 1:dd1f7e162f91 155 VBOXTicks++;
AndyA 1:dd1f7e162f91 156 if (VBOXTicks >= (24 * 60 * 60 * 100))
AndyA 1:dd1f7e162f91 157 VBOXTicks -= (24 * 60 * 60 * 100);
AndyA 1:dd1f7e162f91 158 }
AndyA 0:97661408d0f9 159 }
AndyA 0:97661408d0f9 160
AndyA 1:dd1f7e162f91 161 void OnPPSCync()
AndyA 1:dd1f7e162f91 162 {
AndyA 1:dd1f7e162f91 163 PPS = 1;
AndyA 1:dd1f7e162f91 164 led3=0;
AndyA 1:dd1f7e162f91 165 PPSOutputTimer.detach();
AndyA 1:dd1f7e162f91 166 PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
AndyA 1:dd1f7e162f91 167 PPSHigh = true;
AndyA 1:dd1f7e162f91 168 VBOXTicks++;
AndyA 1:dd1f7e162f91 169 ppsActive = true;
AndyA 1:dd1f7e162f91 170 led1 = 1;
AndyA 0:97661408d0f9 171 }
AndyA 0:97661408d0f9 172
AndyA 1:dd1f7e162f91 173 const char *ticksToTime(unsigned long vboxTickCount)
AndyA 1:dd1f7e162f91 174 {
AndyA 1:dd1f7e162f91 175 static char timeString[32];
AndyA 1:dd1f7e162f91 176 int hours = vboxTickCount / 360000;
AndyA 1:dd1f7e162f91 177 int minutes = (vboxTickCount / 6000) % 60;
AndyA 1:dd1f7e162f91 178 int seconds = (vboxTickCount / 100) % 60;
AndyA 1:dd1f7e162f91 179 int ms = 10 * (vboxTickCount % 100);
AndyA 1:dd1f7e162f91 180 sprintf(timeString, "%02d:%02d:%02d.%03d", hours, minutes, seconds, ms);
AndyA 1:dd1f7e162f91 181 return timeString;
AndyA 0:97661408d0f9 182 }
AndyA 0:97661408d0f9 183
AndyA 1:dd1f7e162f91 184 bool isStartOfSecond(int minutes, int seconds, int frame, bool dropFrame)
AndyA 1:dd1f7e162f91 185 {
AndyA 1:dd1f7e162f91 186 if (frame == 0)
AndyA 1:dd1f7e162f91 187 return true;
AndyA 1:dd1f7e162f91 188 if (dropFrame && (2 == frame) && (0 == seconds) && (minutes % 10 != 0))
AndyA 1:dd1f7e162f91 189 return true;
AndyA 1:dd1f7e162f91 190 return false;
AndyA 0:97661408d0f9 191 }
AndyA 0:97661408d0f9 192
AndyA 0:97661408d0f9 193
AndyA 1:dd1f7e162f91 194 int getClosestRateIndex(long int framePeriodUS)
AndyA 1:dd1f7e162f91 195 {
AndyA 1:dd1f7e162f91 196 int indexOver = 1;
AndyA 1:dd1f7e162f91 197 while (framePeriodUS <= frameRateInfo::FramePeriods[indexOver])
AndyA 1:dd1f7e162f91 198 indexOver++;
AndyA 1:dd1f7e162f91 199 float amountOver = framePeriodUS - frameRateInfo::FramePeriods[indexOver];
AndyA 1:dd1f7e162f91 200 float amountUnder = frameRateInfo::FramePeriods[indexOver - 1] - framePeriodUS;
AndyA 1:dd1f7e162f91 201 if (amountOver > amountUnder)
AndyA 1:dd1f7e162f91 202 return indexOver - 1;
AndyA 1:dd1f7e162f91 203 return indexOver;
AndyA 0:97661408d0f9 204 }
AndyA 0:97661408d0f9 205
AndyA 0:97661408d0f9 206 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 207 void frameRateMeasure(uint32_t frameStartTime, bool dropFrame=false)
AndyA 1:dd1f7e162f91 208 {
AndyA 1:dd1f7e162f91 209 frameToggle =!frameToggle;
AndyA 1:dd1f7e162f91 210 static int frameRateCount = 0;
AndyA 1:dd1f7e162f91 211 static uint32_t rateCalcStartTime;
AndyA 1:dd1f7e162f91 212 if (frameRateCount == 0) {
AndyA 1:dd1f7e162f91 213 rateCalcStartTime = frameStartTime;
AndyA 1:dd1f7e162f91 214 frameRateCount = 1;
AndyA 1:dd1f7e162f91 215 } else {
AndyA 1:dd1f7e162f91 216 if (frameRateCount == framesToCount) {
AndyA 1:dd1f7e162f91 217 uint32_t timeTaken = frameStartTime - rateCalcStartTime;
AndyA 1:dd1f7e162f91 218 long int timePerFrame = timeTaken / framesToCount;
AndyA 1:dd1f7e162f91 219 detectedRate.setRate(getClosestRateIndex(timePerFrame));
AndyA 1:dd1f7e162f91 220 float TrueSeconds = framesToCount / detectedRate.currentRate();
AndyA 1:dd1f7e162f91 221 // +ve PPM means I counted more than real time = my clock is fast.
AndyA 1:dd1f7e162f91 222 float ppmError = (timeTaken - (1000000 * TrueSeconds)) / TrueSeconds;
AndyA 1:dd1f7e162f91 223 if ((ppmError<250) && (ppmError>-250)) { // something went very wrong with ppm calculation.
AndyA 1:dd1f7e162f91 224 if (PPMHighAcc!=0) {
AndyA 1:dd1f7e162f91 225 PPMTrackTotal -= PPMErrors[PPMTrackIndex];
AndyA 1:dd1f7e162f91 226 PPMTrackTotal += ppmError;
AndyA 1:dd1f7e162f91 227 PPMErrors[PPMTrackIndex++] = ppmError;
AndyA 1:dd1f7e162f91 228 PPMHighAcc = PPMTrackTotal / _longPPMTrackLen_;
AndyA 1:dd1f7e162f91 229 } else { // first time
AndyA 1:dd1f7e162f91 230 for (int i=0; i<_longPPMTrackLen_; i++)
AndyA 1:dd1f7e162f91 231 PPMErrors[i] = ppmError;
AndyA 1:dd1f7e162f91 232 PPMTrackTotal = ppmError*_longPPMTrackLen_;
AndyA 1:dd1f7e162f91 233 PPMHighAcc = ppmError;
AndyA 1:dd1f7e162f91 234 }
AndyA 1:dd1f7e162f91 235 if (PPMTrackIndex == _longPPMTrackLen_)
AndyA 1:dd1f7e162f91 236 PPMTrackIndex = 0;
AndyA 1:dd1f7e162f91 237 printf("Frame rate detected as %s. My clock is %.4f ppm fast\r\n", detectedRate.frameRateString(), PPMHighAcc);
AndyA 1:dd1f7e162f91 238 } else {
AndyA 1:dd1f7e162f91 239 printf("Frame rate unclear\r\n");
AndyA 1:dd1f7e162f91 240 PPMHighAcc=0;
AndyA 1:dd1f7e162f91 241 detectedRate.setRate(0);
AndyA 1:dd1f7e162f91 242 }
AndyA 1:dd1f7e162f91 243 frameRateCount = 0;
AndyA 1:dd1f7e162f91 244 } else {
AndyA 1:dd1f7e162f91 245 frameRateCount++;
AndyA 0:97661408d0f9 246 }
AndyA 0:97661408d0f9 247 }
AndyA 0:97661408d0f9 248 }
AndyA 0:97661408d0f9 249
AndyA 0:97661408d0f9 250
AndyA 0:97661408d0f9 251 //called once per frame to output the current postition
AndyA 1:dd1f7e162f91 252 void framePositionOutput()
AndyA 1:dd1f7e162f91 253 {
AndyA 0:97661408d0f9 254 sendPosition(VIPS.sendPositionForTime(TimeSinceLastFrame.read_us()));
AndyA 0:97661408d0f9 255 TimeSinceLastFrame.reset();
AndyA 3:14d241e29be3 256 FIZPort.requestCurrent();
AndyA 0:97661408d0f9 257 }
AndyA 0:97661408d0f9 258
AndyA 0:97661408d0f9 259 // called by background loop at the end of each frame
AndyA 0:97661408d0f9 260 // frameStartTime = time of inputTimer at the start of the first bit of the
AndyA 0:97661408d0f9 261 // frame. dropFrame = true if the protocol includes frameDrops
AndyA 0:97661408d0f9 262 void frameComplete(int hours, int minutes, int seconds, int frame,
AndyA 1:dd1f7e162f91 263 bool dropFrame, uint32_t frameStartTime)
AndyA 1:dd1f7e162f91 264 {
AndyA 1:dd1f7e162f91 265 static int lastFrame = 0;
AndyA 1:dd1f7e162f91 266 static float ppmError;
AndyA 1:dd1f7e162f91 267 static int outCount = 0;
AndyA 1:dd1f7e162f91 268 if ((frame != (lastFrame + 1)) && detectedRate.currentRate()) {
AndyA 1:dd1f7e162f91 269 long timeSinceSecondStart = detectedRate.getOffsetFromSecondStart(minutes, seconds, frame);
AndyA 0:97661408d0f9 270 // printf("Time Code %02d:%02d:%02d:%02d - VBOX time %s\r", hours, minutes, seconds, frame, ticksToTime(VBOXTicks));
AndyA 1:dd1f7e162f91 271 uint32_t ThisSecondStart = frameStartTime - timeSinceSecondStart;
AndyA 1:dd1f7e162f91 272 if (!ppsRunning) {
AndyA 1:dd1f7e162f91 273 uint32_t nextSecondStart = ThisSecondStart + 1000000 + (int)(ppmError + 0.5);
AndyA 1:dd1f7e162f91 274 ppsRunning = true;
AndyA 1:dd1f7e162f91 275 __disable_irq();
AndyA 1:dd1f7e162f91 276 uint32_t nextSec = nextSecondStart - inputTimer.read_us();
AndyA 1:dd1f7e162f91 277 PPSsyncTimer.attach_us(callback(OnPPSCync), nextSec);
AndyA 1:dd1f7e162f91 278 __enable_irq();
AndyA 1:dd1f7e162f91 279 printf("PPS start scheduled for %0.3f seconds.\r\n", nextSec / 1000000.0f);
AndyA 1:dd1f7e162f91 280 VBOXTicks = (hours * 3600 + minutes * 60 + (seconds + 1)) * 100;
AndyA 1:dd1f7e162f91 281 } else if (detectedRate.isSyncable()) { // PPS running and from a syncable source
AndyA 1:dd1f7e162f91 282 long timeError = (lastPPSSecondStart - ThisSecondStart);
AndyA 1:dd1f7e162f91 283 if (timeError < -500000)
AndyA 1:dd1f7e162f91 284 timeError += 1000000;
AndyA 1:dd1f7e162f91 285 timeError = timeError % 1000000;
AndyA 0:97661408d0f9 286 // printf("PPS error measured as %ld us.\r", timeError);
AndyA 0:97661408d0f9 287
AndyA 1:dd1f7e162f91 288 if ((timeError > MaxTimeErrorUS) || (timeError < -MaxTimeErrorUS)) {
AndyA 1:dd1f7e162f91 289 outCount++;
AndyA 1:dd1f7e162f91 290 if (outCount > 3) {
AndyA 1:dd1f7e162f91 291 int newPeriod = 5000 - timeError / 2;
AndyA 1:dd1f7e162f91 292 if (newPeriod < 4500)
AndyA 1:dd1f7e162f91 293 newPeriod = 4500;
AndyA 1:dd1f7e162f91 294 if (newPeriod > 5500)
AndyA 1:dd1f7e162f91 295 newPeriod = 5500;
AndyA 0:97661408d0f9 296
AndyA 1:dd1f7e162f91 297 // printf("Resync attempt. Error was %ld. 1 cycle at %d us\r\n", timeError, newPeriod);
AndyA 1:dd1f7e162f91 298 __disable_irq();
AndyA 1:dd1f7e162f91 299 resyncPeriod = newPeriod - timerOverheadTime;
AndyA 1:dd1f7e162f91 300 resync = true;
AndyA 1:dd1f7e162f91 301 __enable_irq();
AndyA 1:dd1f7e162f91 302 }
AndyA 1:dd1f7e162f91 303 } else
AndyA 1:dd1f7e162f91 304 outCount = 0;
AndyA 1:dd1f7e162f91 305 } // either no PPS and can't start it yet or running from unsyncable source
AndyA 1:dd1f7e162f91 306 // so just free run.
AndyA 1:dd1f7e162f91 307 }
AndyA 1:dd1f7e162f91 308 lastFrame = frame;
AndyA 0:97661408d0f9 309
AndyA 0:97661408d0f9 310 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 311 frameRateMeasure(frameStartTime, dropFrame);
AndyA 1:dd1f7e162f91 312 framePositionOutput();
AndyA 0:97661408d0f9 313 }
AndyA 0:97661408d0f9 314
AndyA 0:97661408d0f9 315 // called by background loop at the end of each frame when running from a sync signal not timecode
AndyA 1:dd1f7e162f91 316 void framePulse(uint32_t frameStartTime)
AndyA 1:dd1f7e162f91 317 {
AndyA 1:dd1f7e162f91 318 static int outCount = 0;
AndyA 1:dd1f7e162f91 319 uint32_t patternStartTime;
AndyA 1:dd1f7e162f91 320 if (detectedRate.isValid()) {
AndyA 1:dd1f7e162f91 321 uint32_t ThisSecondStart = frameStartTime ;
AndyA 1:dd1f7e162f91 322 if (!ppsRunning) {
AndyA 1:dd1f7e162f91 323 uint32_t pulseStartTime = frameStartTime + (int)(detectedRate.currentPeriodUS()*5+0.5f);
AndyA 1:dd1f7e162f91 324 uint32_t timeTillStart = pulseStartTime - inputTimer.read_us();
AndyA 1:dd1f7e162f91 325 ppsRunning = true;
AndyA 1:dd1f7e162f91 326 __disable_irq();
AndyA 1:dd1f7e162f91 327 PPSsyncTimer.attach_us(callback(OnPPSCync), timeTillStart);
AndyA 1:dd1f7e162f91 328 __enable_irq();
AndyA 1:dd1f7e162f91 329 printf("PPS start scheduled for %0.3f seconds.\r\n",
AndyA 1:dd1f7e162f91 330 timeTillStart / 1000000.0f);
AndyA 1:dd1f7e162f91 331 VBOXTicks = 0;
AndyA 1:dd1f7e162f91 332 outCount = 0;
AndyA 1:dd1f7e162f91 333 } else {
AndyA 0:97661408d0f9 334 // if (outCount == 0)
AndyA 0:97661408d0f9 335 // patternStartTime = inputTimer.elapsed_time();
AndyA 0:97661408d0f9 336 // else {
AndyA 0:97661408d0f9 337 // expectedTotalUS = outCount
AndyA 0:97661408d0f9 338 // }
AndyA 1:dd1f7e162f91 339 }
AndyA 0:97661408d0f9 340 }
AndyA 1:dd1f7e162f91 341 // frame rate and clock error calculation
AndyA 1:dd1f7e162f91 342 frameRateMeasure(frameStartTime, false);
AndyA 1:dd1f7e162f91 343 framePositionOutput();
AndyA 1:dd1f7e162f91 344 }
AndyA 0:97661408d0f9 345
AndyA 0:97661408d0f9 346 volatile bool NewFramePulse= false;
AndyA 0:97661408d0f9 347 uint32_t FramePulseTime;
AndyA 0:97661408d0f9 348 volatile int framesIn = 0;
AndyA 1:dd1f7e162f91 349 void OnPPFInputStartup(void)
AndyA 1:dd1f7e162f91 350 {
AndyA 0:97661408d0f9 351 framesIn++;
AndyA 0:97661408d0f9 352 }
AndyA 0:97661408d0f9 353
AndyA 0:97661408d0f9 354 volatile int SyncInCount = 0;
AndyA 1:dd1f7e162f91 355 void OnSyncInputStartup(void)
AndyA 1:dd1f7e162f91 356 {
AndyA 0:97661408d0f9 357 SyncInCount++;
AndyA 0:97661408d0f9 358 }
AndyA 0:97661408d0f9 359
AndyA 0:97661408d0f9 360
AndyA 1:dd1f7e162f91 361 void OnPPFInput(void)
AndyA 1:dd1f7e162f91 362 {
AndyA 0:97661408d0f9 363 FramePulseTime = inputTimer.read_us();
AndyA 0:97661408d0f9 364 NewFramePulse = true;
AndyA 0:97661408d0f9 365 }
AndyA 0:97661408d0f9 366
AndyA 0:97661408d0f9 367
AndyA 1:dd1f7e162f91 368 int main()
AndyA 1:dd1f7e162f91 369 {
AndyA 1:dd1f7e162f91 370 pc.baud(115200);
AndyA 1:dd1f7e162f91 371 COM1.baud(115200);
AndyA 1:dd1f7e162f91 372 inputTimer.reset();
AndyA 1:dd1f7e162f91 373 inputTimer.start();
AndyA 0:97661408d0f9 374
AndyA 1:dd1f7e162f91 375 pc.printf("Startup\r\n");
AndyA 1:dd1f7e162f91 376 led1 = 1;
AndyA 0:97661408d0f9 377
AndyA 1:dd1f7e162f91 378 prepPacketOut();
AndyA 3:14d241e29be3 379
AndyA 1:dd1f7e162f91 380 LTCInput.setInputTimer(&inputTimer);
AndyA 3:14d241e29be3 381
AndyA 1:dd1f7e162f91 382 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 3:14d241e29be3 383
AndyA 1:dd1f7e162f91 384 VIPS.run();
AndyA 1:dd1f7e162f91 385
AndyA 1:dd1f7e162f91 386 pc.printf("armed\r\n");
AndyA 1:dd1f7e162f91 387 led2 = 1;
AndyA 0:97661408d0f9 388
AndyA 1:dd1f7e162f91 389 for (int i = 0; i < _longPPMTrackLen_; i++)
AndyA 1:dd1f7e162f91 390 PPMErrors[i] = 0;
AndyA 1:dd1f7e162f91 391 pc.printf("Startup\r\n");
AndyA 1:dd1f7e162f91 392 PPMTrackIndex = 0;
AndyA 1:dd1f7e162f91 393 PPMTrackTotal = 0;
AndyA 1:dd1f7e162f91 394 remainingClockError = 0;
AndyA 1:dd1f7e162f91 395 bool GenLockOnlyMode = false;
AndyA 1:dd1f7e162f91 396 bool GenLockToSync = false;
AndyA 0:97661408d0f9 397
AndyA 1:dd1f7e162f91 398 TimeSinceLastFrame.reset();
AndyA 1:dd1f7e162f91 399 TimeSinceLastFrame.start();
AndyA 1:dd1f7e162f91 400 while (true) {
AndyA 1:dd1f7e162f91 401 pc.printf("Waiting for sync\r\n");
AndyA 1:dd1f7e162f91 402 PPFin.rise(callback(&OnPPFInputStartup));
AndyA 1:dd1f7e162f91 403 Syncin.rise(callback(&OnSyncInputStartup));
AndyA 1:dd1f7e162f91 404 framesIn = 0;
AndyA 1:dd1f7e162f91 405 SyncInCount = 0;
AndyA 1:dd1f7e162f91 406 GenLockOnlyMode = false;
AndyA 1:dd1f7e162f91 407 GenLockToSync = false;
AndyA 1:dd1f7e162f91 408 while (!LTCInput.searchForSync()) {
AndyA 1:dd1f7e162f91 409 if (framesIn == 100) {
AndyA 1:dd1f7e162f91 410 GenLockOnlyMode = true;
AndyA 1:dd1f7e162f91 411 break;
AndyA 1:dd1f7e162f91 412 }
AndyA 1:dd1f7e162f91 413 if ((SyncInCount == 100) && (framesIn<45)) { // prefer frame input pin, sync may be twice as high for interlaced systems.
AndyA 1:dd1f7e162f91 414 GenLockOnlyMode = true;
AndyA 1:dd1f7e162f91 415 GenLockToSync= true;
AndyA 1:dd1f7e162f91 416 break;
AndyA 1:dd1f7e162f91 417 }
AndyA 1:dd1f7e162f91 418
AndyA 1:dd1f7e162f91 419 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 420 if (message)
AndyA 1:dd1f7e162f91 421 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 0:97661408d0f9 422
AndyA 0:97661408d0f9 423 }
AndyA 1:dd1f7e162f91 424 if (GenLockOnlyMode) {
AndyA 1:dd1f7e162f91 425 if (GenLockToSync) {
AndyA 1:dd1f7e162f91 426 pc.printf("No LTC detected for 100 frames. Falling back to genlock on sync input\r\n");
AndyA 1:dd1f7e162f91 427 Syncin.rise(callback(&OnPPFInput));
AndyA 1:dd1f7e162f91 428 PPFin.rise(NULL);
AndyA 1:dd1f7e162f91 429 } else {
AndyA 1:dd1f7e162f91 430 pc.printf("No LTC detected for 100 frames. Falling back to genlock on frame input\r\n");
AndyA 1:dd1f7e162f91 431 PPFin.rise(callback(&OnPPFInput));
AndyA 1:dd1f7e162f91 432 Syncin.rise(NULL);
AndyA 1:dd1f7e162f91 433 }
AndyA 1:dd1f7e162f91 434 uint32_t lastframeTime = inputTimer.read_us();
AndyA 1:dd1f7e162f91 435
AndyA 1:dd1f7e162f91 436 while (true) {
AndyA 1:dd1f7e162f91 437 if (NewFramePulse) { // running on a frame
AndyA 1:dd1f7e162f91 438 // printf("frame\r");
AndyA 1:dd1f7e162f91 439 lastframeTime = FramePulseTime;
AndyA 1:dd1f7e162f91 440 framePulse(lastframeTime);
AndyA 1:dd1f7e162f91 441 NewFramePulse = false;
AndyA 1:dd1f7e162f91 442 }
AndyA 1:dd1f7e162f91 443 if ((inputTimer.read_us()-lastframeTime) > 1000000) {
AndyA 1:dd1f7e162f91 444 pc.printf("No frames for > 1 second\r\n");
AndyA 1:dd1f7e162f91 445 break;
AndyA 1:dd1f7e162f91 446 }
AndyA 1:dd1f7e162f91 447 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 448 if (message)
AndyA 1:dd1f7e162f91 449 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 1:dd1f7e162f91 450 }
AndyA 1:dd1f7e162f91 451 } else { // not genlock
AndyA 1:dd1f7e162f91 452 pc.printf("Got LTC sync\r\n");
AndyA 1:dd1f7e162f91 453 PPFin.rise(NULL);
AndyA 1:dd1f7e162f91 454 while (LTCInput.synced()) {
AndyA 1:dd1f7e162f91 455 position *posPtr = VIPS.getWaitingPostion();
AndyA 1:dd1f7e162f91 456 if (posPtr) {
AndyA 1:dd1f7e162f91 457 sendPosition(posPtr);
AndyA 1:dd1f7e162f91 458 }
AndyA 1:dd1f7e162f91 459 if (LTCInput.readWaitingData()) {
AndyA 1:dd1f7e162f91 460 if (LTCInput.synced()) {
AndyA 1:dd1f7e162f91 461 const LTCDecode::LTCData_t *frameData = LTCInput.getLastFrame();
AndyA 1:dd1f7e162f91 462 detectedRate.setDrop(frameData->frameDrop);
AndyA 1:dd1f7e162f91 463 frameComplete(frameData->hours, frameData->minutes,
AndyA 1:dd1f7e162f91 464 frameData->seconds, frameData->frame,
AndyA 1:dd1f7e162f91 465 frameData->frameDrop, frameData->frameStartTime);
AndyA 1:dd1f7e162f91 466 } // end if good frame
AndyA 1:dd1f7e162f91 467 } // end if new data
AndyA 1:dd1f7e162f91 468 int message = VIPS.getStatusMessage();
AndyA 1:dd1f7e162f91 469 if (message)
AndyA 1:dd1f7e162f91 470 pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
AndyA 1:dd1f7e162f91 471 } // end while synced
AndyA 1:dd1f7e162f91 472 pc.printf("Sync lost\r");
AndyA 0:97661408d0f9 473 }
AndyA 1:dd1f7e162f91 474 } // end while true
AndyA 0:97661408d0f9 475 }