Tianji Wu
syncSlave for problem 3
Diff: timesync_slave.cpp
- Revision:
- 0:362932d519c6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_slave.cpp Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,456 @@
+#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;
+
+//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 && coeff.size>8) {
+ tmp_synctype = SYNCTYPE_24;
+ tmr_sync.attach(&sync_req, 60);
+ } else if (ticks_diff < 500 && coeff.size>4) {
+ tmp_synctype = SYNCTYPE_24;
+ tmr_sync.attach(&sync_req, 30);
+ } else if (ticks_diff < 800 && coeff.size>2) {
+ 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;
+}