Revision 0:6a0716eb2e73, committed 2010-12-03

Comitter:

the729

Date:

Fri Dec 03 20:52:00 2010 +0000

Commit message:

working

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.cpp	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,67 @@
+#include "hdtimeval_math.h"
+
+void hdtv_add (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2)
+{
+    r->tv_sec = v1->tv_sec + v2->tv_sec;
+    r->ticks = v1->ticks + v2->ticks;
+    if (r->ticks >= MAX_TICK) {
+        r->tv_sec++;
+        r->ticks -= MAX_TICK;
+    }
+}
+
+void hdtv_div2 (hdtimeval_t * r, hdtimeval_t * v)
+{
+    if (v->tv_sec & 0x1) {
+        r->ticks = ((v->ticks+MAX_TICK)>>1);
+    } else {
+        r->ticks = (v->ticks>>1);
+    }
+    r->tv_sec = (v->tv_sec>>1);
+}
+
+void hdtv_div8 (hdtimeval_t * r, hdtimeval_t * v)
+{
+    r->ticks = ((v->ticks+MAX_TICK*(v->tv_sec & 0x7))>>3);
+    r->tv_sec = (v->tv_sec>>3);
+}
+
+void hdtv_sub (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2)
+{
+    r->tv_sec = v1->tv_sec - v2->tv_sec;
+    if (v1->ticks < v2->ticks) {
+        r->ticks = MAX_TICK + v1->ticks - v2->ticks;
+        r->tv_sec --;
+    } else {
+        r->ticks = v1->ticks - v2->ticks;
+    }
+}
+
+void hdtv_muldiv (hdtimeval_t * r, hdtimeval_t * v, hdtimeval_t * m, hdtimeval_t * d)
+{
+    double dblm, dbld, factor, rsec, rticks;
+    uint32_t irticks, c;
+    int32_t irsec;
+
+    dblm = (double)(m->tv_sec) * (double)MAX_TICK + (double)(m->ticks);
+    dbld = (double)(d->tv_sec) * (double)MAX_TICK + (double)(d->ticks);
+    factor = dblm / dbld;
+    
+    rsec = (double)(v->tv_sec) * factor;
+    rticks = (double)(v->ticks) * factor + (rsec - floor(rsec)) * (double)MAX_TICK;
+    irticks = (uint32_t)rticks;
+    irsec = (int32_t)rsec;
+    c = irticks / MAX_TICK;
+    r->tv_sec = irsec + c;
+    r->ticks = irticks - c * MAX_TICK;
+}
+
+void hdtv_totv(timeval_t * r, hdtimeval_t * v)
+{
+    r->tv_sec = v->tv_sec;
+    r->tv_usec = (v->ticks + 48)/96;
+    if (r->tv_usec >= 1000000) {
+        r->tv_sec ++;
+        r->tv_usec -= 1000000;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.h	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,13 @@
+#ifndef HDTIMEVAL_MATH_H
+#define HDTIMEVAL_MATH_H
+
+#include "timesync.h"
+
+void hdtv_add (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2);
+void hdtv_div2 (hdtimeval_t * r, hdtimeval_t * v);
+void hdtv_div8 (hdtimeval_t * r, hdtimeval_t * v);
+void hdtv_sub (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2);
+void hdtv_muldiv (hdtimeval_t * r, hdtimeval_t * v, hdtimeval_t * m, hdtimeval_t * d);
+void hdtv_totv(timeval_t * r, hdtimeval_t * v);
+
+#endif //HDTIMEVAL_MATH_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "timesync.h"
+
+DigitalOut myled(LED2);
+DigitalOut mypin(p21);
+
+Serial forwarding(p9,p10);
+Serial command(USBTX,USBRX);
+
+void pinToggle()
+{
+    mypin = !mypin;
+    myled = !myled;
+}
+
+void reportToggle(struct timeval * t)
+{
+    uint32_t diff;
+    diff = t->tv_sec * 1000000 + t->tv_usec;
+    command.printf("%u\r\n", diff);
+}
+
+void forward1()
+{
+    command.putc(forwarding.getc());
+}
+
+int main() {
+    enum {
+        IDLE=0, 
+        HOST_INPUT
+    } state;
+    uint8_t c = 0;
+    uint32_t data = 0;
+
+    state  = IDLE;
+    timesync_init();
+    
+    forwarding.attach(&forward1, Serial::RxIrq);
+    runAtTrigger(&reportToggle);
+    while(1)
+    {
+        timeval_t t;
+        switch(state) {
+            case IDLE:
+                forwarding.putc(c = command.getc());
+                if (c == 'S') {
+                    data = 0;
+                    state = HOST_INPUT;
+                }
+                break;
+            case HOST_INPUT:
+                forwarding.putc(c = command.getc());
+                if (c >= '0' && c <= '9') {
+                    data = data * 10 + c-'0';
+                } else if (c == 'E') {
+                    t.tv_sec = data / 1000000;
+                    t.tv_usec = data % 1000000;
+                    runAtTime(&pinToggle, &t);
+                    state = IDLE;
+                }
+                break;
+        }
+    }    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync.h	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,74 @@
+#ifndef TIMESYNC_H
+#define TIMESYNC_H
+#include "mbed.h"
+
+typedef unsigned int uint32_t;
+typedef int int32_t;
+typedef unsigned short uint16_t;
+typedef short int16_t;
+typedef unsigned char uint8_t;
+//typedef char int8_t;
+
+typedef struct timeval {
+    int32_t tv_sec;
+    uint32_t tv_usec;
+} timeval_t;
+
+typedef struct hdtimeval {
+    int32_t tv_sec;
+    uint32_t ticks;
+} hdtimeval_t;
+
+typedef union sync_pkt24 {
+    struct {
+        uint32_t rx_time;
+        uint16_t tx_time;
+    };
+    struct {
+        char no_use;
+        char raw[5];
+    };
+} sync_pkt24_t;
+
+typedef union sync_pkt32 {
+    struct {
+        uint32_t rx_time;
+        uint16_t tx_time;
+    };
+    char raw[6];
+} sync_pkt32_t;
+
+typedef union sync_pkt64 {
+    struct {
+        hdtimeval_t rx_time;
+        uint16_t tx_time;
+    };
+    char raw[10];
+} sync_pkt64_t;
+
+typedef enum {
+    SYNCTYPE_NONE = 0,
+    SYNCTYPE_64 = 0xA0,
+    SYNCTYPE_32 = 0xA3,
+    SYNCTYPE_24 = 0xA5
+} synctype_t;
+
+void timesync_init();
+void getTime(struct timeval * tv);
+int runAtTime(void (*schedFunc)(void), struct timeval *tv);
+void runAtTrigger(void (*trigFunc)(struct timeval *tv));
+
+#define TMR LPC_TIM2
+#define MAX_TICK (SystemCoreClock)
+
+#define PCLK_DEVIDER 1  // PCLK = CCLK
+#define CLK_TIMER 12     // TIMER2 as the timer
+#define CLK_UART 16     // UART2 as uart
+#define PCTIM0 1        // Power Control Timer0
+#define PCTIM1 2        // Power Control Timer1
+#define PCTIM2 22        // Power Control Timer1
+#define PCUART2 24      // Power Control UART2
+
+#define QUEUE_SIZE 16
+
+#endif //TIMESYNC_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_master.cpp	Fri Dec 03 20:52:00 2010 +0000
@@ -0,0 +1,229 @@
+#include "mbed.h"
+#include "timesync.h"
+#include "hdtimeval_math.h"
+
+int32_t clock_sec;
+DigitalOut led(LED1);
+Serial uart_sync(p28,p27);
+Timeout tmr_callback;
+
+hdtimeval_t timeq[QUEUE_SIZE];
+void (*funcq[QUEUE_SIZE])(void);
+uint8_t qfront;
+uint8_t qtail;
+
+void (*trigf)(struct timeval *);
+hdtimeval_t trigt;
+
+void clock_init();
+void tmr1_irq();
+void uart_recv();
+__INLINE void getHDTime(struct hdtimeval * hdtv);
+void runTriggerFunc();
+
+void timesync_init()
+{
+    wait_ms(100);
+    clock_init();
+    uart_sync.baud(115200);
+    uart_sync.attach(&uart_recv, Serial::RxIrq);
+    
+    qfront = 0;
+    qtail = 0;
+    trigf = NULL;
+}
+
+void clock_init()
+{
+    // select PCLK
+    LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
+    // power on timer
+    LPC_SC->PCONP |= (1<<PCTIM2);
+    
+    // reset timer
+    TMR->TCR = 2;
+    TMR->CTCR = 0;
+    TMR->PR = 0;
+    TMR->TC = 1;
+    TMR->MR0 = SystemCoreClock - 1;
+    TMR->MCR = 3;
+    
+    clock_sec = 0;
+
+    // capture on CAP.0        
+    TMR->CCR = 0x07;
+    LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0  | (3<<8);
+    
+    // setup interrupt
+    __enable_irq();
+    NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
+    NVIC_EnableIRQ(TIMER2_IRQn);
+    
+    NVIC_SetPriority(TIMER2_IRQn, 0);
+    NVIC_SetPriority(UART2_IRQn, 1);
+    NVIC_SetPriority(TIMER3_IRQn, 2);
+    NVIC_SetPriority(UART0_IRQn, 2);
+    NVIC_SetPriority(UART3_IRQn, 1);
+
+    // begin freerun
+    TMR->TCR = 1;
+}
+
+void tmr1_irq()
+{
+    if (TMR->IR & (1<<4)) {
+        if (trigf) {
+            trigt.tv_sec = clock_sec;
+            if ((TMR->IR & 0x01) && TMR->CR0 < 256)
+                trigt.tv_sec = clock_sec+1;
+            trigt.ticks = TMR->CR0;
+            //runTriggerFunc();
+            tmr_callback.attach_us(&runTriggerFunc, 10);
+        }
+        TMR->IR = (1<<4);
+    }
+    if (TMR->IR & 0x1) {
+        // Overflow!
+        clock_sec ++;
+        TMR->IR = 0x1;
+    }
+    if (TMR->IR & 0x2) {
+        if (clock_sec == timeq[qfront].tv_sec) {
+            uint32_t nowtick = timeq[qfront].ticks;
+            funcq[qfront]();
+            qfront = (qfront+1)%QUEUE_SIZE;
+            while(qfront != qtail 
+            && clock_sec == timeq[qfront].tv_sec
+            && nowtick == timeq[qfront].ticks) {
+                funcq[qfront]();
+                qfront = (qfront+1)%QUEUE_SIZE;
+            }
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                TMR->MR1 = timeq[qfront].ticks;
+            }
+        } else if (clock_sec > timeq[qfront].tv_sec) {
+            // we missed it!
+            qfront = (qfront+1)%QUEUE_SIZE;
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                TMR->MR1 = timeq[qfront].ticks;
+            }
+        }
+        TMR->IR = 0x2;
+    }
+}
+
+void runTriggerFunc()
+{
+    timeval_t t;
+    hdtv_totv(&t, &trigt);
+    trigf(&t);
+}
+
+void uart_recv()
+{
+    hdtimeval_t tstamp;
+    getHDTime(&tstamp);
+
+    //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
+    if (uart_sync.readable()) {
+        sync_pkt64_t pkt64;
+        sync_pkt32_t pkt32;
+        sync_pkt24_t pkt24;
+        char * data = NULL;
+        uint8_t len = 0;
+        char cmd;
+        
+        cmd = uart_sync.getc();
+        //cmd = LPC_UART2->RBR;
+        //printf("byte: %x \r\n", cmd);
+        switch (cmd) {
+            case SYNCTYPE_64: 
+                pkt64.rx_time = tstamp;
+                pkt64.tx_time = TMR->TC;
+                uart_sync.putc(pkt64.raw[0]);
+                len = 9;
+                data = pkt64.raw + 1;
+                break;
+            case SYNCTYPE_32: 
+                pkt32.rx_time = tstamp.ticks;
+                pkt32.tx_time = TMR->TC; 
+                uart_sync.putc(pkt32.raw[0]);
+                len = 5;
+                data = pkt32.raw + 1;
+                break;
+            case SYNCTYPE_24: 
+                pkt24.rx_time = tstamp.ticks << 8;
+                pkt24.tx_time = TMR->TC; 
+                uart_sync.putc(pkt24.raw[0]);
+                len = 4;
+                data = pkt24.raw + 1;
+                break;
+        }
+        while (len-->0) {
+            uart_sync.putc(*(data++));
+        }
+        //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF);
+    }
+    led = !led;
+}
+
+__INLINE void getHDTime(struct hdtimeval * hdtv)
+{
+    __disable_irq();
+    hdtv->tv_sec = clock_sec;
+    hdtv->ticks = TMR->TC;
+    __enable_irq();
+    return;
+}
+
+void getTime(struct timeval * tv)
+{
+    hdtimeval_t tstamp; 
+
+    getHDTime(&tstamp);
+    hdtv_totv(tv, &tstamp);
+    return; 
+}
+
+int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
+{
+    uint8_t p = qtail;
+    if ((qtail+1)%QUEUE_SIZE == qfront) {
+        return 1;
+    }
+    while(p != qfront) {
+        uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
+        if (tv->tv_sec > timeq[prev].tv_sec 
+        || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
+        timeq[p] = timeq[prev];
+        funcq[p] = funcq[prev];
+        p = prev;
+    } 
+    timeq[p] = *tv;
+    funcq[p] = schedFunc;
+    qtail = (qtail+1)%QUEUE_SIZE;
+    return 0;
+}
+
+int runAtTime(void (*schedFunc)(void), struct timeval *tv)
+{
+    hdtimeval_t hdtv;
+    int ret;
+    hdtv.tv_sec = tv->tv_sec;
+    hdtv.ticks = tv->tv_usec * 96;
+    ret = enqueue(schedFunc, &hdtv);
+    if (qtail != qfront) {
+        TMR->MR1 = timeq[qfront].ticks;
+        TMR->MCR |= (1<<3);
+    }
+    return ret;
+}
+
+void runAtTrigger(void (*trigFunc)(struct timeval *tv))
+{
+    trigf = trigFunc;
+}