syncMaster for problem 3

Files at this revision

API Documentation at this revision

Comitter:
the729
Date:
Fri Dec 03 20:53:21 2010 +0000
Commit message:

Changed in this revision

hdtimeval_math.cpp Show annotated file Show diff for this revision Revisions of this file
hdtimeval_math.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
timesync.h Show annotated file Show diff for this revision Revisions of this file
timesync_master.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b9f06a7ae791 hdtimeval_math.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.cpp	Fri Dec 03 20:53:21 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;
+    }
+}
diff -r 000000000000 -r b9f06a7ae791 hdtimeval_math.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.h	Fri Dec 03 20:53:21 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
diff -r 000000000000 -r b9f06a7ae791 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 03 20:53:21 2010 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "timesync.h"
+
+extern uint32_t sync_count[3];
+
+DigitalOut myled(LED2);
+DigitalOut mypin(p21);
+timeval_t tblink, tblink1, ttoggle;
+
+void pinToggle()
+{
+    getTime(&tblink1);
+    mypin = !mypin;
+    myled = !myled;
+}
+
+void reportToggle(struct timeval * t)
+{
+    int32_t diff;
+    diff = (tblink1.tv_sec - t->tv_sec)*1000000+tblink1.tv_usec - t->tv_usec;
+    //diff = t->tv_sec * 1000000 + t->tv_usec;
+    //printf("triggered: %d.%06d ", t->tv_sec, t->tv_usec);
+    //printf("expect: %d.%06d\r\n", texpect.tv_sec, texpect.tv_usec);
+    printf("%d,%d,%d,%d\r\n", diff, sync_count[0],sync_count[1],sync_count[2]);
+}
+
+int main() {
+    timesync_init();
+    
+    tblink.tv_sec = 5;
+    tblink.tv_usec = 500000;
+    runAtTrigger(&reportToggle);
+    while(1)
+    {
+        if (runAtTime(&pinToggle, &tblink)==0) {
+            tblink.tv_sec ++;
+        }
+        wait_ms(20);
+    }    
+}
diff -r 000000000000 -r b9f06a7ae791 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 03 20:53:21 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e
diff -r 000000000000 -r b9f06a7ae791 timesync.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync.h	Fri Dec 03 20:53:21 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
diff -r 000000000000 -r b9f06a7ae791 timesync_master.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_master.cpp	Fri Dec 03 20:53:21 2010 +0000
@@ -0,0 +1,239 @@
+#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;
+
+uint32_t sync_count[3];
+
+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()
+{
+    sync_count[0]=0;
+    sync_count[1]=0;
+    sync_count[2]=0;
+
+    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 = 0x05;
+    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!
+            printf("miss\r\n");
+            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;
+                sync_count[0]++;
+                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;
+                sync_count[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;
+                sync_count[2]++;
+                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 (!ret && qtail != qfront) {
+        TMR->MR1 = timeq[qfront].ticks;
+        TMR->MCR |= (1<<3);
+    }
+    return ret;
+}
+
+void runAtTrigger(void (*trigFunc)(struct timeval *tv))
+{
+    trigf = trigFunc;
+}