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@8:961bb15570a1, 2021-02-16 (annotated)

Committer:
AndyA
Date:
Tue Feb 16 09:53:54 2021 +0000
Revision:
8:961bb15570a1
Parent:
7:87aea27cc68b
Child:
9:7214e3c3e5f8
Added support for status block in VIPS data

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