syncMaster for problem 3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers timesync_master.cpp Source File

timesync_master.cpp

00001 #include "mbed.h"
00002 #include "timesync.h"
00003 #include "hdtimeval_math.h"
00004 
00005 int32_t clock_sec;
00006 DigitalOut led(LED1);
00007 Serial uart_sync(p28,p27);
00008 Timeout tmr_callback;
00009 
00010 hdtimeval_t timeq[QUEUE_SIZE];
00011 void (*funcq[QUEUE_SIZE])(void);
00012 uint8_t qfront;
00013 uint8_t qtail;
00014 
00015 uint32_t sync_count[3];
00016 
00017 void (*trigf)(struct timeval *);
00018 hdtimeval_t trigt;
00019 
00020 void clock_init();
00021 void tmr1_irq();
00022 void uart_recv();
00023 __INLINE void getHDTime(struct hdtimeval * hdtv);
00024 void runTriggerFunc();
00025 
00026 void timesync_init()
00027 {
00028     sync_count[0]=0;
00029     sync_count[1]=0;
00030     sync_count[2]=0;
00031 
00032     wait_ms(100);
00033     clock_init();
00034     uart_sync.baud(115200);
00035     uart_sync.attach(&uart_recv, Serial::RxIrq);
00036     
00037     qfront = 0;
00038     qtail = 0;
00039     trigf = NULL;
00040 }
00041 
00042 void clock_init()
00043 {
00044     // select PCLK
00045     LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
00046     // power on timer
00047     LPC_SC->PCONP |= (1<<PCTIM2);
00048     
00049     // reset timer
00050     TMR->TCR = 2;
00051     TMR->CTCR = 0;
00052     TMR->PR = 0;
00053     TMR->TC = 1;
00054     TMR->MR0 = SystemCoreClock - 1;
00055     TMR->MCR = 3;
00056     
00057     clock_sec = 0;
00058 
00059     // capture on CAP.0        
00060     TMR->CCR = 0x05;
00061     LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0  | (3<<8);
00062     
00063     // setup interrupt
00064     __enable_irq();
00065     NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
00066     NVIC_EnableIRQ(TIMER2_IRQn);
00067     
00068     NVIC_SetPriority(TIMER2_IRQn, 0);
00069     NVIC_SetPriority(UART2_IRQn, 1);
00070     NVIC_SetPriority(TIMER3_IRQn, 2);
00071     NVIC_SetPriority(UART0_IRQn, 2);
00072     NVIC_SetPriority(UART3_IRQn, 1);
00073 
00074     // begin freerun
00075     TMR->TCR = 1;
00076 }
00077 
00078 void tmr1_irq()
00079 {
00080     if (TMR->IR & (1<<4)) {
00081         if (trigf) {
00082             trigt.tv_sec = clock_sec;
00083             if ((TMR->IR & 0x01) && TMR->CR0 < 256)
00084                 trigt.tv_sec = clock_sec+1;
00085             trigt.ticks = TMR->CR0;
00086             //runTriggerFunc();
00087             tmr_callback.attach_us(&runTriggerFunc, 10);
00088         }
00089         TMR->IR = (1<<4);
00090     }
00091     if (TMR->IR & 0x1) {
00092         // Overflow!
00093         clock_sec ++;
00094         TMR->IR = 0x1;
00095     }
00096     if (TMR->IR & 0x2) {
00097         if (clock_sec == timeq[qfront].tv_sec) {
00098             uint32_t nowtick = timeq[qfront].ticks;
00099             funcq[qfront]();
00100             qfront = (qfront+1)%QUEUE_SIZE;
00101             while(qfront != qtail 
00102             && clock_sec == timeq[qfront].tv_sec
00103             && nowtick == timeq[qfront].ticks) {
00104                 funcq[qfront]();
00105                 qfront = (qfront+1)%QUEUE_SIZE;
00106             }
00107             if (qfront == qtail) {
00108                 TMR->MCR &= ~(1<<3);
00109             } else {
00110                 TMR->MR1 = timeq[qfront].ticks;
00111             }
00112         } else if (clock_sec > timeq[qfront].tv_sec) {
00113             // we missed it!
00114             printf("miss\r\n");
00115             qfront = (qfront+1)%QUEUE_SIZE;
00116             if (qfront == qtail) {
00117                 TMR->MCR &= ~(1<<3);
00118             } else {
00119                 TMR->MR1 = timeq[qfront].ticks;
00120             }
00121         }
00122         TMR->IR = 0x2;
00123     }
00124 }
00125 
00126 void runTriggerFunc()
00127 {
00128     timeval_t t;
00129     hdtv_totv(&t, &trigt);
00130     trigf(&t);
00131 }
00132 
00133 void uart_recv()
00134 {
00135     hdtimeval_t tstamp;
00136     getHDTime(&tstamp);
00137 
00138     //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
00139     if (uart_sync.readable()) {
00140         sync_pkt64_t pkt64;
00141         sync_pkt32_t pkt32;
00142         sync_pkt24_t pkt24;
00143         char * data = NULL;
00144         uint8_t len = 0;
00145         char cmd;
00146         
00147         cmd = uart_sync.getc();
00148         //cmd = LPC_UART2->RBR;
00149         //printf("byte: %x \r\n", cmd);
00150         switch (cmd) {
00151             case SYNCTYPE_64: 
00152                 pkt64.rx_time = tstamp;
00153                 pkt64.tx_time = TMR->TC;
00154                 uart_sync.putc(pkt64.raw[0]);
00155                 len = 9;
00156                 data = pkt64.raw + 1;
00157                 sync_count[0]++;
00158                 break;
00159             case SYNCTYPE_32: 
00160                 pkt32.rx_time = tstamp.ticks;
00161                 pkt32.tx_time = TMR->TC; 
00162                 uart_sync.putc(pkt32.raw[0]);
00163                 len = 5;
00164                 data = pkt32.raw + 1;
00165                 sync_count[1]++;
00166                 break;
00167             case SYNCTYPE_24: 
00168                 pkt24.rx_time = tstamp.ticks << 8;
00169                 pkt24.tx_time = TMR->TC; 
00170                 uart_sync.putc(pkt24.raw[0]);
00171                 len = 4;
00172                 data = pkt24.raw + 1;
00173                 sync_count[2]++;
00174                 break;
00175         }
00176         while (len-->0) {
00177             uart_sync.putc(*(data++));
00178         }
00179         //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF);
00180     }
00181     led = !led;
00182 }
00183 
00184 __INLINE void getHDTime(struct hdtimeval * hdtv)
00185 {
00186     __disable_irq();
00187     hdtv->tv_sec = clock_sec;
00188     hdtv->ticks = TMR->TC;
00189     __enable_irq();
00190     return;
00191 }
00192 
00193 void getTime(struct timeval * tv)
00194 {
00195     hdtimeval_t tstamp; 
00196 
00197     getHDTime(&tstamp);
00198     hdtv_totv(tv, &tstamp);
00199     return; 
00200 }
00201 
00202 int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
00203 {
00204     uint8_t p = qtail;
00205     if ((qtail+1)%QUEUE_SIZE == qfront) {
00206         return 1;
00207     }
00208     while(p != qfront) {
00209         uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
00210         if (tv->tv_sec > timeq[prev].tv_sec 
00211         || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
00212         timeq[p] = timeq[prev];
00213         funcq[p] = funcq[prev];
00214         p = prev;
00215     } 
00216     timeq[p] = *tv;
00217     funcq[p] = schedFunc;
00218     qtail = (qtail+1)%QUEUE_SIZE;
00219     return 0;
00220 }
00221 
00222 int runAtTime(void (*schedFunc)(void), struct timeval *tv)
00223 {
00224     hdtimeval_t hdtv;
00225     int ret;
00226     hdtv.tv_sec = tv->tv_sec;
00227     hdtv.ticks = tv->tv_usec * 96;
00228     ret = enqueue(schedFunc, &hdtv);
00229     if (!ret && qtail != qfront) {
00230         TMR->MR1 = timeq[qfront].ticks;
00231         TMR->MCR |= (1<<3);
00232     }
00233     return ret;
00234 }
00235 
00236 void runAtTrigger(void (*trigFunc)(struct timeval *tv))
00237 {
00238     trigf = trigFunc;
00239 }