File content as of revision 6:61274e214f46:

#include "mbed.h"
#include "LTCApp.h"

const int framesToCount = 300;
const int MaxTimeErrorUS = 150;
const int timerOverheadTime = 19;

BufferedSerial pc(USBTX, USBRX);
VIPSSerial VIPS(p28, p27);
BufferedSerial COM1(p13, p14);
FIZReader FIZPort(p9, p10);

DigitalOut led1(LED1);
DigitalOut PPS(p12);

DigitalOut led2(LED2);
DigitalOut led3(LED3);

DigitalOut frameToggle(LED4);
DigitalOut second(p23);

LTCDecode LTCInput(p7);
InterruptIn PPFin(p29);
InterruptIn Syncin(p8);

// clock to time everything with
Timer inputTimer;

// Time since last frame event, used for position output interpolation
Timer TimeSinceLastFrame;
uint32_t TimeSinceLastFrameWrap;

// used to start PPS at the correct point
Timeout PPSsyncTimer;

// used to generate PPS edges
Ticker PPSOutputTimer;

frameRates detectedRate;
bool ppsRunning = false; // set when PPS start has been scheduled
volatile bool ppsActive = false;  // set when PPS is actuallt going (up to 1 second later)

volatile bool PPSHigh;
bool resync = false;
bool resyncDone = false;
uint32_t resyncPeriod;
bool OKToCheckSync = false;
volatile uint32_t VBOXTicks = 0; // time at the NEXT PPS edge
uint32_t lastPPSSecondStart;

#define _longPPMTrackLen_ 20
float PPMErrors[_longPPMTrackLen_];
float PPMTrackTotal;
int PPMTrackIndex;
float PPMHighAcc;
float remainingClockError;
bool ppmCorrection;

struct outputFormat_s {
    uint32_t header; // 2 byte header + 2 byte length
    uint32_t mask;
    uint32_t time;
    double x;
    double y;
    float z;
    float roll;
    float pitch;
    float yaw;
    uint8_t accuracy[4];
    uint32_t focus;
    uint16_t iris;
    uint16_t zoom;
    uint16_t checksum;
} __attribute__((packed)) ;

struct outputFormat_s packetOut;

void prepPacketOut()
{
    uint8_t bytes[4];
    bytes[0]=0x24;
    bytes[1]=0xd9;
    *(uint16_t*)(bytes+2) = sizeof(struct outputFormat_s);
    packetOut.header = *(uint32_t*)bytes;
    packetOut.mask = 0x0444;
    packetOut.accuracy[0] = 0;
    packetOut.accuracy[1] = 0;
    packetOut.accuracy[2] = 0;
    packetOut.accuracy[3] = 0;
}

void sendPosition(position *posPtr)
{
    if (posPtr) {
        packetOut.time = posPtr->time;
        packetOut.x = posPtr->X;
        packetOut.y = posPtr->Y;
        packetOut.z = posPtr->Height;
        packetOut.roll = posPtr->roll;
        packetOut.pitch = posPtr->pitch;
        packetOut.yaw = posPtr->yaw;
        packetOut.accuracy[3] = posPtr->ID;
    } else {
        packetOut.time = 0;
        packetOut.x = 0;
        packetOut.y = 0;
        packetOut.z = 0;
        packetOut.roll = 0;
        packetOut.pitch = 0;
        packetOut.yaw = 0;
        packetOut.accuracy[3] = 0;
    }
    FIZPort.getMostRecent(&packetOut.focus, &packetOut.iris, &packetOut.zoom);
    VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
    COM1.write(&packetOut, sizeof(struct outputFormat_s));
}


void OnPPSEdge()
{
    if (PPSHigh) {
        PPS = 0;
        led3=1;
        PPSHigh = false;
    } else {
        led3=0;
        PPS = 1;
        PPSHigh = true;

        if (resync && !ppmCorrection) {
            PPSOutputTimer.detach();
            PPSOutputTimer.attach_us(callback(OnPPSEdge), resyncPeriod);
            resync = false;
            resyncDone = true;
        } else if (resyncDone) {
            resyncDone = false;
            PPSOutputTimer.detach();
            PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
            remainingClockError = 0;
        }

        if ((VBOXTicks % 100) == 0) {
            lastPPSSecondStart = inputTimer.read_us();
            second = 1;
            remainingClockError+=PPMHighAcc;
            int adjustment = (int)(remainingClockError/2);
            if (!resyncDone && ((adjustment > 2)|| (adjustment <-2))) {
                PPSOutputTimer.detach();
                PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000+adjustment-timerOverheadTime);
                ppmCorrection=true;
                remainingClockError-=2*adjustment;
            }
        } else {
            if ((VBOXTicks % 100) == 1) {
                second = 0;
                if (ppmCorrection) {
                    PPSOutputTimer.detach();
                    PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
                    ppmCorrection=false;
                }
            }
        }

        VBOXTicks++;
        if (VBOXTicks >= (24 * 60 * 60 * 100))
            VBOXTicks -= (24 * 60 * 60 * 100);
    }
}

void OnPPSCync()
{
    PPS = 1;
    led3=0;
    PPSOutputTimer.detach();
    PPSOutputTimer.attach_us(callback(OnPPSEdge), 5000);
    PPSHigh = true;
    VBOXTicks++;
    ppsActive = true;
    led1 = 1;
}

const char *ticksToTime(unsigned long vboxTickCount)
{
    static char timeString[32];
    int hours = vboxTickCount / 360000;
    int minutes = (vboxTickCount / 6000) % 60;
    int seconds = (vboxTickCount / 100) % 60;
    int ms = 10 * (vboxTickCount % 100);
    sprintf(timeString, "%02d:%02d:%02d.%03d", hours, minutes, seconds, ms);
    return timeString;
}

bool isStartOfSecond(int minutes, int seconds, int frame, bool dropFrame)
{
    if (frame == 0)
        return true;
    if (dropFrame && (2 == frame) && (0 == seconds) && (minutes % 10 != 0))
        return true;
    return false;
}


int getClosestRateIndex(long int framePeriodUS)
{
    int indexOver = 1;
    while (framePeriodUS <= frameRateInfo::FramePeriods[indexOver])
        indexOver++;
    float amountOver = framePeriodUS - frameRateInfo::FramePeriods[indexOver];
    float amountUnder = frameRateInfo::FramePeriods[indexOver - 1] - framePeriodUS;
    if (amountOver > amountUnder)
        return indexOver - 1;
    return indexOver;
}

// frame rate and clock error calculation
void frameRateMeasure(uint32_t frameStartTime, bool dropFrame=false)
{
    frameToggle =!frameToggle;
    static int frameRateCount = 0;
    static uint32_t rateCalcStartTime;
    if (frameRateCount == 0) {
        rateCalcStartTime = frameStartTime;
        frameRateCount = 1;
    } else {
        if (frameRateCount == framesToCount) {
            uint32_t timeTaken = frameStartTime - rateCalcStartTime;
            long int timePerFrame = timeTaken / framesToCount;
            detectedRate.setRate(getClosestRateIndex(timePerFrame));
            float TrueSeconds = framesToCount / detectedRate.currentRate();
            // +ve PPM means I counted more than real time = my clock is fast.
            float ppmError = (timeTaken - (1000000 * TrueSeconds)) / TrueSeconds;
            if ((ppmError<250) && (ppmError>-250)) { // something went very wrong with ppm calculation.
                if (PPMHighAcc!=0) {
                    PPMTrackTotal -= PPMErrors[PPMTrackIndex];
                    PPMTrackTotal += ppmError;
                    PPMErrors[PPMTrackIndex++] = ppmError;
                    PPMHighAcc = PPMTrackTotal / _longPPMTrackLen_;
                } else { // first time
                    for (int i=0; i<_longPPMTrackLen_; i++)
                        PPMErrors[i] = ppmError;
                    PPMTrackTotal = ppmError*_longPPMTrackLen_;
                    PPMHighAcc = ppmError;
                }
                if (PPMTrackIndex == _longPPMTrackLen_)
                    PPMTrackIndex = 0;
                printf("Frame rate detected as %s. My clock is %.4f ppm fast\r\n", detectedRate.frameRateString(), PPMHighAcc);
            } else {
                printf("Frame rate unclear\r\n");
                PPMHighAcc=0;
                detectedRate.setRate(0);
            }
            frameRateCount = 0;
        } else {
            frameRateCount++;
        }
    }
}


//called once per frame to output the current postition
void framePositionOutput()
{
    sendPosition(VIPS.sendPositionForTime(TimeSinceLastFrame.read_us()));
    TimeSinceLastFrame.reset();
    FIZPort.requestCurrent();
}

// called by background loop at the end of each frame
// frameStartTime = time of inputTimer at the start of the first bit of the
// frame. dropFrame = true if the protocol includes frameDrops
void frameComplete(int hours, int minutes, int seconds, int frame,
                   bool dropFrame, uint32_t frameStartTime)
{
    static int lastFrame = 0;
    static float ppmError;
    static int outCount = 0;
    if ((frame != (lastFrame + 1)) && detectedRate.currentRate()) {
        long timeSinceSecondStart = detectedRate.getOffsetFromSecondStart(minutes, seconds, frame);
//    printf("Time Code %02d:%02d:%02d:%02d - VBOX time %s\r", hours, minutes, seconds, frame, ticksToTime(VBOXTicks));
        uint32_t ThisSecondStart = frameStartTime - timeSinceSecondStart;
        if (!ppsRunning) {
            uint32_t nextSecondStart = ThisSecondStart + 1000000 + (int)(ppmError + 0.5);
            ppsRunning = true;
            __disable_irq();
            uint32_t nextSec =  nextSecondStart - inputTimer.read_us();
            PPSsyncTimer.attach_us(callback(OnPPSCync), nextSec);
            __enable_irq();
            printf("PPS start scheduled for %0.3f seconds.\r\n", nextSec / 1000000.0f);
            VBOXTicks = (hours * 3600 + minutes * 60 + (seconds + 1)) * 100;
        } else if (detectedRate.isSyncable()) { // PPS running and from a syncable source
            long timeError = (lastPPSSecondStart - ThisSecondStart);
            if (timeError < -500000)
                timeError += 1000000;
            timeError = timeError % 1000000;
//      printf("PPS error measured as %ld us.\r", timeError);

            if ((timeError > MaxTimeErrorUS) || (timeError < -MaxTimeErrorUS)) {
                outCount++;
                if (outCount > 3) {
                    int newPeriod = 5000 - timeError / 2;
                    if (newPeriod < 4500)
                        newPeriod = 4500;
                    if (newPeriod > 5500)
                        newPeriod = 5500;

                    //      printf("Resync attempt. Error was %ld. 1 cycle at %d us\r\n", timeError, newPeriod);
                    __disable_irq();
                    resyncPeriod = newPeriod - timerOverheadTime;
                    resync = true;
                    __enable_irq();
                }
            } else
                outCount = 0;
        } // either no PPS and can't start it yet or running from unsyncable source
        // so just free run.
    }
    lastFrame = frame;

    // frame rate and clock error calculation
    frameRateMeasure(frameStartTime, dropFrame);
    framePositionOutput();
}

// called by background loop at the end of each frame when running from a sync signal not timecode
void framePulse(uint32_t frameStartTime)
{
    static int outCount = 0;
    uint32_t patternStartTime;
    if (detectedRate.isValid()) {
        uint32_t ThisSecondStart = frameStartTime ;
        if (!ppsRunning) {
            uint32_t pulseStartTime = frameStartTime + (int)(detectedRate.currentPeriodUS()*5+0.5f);
            uint32_t timeTillStart =  pulseStartTime - inputTimer.read_us();
            ppsRunning = true;
            __disable_irq();
            PPSsyncTimer.attach_us(callback(OnPPSCync), timeTillStart);
            __enable_irq();
            printf("PPS start scheduled for %0.3f seconds.\r\n",
                   timeTillStart / 1000000.0f);
            VBOXTicks = 0;
            outCount = 0;
        } else {
//        if (outCount == 0)
//            patternStartTime = inputTimer.elapsed_time();
//        else {
//            expectedTotalUS = outCount
//        }
        }
    }
    // frame rate and clock error calculation
    frameRateMeasure(frameStartTime, false);
    framePositionOutput();
}

volatile bool NewFramePulse= false;
uint32_t FramePulseTime;
volatile int framesIn = 0;
void OnPPFInputStartup(void)
{
    framesIn++;
}

volatile int SyncInCount = 0;
void OnSyncInputStartup(void)
{
    SyncInCount++;
}


void OnPPFInput(void)
{
    FramePulseTime = inputTimer.read_us();
    NewFramePulse = true;
}


int main()
{
    pc.baud(115200);
    COM1.baud(115200);
    inputTimer.reset();
    inputTimer.start();

    pc.printf("Startup\r\n");
    led1 = 1;

    prepPacketOut();

    LTCInput.setInputTimer(&inputTimer);

    PPFin.rise(callback(&OnPPFInputStartup));

    VIPS.run();

    pc.printf("armed\r\n");
    led2 = 1;

    for (int i = 0; i < _longPPMTrackLen_; i++)
        PPMErrors[i] = 0;
    pc.printf("Startup\r\n");
    PPMTrackIndex = 0;
    PPMTrackTotal = 0;
    remainingClockError = 0;
    bool GenLockOnlyMode = false;
    bool GenLockToSync = false;

    TimeSinceLastFrame.reset();
    TimeSinceLastFrame.start();
    while (true) {
        pc.printf("Waiting for sync\r\n");
        PPFin.rise(callback(&OnPPFInputStartup));
        Syncin.rise(callback(&OnSyncInputStartup));
        framesIn = 0;
        SyncInCount = 0;
        GenLockOnlyMode = false;
        GenLockToSync = false;
        while (!LTCInput.searchForSync()) {
            if (framesIn == 100) {
                GenLockOnlyMode = true;
                break;
            }
            if ((SyncInCount == 100) && (framesIn<45)) { // prefer frame input pin, sync may be twice as high for interlaced systems.
                GenLockOnlyMode = true;
                GenLockToSync= true;
                break;
            }

            int message = VIPS.getStatusMessage();
            if (message)
                pc.printf("%s\r\n",VIPSStatusMessages[message-1]);

        }
        if (GenLockOnlyMode) {
            if (GenLockToSync) {
                pc.printf("No LTC detected for 100 frames. Falling back to genlock on sync input\r\n");
                Syncin.rise(callback(&OnPPFInput));
                PPFin.rise(NULL);
            } else {
                pc.printf("No LTC detected for 100 frames. Falling back to genlock on frame input\r\n");
                PPFin.rise(callback(&OnPPFInput));
                Syncin.rise(NULL);
            }
            uint32_t lastframeTime = inputTimer.read_us();

            while (true) {
                if (NewFramePulse) { // running on a frame
//            printf("frame\r");
                    lastframeTime = FramePulseTime;
                    framePulse(lastframeTime);
                    NewFramePulse = false;
                }
                if ((inputTimer.read_us()-lastframeTime) > 1000000) {
                    pc.printf("No frames for > 1 second\r\n");
                    break;
                }
                int message = VIPS.getStatusMessage();
                if (message)
                    pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
            }
        } else { // not genlock
            pc.printf("Got LTC sync\r\n");
            PPFin.rise(NULL);
            while (LTCInput.synced()) {
                position *posPtr = VIPS.getWaitingPostion();
                if (posPtr) {
                    sendPosition(posPtr);
                }
                if (LTCInput.readWaitingData()) {
                    if (LTCInput.synced()) {
                        const LTCDecode::LTCData_t *frameData = LTCInput.getLastFrame();
                        detectedRate.setDrop(frameData->frameDrop);
                        frameComplete(frameData->hours, frameData->minutes,
                                      frameData->seconds, frameData->frame,
                                      frameData->frameDrop, frameData->frameStartTime);
                    } // end if good frame
                }   // end if new data
                int message = VIPS.getStatusMessage();
                if (message)
                    pc.printf("%s\r\n",VIPSStatusMessages[message-1]);
            }     // end while synced
            pc.printf("Sync lost\r");
        }
    } // end while true
}