Diff: timesync_slave.cpp

Revision:

0:988132ee7271

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_slave.cpp	Fri Dec 03 20:52:44 2010 +0000
@@ -0,0 +1,457 @@
+#include "mbed.h"
+#include "timesync.h"
+#include "timesync_slave.h"
+#include "hdtimeval_math.h"
+
+int32_t clock_sec;
+DigitalOut led(LED1);
+Serial uart_sync(p28,p27);
+Timeout tmr_sync;
+Timeout tmr_callback;
+
+hdtimeval_t front_lt;
+hdtimeval_t timeq[QUEUE_SIZE];
+void (*funcq[QUEUE_SIZE])(void);
+uint8_t qfront;
+uint8_t qtail;
+
+void (*trigf)(struct timeval *);
+hdtimeval_t trigt;
+//bool trigactive;
+
+//syncdata_t syncdata[2];
+//state_t state;
+synctype_t syncbusy;
+synctype_t tmp_synctype;
+hdtimeval_t txstamp;
+
+coeff_t coeff;
+
+void clock_init();
+void tmr1_irq();
+void uart_recv();
+__INLINE void getHDTime(struct hdtimeval * hdtv);
+void sync_req();
+void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp);
+void getLocalTime(hdtimeval * gt, hdtimeval * lt);
+void getGlobalTime(hdtimeval * lt, hdtimeval * gt);
+void runTriggerFunc();
+
+void timesync_init()
+{
+    wait_ms(100);
+    coeff.v1div2.tv_sec = 0;
+    coeff.v1div2.ticks = 0;
+    coeff.w1div2.tv_sec = 0;
+    coeff.w1div2.ticks = 0;
+    coeff.beta1.tv_sec = 0;
+    coeff.beta1.ticks = 1;
+    coeff.beta2.tv_sec = 0;
+    coeff.beta2.ticks = 1;
+    coeff.size = 0;
+    coeff.front = 0;
+    trigf = NULL;
+    
+    clock_init();
+    uart_sync.baud(115200);
+    uart_sync.attach(&uart_recv, Serial::RxIrq);
+
+    syncbusy = SYNCTYPE_NONE;
+    tmp_synctype = SYNCTYPE_64;
+    tmr_sync.attach(&sync_req, 0.1);
+}
+/*
+void timesync_poll()
+{
+    //static last_poll;
+    tmp_synctype = SYNCTYPE_24;
+    sync_req();
+}
+*/
+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 == front_lt.tv_sec) {
+            hdtimeval_t * nowtv = timeq+qfront;
+            funcq[qfront]();
+            qfront = (qfront+1)%QUEUE_SIZE;
+            while(qfront != qtail 
+            && nowtv->tv_sec == timeq[qfront].tv_sec
+            && nowtv->ticks == timeq[qfront].ticks) {
+                funcq[qfront]();
+                qfront = (qfront+1)%QUEUE_SIZE;
+            }
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                getLocalTime(&timeq[qfront], &front_lt);
+                TMR->MR1 = front_lt.ticks;
+            }
+        } else if (clock_sec > front_lt.tv_sec) {
+            // we missed it!
+            qfront = (qfront+1)%QUEUE_SIZE;
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                getLocalTime(&timeq[qfront], &front_lt);
+                TMR->MR1 = front_lt.ticks;
+            }
+        }
+        TMR->IR = 0x2;
+    }
+}
+
+void runTriggerFunc()
+{
+    timeval_t t;
+    getGlobalTime(&trigt, &trigt);
+    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;
+        void * pkt_p = NULL;
+        uint8_t len = 0;
+        
+        if (!syncbusy) {
+            len = uart_sync.getc();
+            return;
+        }
+        
+        switch(syncbusy) {
+            case SYNCTYPE_64:
+                data = pkt64.raw;
+                pkt_p = (void *)&pkt64;
+                len = 10;
+                break;
+            case SYNCTYPE_32:
+                data = pkt32.raw;
+                pkt_p = (void *)&pkt32;
+                len = 6;
+                break;
+            case SYNCTYPE_24:
+                pkt24.no_use = 0;
+                pkt_p = (void *)&pkt24;
+                data = pkt24.raw;
+                len = 5;
+                break;
+        }
+        while(len-->0) {
+            //while(!(LPC_UART2->LSR & 0x01));
+            *(data++)= uart_sync.getc(); //LPC_UART2->RBR;
+        }
+        
+        update_data(syncbusy, pkt_p, &tstamp);
+    }
+    syncbusy = SYNCTYPE_NONE;
+}
+
+void sync_req()
+{
+    //if (syncbusy) return;
+    __disable_irq();
+    txstamp.tv_sec = clock_sec;
+    txstamp.ticks = TMR->TC;
+    uart_sync.putc(tmp_synctype);
+    //LPC_UART2->THR = tmp_synctype;
+    __enable_irq();
+    syncbusy = tmp_synctype;
+}
+
+void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp)
+{
+    hdtimeval_t v, w;
+    hdtimeval_t master_rxt, master_txt;
+    int32_t ticks_diff;
+    hdtv_add(&v, &txstamp, rxstamp);
+    switch (t) {
+        case SYNCTYPE_64: {
+            sync_pkt64_t * p = (sync_pkt64_t *)pkt_p;
+            master_rxt = p->rx_time;
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
+        } break;
+        case SYNCTYPE_32: {
+            sync_pkt32_t * p = (sync_pkt32_t *)pkt_p;
+            hdtimeval_t tmp;
+            getGlobalTime(&txstamp, &tmp);
+            if (tmp.ticks > MAX_TICK/2) {
+                if (p->rx_time > tmp.ticks-MAX_TICK/2) {
+                    master_rxt.tv_sec = tmp.tv_sec;
+                } else {
+                    master_rxt.tv_sec = tmp.tv_sec +1;
+                }
+            } else {
+                if (p->rx_time > tmp.ticks+MAX_TICK/2) {
+                    master_rxt.tv_sec = tmp.tv_sec-1;
+                } else {
+                    master_rxt.tv_sec = tmp.tv_sec;
+                }
+            }
+            master_rxt.ticks = p->rx_time;
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
+        } break;
+        case SYNCTYPE_24: {
+            sync_pkt24_t * p = (sync_pkt24_t *)pkt_p;
+            hdtimeval_t tmp;
+            getGlobalTime(&txstamp, &tmp);
+            p->rx_time >>= 8;
+            if (tmp.ticks > (1<<23)) {
+                tmp.ticks = tmp.ticks-(1<<23);
+                master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
+                master_rxt.tv_sec = tmp.tv_sec;
+            } else {
+                tmp.ticks = tmp.ticks+MAX_TICK-(1<<23);
+                master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
+                master_rxt.tv_sec = tmp.tv_sec-1;
+            }
+            if (master_rxt.ticks < tmp.ticks) {
+                master_rxt.ticks += 0x1000000;
+                if (master_rxt.ticks > MAX_TICK) {
+                    master_rxt.ticks &= 0xFFFFFF;
+                    master_rxt.tv_sec ++;
+                }
+            }
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;            
+        } break;
+    }
+    //printf("t1,t2: %d,%d, %d,%d \r\n",master_rxt.tv_sec, master_rxt.ticks>>24, master_rxt.ticks & 0xFFFFFF, master_txt.ticks);
+    if (master_rxt.ticks > master_txt.ticks) {
+        master_txt.ticks += 0x10000;
+        if (master_txt.ticks >= MAX_TICK) {
+            master_txt.ticks &= 0xFFFF;
+            master_txt.tv_sec ++;
+        }
+    }
+    hdtv_add(&w, &master_rxt, &master_txt);
+
+    hdtv_div2(&v, &v);
+    hdtv_div2(&w, &w);
+    // evaluate performance
+    {
+        hdtimeval_t tmp;
+        getGlobalTime(&v, &tmp);
+        hdtv_sub(&tmp, &tmp, &w);
+        if (tmp.tv_sec > 10) 
+            ticks_diff = 0x7FFFFFFF;
+        else {
+            ticks_diff = tmp.tv_sec * MAX_TICK + tmp.ticks;
+        }
+        //printf("perf: %d \r\n", ticks_diff);
+    }
+    if (coeff.size)
+        if (coeff.size<HISTORY_LEN) {
+            hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[0]));
+            hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[0]));
+        } else {
+            hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[coeff.front]));
+            hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[coeff.front]));
+        }
+    /*if(coeff.size>=8) {
+        hdtimeval_t tmp;
+        hdtv_div8(&tmp, &(coeff.hisw[coeff.front]));
+        hdtv_sub(&coeff.w1div2, &coeff.w1div2, &tmp);
+        hdtv_div8(&tmp, &(coeff.hisv[coeff.front]));
+        hdtv_sub(&coeff.v1div2, &coeff.v1div2, &tmp);
+        hdtv_div8(&tmp, &w);
+        hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
+        hdtv_div8(&tmp, &v);
+        hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
+    } else {*/
+        coeff.v1div2 = v;
+        coeff.w1div2 = w;
+    //}
+    coeff.hisw[coeff.front] = w;
+    coeff.hisv[coeff.front] = v;
+    coeff.front = (coeff.front+1) % HISTORY_LEN;
+    if (coeff.size<=HISTORY_LEN) coeff.size++;
+    /*if (coeff.size==8) {
+        hdtimeval_t tmp;
+        coeff.w1div2.tv_sec = 0;
+        coeff.w1div2.ticks = 0;
+        coeff.v1div2.tv_sec = 0;
+        coeff.v1div2.ticks = 0;
+        for (uint8_t i=0; i<8; i++) {
+            hdtv_div8(&tmp, &(coeff.hisw[i]));
+            hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
+            hdtv_div8(&tmp, &(coeff.hisv[i]));
+            hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
+        }
+        printf("updated!\r\n");
+    }*/
+    //printf("w/v: %d, %d \r\n", coeff.w1div2.ticks, coeff.v1div2.ticks);
+    if (qfront != qtail) {
+        getLocalTime(&timeq[qfront], &front_lt);
+        TMR->MR1 = front_lt.ticks;
+        TMR->MCR |= (1<<3);
+    }
+    // schedule next sync
+    ticks_diff = abs(ticks_diff);
+    if (ticks_diff < 200) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 60);
+    } else if (ticks_diff < 500) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 30);
+    } else if (ticks_diff < 800) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 20);
+    } else if (ticks_diff < 5000) {
+        tmp_synctype = SYNCTYPE_32;
+        tmr_sync.attach(&sync_req, 10);
+    } else {
+        tmp_synctype = SYNCTYPE_64;
+        tmr_sync.attach(&sync_req, 5);
+    }
+    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, gstamp, tstamp1; 
+    static hdtimeval_t comp_delay = {0,0};
+
+    getHDTime(&tstamp);
+    
+    hdtv_add(&gstamp, &tstamp, &comp_delay);
+    getGlobalTime(&gstamp, &gstamp);
+    
+    getHDTime(&tstamp1);
+    hdtv_sub(&comp_delay, &tstamp1, &tstamp);
+    //printf("delay: %d\r\n", comp_delay.ticks);
+
+    tv->tv_sec = gstamp.tv_sec;
+    tv->tv_usec = ((gstamp.ticks+48) >> 5) / 3;
+    return; 
+}
+
+void getGlobalTime(hdtimeval * lt, hdtimeval * gt)
+{
+    hdtimeval_t tmp;
+    hdtv_sub(&tmp, lt, &coeff.v1div2);
+    hdtv_muldiv(&tmp, &tmp, &coeff.beta1, &coeff.beta2);
+    hdtv_add(gt, &tmp, &coeff.w1div2);
+}
+
+void getLocalTime(hdtimeval * gt, hdtimeval * lt)
+{
+    hdtimeval_t tmp;
+    hdtv_sub(&tmp, gt, &coeff.w1div2);
+    hdtv_muldiv(&tmp, &tmp, &coeff.beta2, &coeff.beta1);
+    hdtv_add(lt, &tmp, &coeff.v1div2);
+}
+
+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;
+    //getLocalTime(&hdtv, &hdtv);
+    ret = enqueue(schedFunc, &hdtv);
+    if (qfront != qtail) {
+        getLocalTime(&timeq[qfront], &front_lt);
+        TMR->MR1 = front_lt.ticks;
+        TMR->MCR |= (1<<3);
+    }
+    return ret;
+}
+
+
+void runAtTrigger(void (*trigFunc)(struct timeval *tv))
+{
+    trigf = trigFunc;
+}