syncSlave for problem 3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers timesync_slave.cpp Source File

timesync_slave.cpp

00001 #include "mbed.h"
00002 #include "timesync.h"
00003 #include "timesync_slave.h"
00004 #include "hdtimeval_math.h"
00005 
00006 int32_t clock_sec;
00007 DigitalOut led(LED1);
00008 Serial uart_sync(p28,p27);
00009 Timeout tmr_sync;
00010 Timeout tmr_callback;
00011 
00012 hdtimeval_t front_lt;
00013 hdtimeval_t timeq[QUEUE_SIZE];
00014 void (*funcq[QUEUE_SIZE])(void);
00015 uint8_t qfront;
00016 uint8_t qtail;
00017 
00018 void (*trigf)(struct timeval *);
00019 hdtimeval_t trigt;
00020 //bool trigactive;
00021 
00022 //state_t state;
00023 synctype_t syncbusy;
00024 synctype_t tmp_synctype;
00025 hdtimeval_t txstamp;
00026 
00027 coeff_t coeff;
00028 
00029 void clock_init();
00030 void tmr1_irq();
00031 void uart_recv();
00032 __INLINE void getHDTime(struct hdtimeval * hdtv);
00033 void sync_req();
00034 void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp);
00035 void getLocalTime(hdtimeval * gt, hdtimeval * lt);
00036 void getGlobalTime(hdtimeval * lt, hdtimeval * gt);
00037 void runTriggerFunc();
00038 
00039 void timesync_init()
00040 {
00041     wait_ms(100);
00042     coeff.v1div2.tv_sec = 0;
00043     coeff.v1div2.ticks = 0;
00044     coeff.w1div2.tv_sec = 0;
00045     coeff.w1div2.ticks = 0;
00046     coeff.beta1.tv_sec = 0;
00047     coeff.beta1.ticks = 1;
00048     coeff.beta2.tv_sec = 0;
00049     coeff.beta2.ticks = 1;
00050     coeff.size = 0;
00051     coeff.front = 0;
00052     trigf = NULL;
00053     
00054     clock_init();
00055     uart_sync.baud(115200);
00056     uart_sync.attach(&uart_recv, Serial::RxIrq);
00057 
00058     syncbusy = SYNCTYPE_NONE;
00059     tmp_synctype = SYNCTYPE_64;
00060     tmr_sync.attach(&sync_req, 0.1);
00061 }
00062 /*
00063 void timesync_poll()
00064 {
00065     //static last_poll;
00066     tmp_synctype = SYNCTYPE_24;
00067     sync_req();
00068 }
00069 */
00070 void clock_init()
00071 {
00072     // select PCLK
00073     LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
00074     // power on timer
00075     LPC_SC->PCONP |= (1<<PCTIM2);
00076     
00077     // reset timer
00078     TMR->TCR = 2;
00079     TMR->CTCR = 0;
00080     TMR->PR = 0;
00081     TMR->TC = 1;
00082     TMR->MR0 = SystemCoreClock - 1;
00083     TMR->MCR = 3;
00084     
00085     clock_sec = 0;
00086 
00087     // capture on CAP.0        
00088     TMR->CCR = 0x07;
00089     LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0  | (3<<8);
00090     
00091     // setup interrupt
00092     __enable_irq();
00093     NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
00094     NVIC_EnableIRQ(TIMER2_IRQn);
00095     
00096     NVIC_SetPriority(TIMER2_IRQn, 0);
00097     NVIC_SetPriority(UART2_IRQn, 1);
00098     NVIC_SetPriority(TIMER3_IRQn, 2);
00099     NVIC_SetPriority(UART0_IRQn, 2);
00100     NVIC_SetPriority(UART3_IRQn, 1);
00101 
00102     // begin freerun
00103     TMR->TCR = 1;
00104 }
00105 
00106 void tmr1_irq()
00107 {
00108     if (TMR->IR & (1<<4)) {
00109         if (trigf) {
00110             trigt.tv_sec = clock_sec;
00111             if ((TMR->IR & 0x01) && TMR->CR0 < 256)
00112                 trigt.tv_sec = clock_sec+1;
00113             trigt.ticks = TMR->CR0;
00114             //runTriggerFunc();
00115             tmr_callback.attach_us(&runTriggerFunc, 10);
00116         }
00117         TMR->IR = (1<<4);
00118     }
00119     if (TMR->IR & 0x1) {
00120         // Overflow!
00121         clock_sec ++;
00122         TMR->IR = 0x1;
00123     }
00124     if (TMR->IR & 0x2) {
00125         if (clock_sec == front_lt.tv_sec) {
00126             hdtimeval_t * nowtv = timeq+qfront;
00127             funcq[qfront]();
00128             qfront = (qfront+1)%QUEUE_SIZE;
00129             while(qfront != qtail 
00130             && nowtv->tv_sec == timeq[qfront].tv_sec
00131             && nowtv->ticks == timeq[qfront].ticks) {
00132                 funcq[qfront]();
00133                 qfront = (qfront+1)%QUEUE_SIZE;
00134             }
00135             if (qfront == qtail) {
00136                 TMR->MCR &= ~(1<<3);
00137             } else {
00138                 getLocalTime(&timeq[qfront], &front_lt);
00139                 TMR->MR1 = front_lt.ticks;
00140             }
00141         } else if (clock_sec > front_lt.tv_sec) {
00142             // we missed it!
00143             qfront = (qfront+1)%QUEUE_SIZE;
00144             if (qfront == qtail) {
00145                 TMR->MCR &= ~(1<<3);
00146             } else {
00147                 getLocalTime(&timeq[qfront], &front_lt);
00148                 TMR->MR1 = front_lt.ticks;
00149             }
00150         }
00151         TMR->IR = 0x2;
00152     }
00153 }
00154 
00155 void runTriggerFunc()
00156 {
00157     timeval_t t;
00158     getGlobalTime(&trigt, &trigt);
00159     hdtv_totv(&t, &trigt);
00160     trigf(&t);
00161 }
00162 
00163 void uart_recv()
00164 {
00165     hdtimeval_t tstamp;
00166     getHDTime(&tstamp);
00167     
00168     //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
00169     if (uart_sync.readable()) {
00170         sync_pkt64_t pkt64;
00171         sync_pkt32_t pkt32;
00172         sync_pkt24_t pkt24;
00173         char * data = NULL;
00174         void * pkt_p = NULL;
00175         uint8_t len = 0;
00176         
00177         if (!syncbusy) {
00178             len = uart_sync.getc();
00179             return;
00180         }
00181         
00182         switch(syncbusy) {
00183             case SYNCTYPE_64:
00184                 data = pkt64.raw;
00185                 pkt_p = (void *)&pkt64;
00186                 len = 10;
00187                 break;
00188             case SYNCTYPE_32:
00189                 data = pkt32.raw;
00190                 pkt_p = (void *)&pkt32;
00191                 len = 6;
00192                 break;
00193             case SYNCTYPE_24:
00194                 pkt24.no_use = 0;
00195                 pkt_p = (void *)&pkt24;
00196                 data = pkt24.raw;
00197                 len = 5;
00198                 break;
00199         }
00200         while(len-->0) {
00201             //while(!(LPC_UART2->LSR & 0x01));
00202             *(data++)= uart_sync.getc(); //LPC_UART2->RBR;
00203         }
00204         
00205         update_data(syncbusy, pkt_p, &tstamp);
00206     }
00207     syncbusy = SYNCTYPE_NONE;
00208 }
00209 
00210 void sync_req()
00211 {
00212     //if (syncbusy) return;
00213     __disable_irq();
00214     txstamp.tv_sec = clock_sec;
00215     txstamp.ticks = TMR->TC;
00216     uart_sync.putc(tmp_synctype);
00217     //LPC_UART2->THR = tmp_synctype;
00218     __enable_irq();
00219     syncbusy = tmp_synctype;
00220 }
00221 
00222 void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp)
00223 {
00224     hdtimeval_t v, w;
00225     hdtimeval_t master_rxt, master_txt;
00226     int32_t ticks_diff;
00227     hdtv_add(&v, &txstamp, rxstamp);
00228     switch (t) {
00229         case SYNCTYPE_64: {
00230             sync_pkt64_t * p = (sync_pkt64_t *)pkt_p;
00231             master_rxt = p->rx_time;
00232             master_txt.tv_sec = master_rxt.tv_sec;
00233             master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
00234         } break;
00235         case SYNCTYPE_32: {
00236             sync_pkt32_t * p = (sync_pkt32_t *)pkt_p;
00237             hdtimeval_t tmp;
00238             getGlobalTime(&txstamp, &tmp);
00239             if (tmp.ticks > MAX_TICK/2) {
00240                 if (p->rx_time > tmp.ticks-MAX_TICK/2) {
00241                     master_rxt.tv_sec = tmp.tv_sec;
00242                 } else {
00243                     master_rxt.tv_sec = tmp.tv_sec +1;
00244                 }
00245             } else {
00246                 if (p->rx_time > tmp.ticks+MAX_TICK/2) {
00247                     master_rxt.tv_sec = tmp.tv_sec-1;
00248                 } else {
00249                     master_rxt.tv_sec = tmp.tv_sec;
00250                 }
00251             }
00252             master_rxt.ticks = p->rx_time;
00253             master_txt.tv_sec = master_rxt.tv_sec;
00254             master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
00255         } break;
00256         case SYNCTYPE_24: {
00257             sync_pkt24_t * p = (sync_pkt24_t *)pkt_p;
00258             hdtimeval_t tmp;
00259             getGlobalTime(&txstamp, &tmp);
00260             p->rx_time >>= 8;
00261             if (tmp.ticks > (1<<23)) {
00262                 tmp.ticks = tmp.ticks-(1<<23);
00263                 master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
00264                 master_rxt.tv_sec = tmp.tv_sec;
00265             } else {
00266                 tmp.ticks = tmp.ticks+MAX_TICK-(1<<23);
00267                 master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
00268                 master_rxt.tv_sec = tmp.tv_sec-1;
00269             }
00270             if (master_rxt.ticks < tmp.ticks) {
00271                 master_rxt.ticks += 0x1000000;
00272                 if (master_rxt.ticks > MAX_TICK) {
00273                     master_rxt.ticks &= 0xFFFFFF;
00274                     master_rxt.tv_sec ++;
00275                 }
00276             }
00277             master_txt.tv_sec = master_rxt.tv_sec;
00278             master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;            
00279         } break;
00280     }
00281     //printf("t1,t2: %d,%d, %d,%d \r\n",master_rxt.tv_sec, master_rxt.ticks>>24, master_rxt.ticks & 0xFFFFFF, master_txt.ticks);
00282     if (master_rxt.ticks > master_txt.ticks) {
00283         master_txt.ticks += 0x10000;
00284         if (master_txt.ticks >= MAX_TICK) {
00285             master_txt.ticks &= 0xFFFF;
00286             master_txt.tv_sec ++;
00287         }
00288     }
00289     hdtv_add(&w, &master_rxt, &master_txt);
00290 
00291     hdtv_div2(&v, &v);
00292     hdtv_div2(&w, &w);
00293     // evaluate performance
00294     {
00295         hdtimeval_t tmp;
00296         getGlobalTime(&v, &tmp);
00297         hdtv_sub(&tmp, &tmp, &w);
00298         if (tmp.tv_sec > 10) 
00299             ticks_diff = 0x7FFFFFFF;
00300         else {
00301             ticks_diff = tmp.tv_sec * MAX_TICK + tmp.ticks;
00302         }
00303         //printf("perf: %d \r\n", ticks_diff);
00304     }
00305     if (coeff.size)
00306         if (coeff.size<HISTORY_LEN) {
00307             hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[0]));
00308             hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[0]));
00309         } else {
00310             hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[coeff.front]));
00311             hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[coeff.front]));
00312         }
00313     /*if(coeff.size>=8) {
00314         hdtimeval_t tmp;
00315         hdtv_div8(&tmp, &(coeff.hisw[coeff.front]));
00316         hdtv_sub(&coeff.w1div2, &coeff.w1div2, &tmp);
00317         hdtv_div8(&tmp, &(coeff.hisv[coeff.front]));
00318         hdtv_sub(&coeff.v1div2, &coeff.v1div2, &tmp);
00319         hdtv_div8(&tmp, &w);
00320         hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
00321         hdtv_div8(&tmp, &v);
00322         hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
00323     } else {*/
00324         coeff.v1div2 = v;
00325         coeff.w1div2 = w;
00326     //}
00327     coeff.hisw[coeff.front] = w;
00328     coeff.hisv[coeff.front] = v;
00329     coeff.front = (coeff.front+1) % HISTORY_LEN;
00330     if (coeff.size<=HISTORY_LEN) coeff.size++;
00331     /*if (coeff.size==8) {
00332         hdtimeval_t tmp;
00333         coeff.w1div2.tv_sec = 0;
00334         coeff.w1div2.ticks = 0;
00335         coeff.v1div2.tv_sec = 0;
00336         coeff.v1div2.ticks = 0;
00337         for (uint8_t i=0; i<8; i++) {
00338             hdtv_div8(&tmp, &(coeff.hisw[i]));
00339             hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
00340             hdtv_div8(&tmp, &(coeff.hisv[i]));
00341             hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
00342         }
00343         printf("updated!\r\n");
00344     }*/
00345     //printf("w/v: %d, %d \r\n", coeff.w1div2.ticks, coeff.v1div2.ticks);
00346     if (qfront != qtail) {
00347         getLocalTime(&timeq[qfront], &front_lt);
00348         TMR->MR1 = front_lt.ticks;
00349         TMR->MCR |= (1<<3);
00350     }
00351     // schedule next sync
00352     ticks_diff = abs(ticks_diff);
00353     if (ticks_diff < 200 && coeff.size>8) {
00354         tmp_synctype = SYNCTYPE_24;
00355         tmr_sync.attach(&sync_req, 60);
00356     } else if (ticks_diff < 500 && coeff.size>4) {
00357         tmp_synctype = SYNCTYPE_24;
00358         tmr_sync.attach(&sync_req, 30);
00359     } else if (ticks_diff < 800 && coeff.size>2) {
00360         tmp_synctype = SYNCTYPE_24;
00361         tmr_sync.attach(&sync_req, 20);
00362     } else if (ticks_diff < 5000) {
00363         tmp_synctype = SYNCTYPE_32;
00364         tmr_sync.attach(&sync_req, 10);
00365     } else {
00366         tmp_synctype = SYNCTYPE_64;
00367         tmr_sync.attach(&sync_req, 5);
00368     }
00369     led = !led;
00370 }
00371 
00372 __INLINE void getHDTime(struct hdtimeval * hdtv)
00373 {
00374     __disable_irq();
00375     hdtv->tv_sec = clock_sec;
00376     hdtv->ticks = TMR->TC;
00377     __enable_irq();
00378     return;
00379 }
00380 
00381 void getTime(struct timeval * tv)
00382 {
00383     hdtimeval_t tstamp, gstamp, tstamp1; 
00384     static hdtimeval_t comp_delay = {0,0};
00385 
00386     getHDTime(&tstamp);
00387     
00388     hdtv_add(&gstamp, &tstamp, &comp_delay);
00389     getGlobalTime(&gstamp, &gstamp);
00390     
00391     getHDTime(&tstamp1);
00392     hdtv_sub(&comp_delay, &tstamp1, &tstamp);
00393     //printf("delay: %d\r\n", comp_delay.ticks);
00394 
00395     tv->tv_sec = gstamp.tv_sec;
00396     tv->tv_usec = ((gstamp.ticks+48) >> 5) / 3;
00397     return; 
00398 }
00399 
00400 void getGlobalTime(hdtimeval * lt, hdtimeval * gt)
00401 {
00402     hdtimeval_t tmp;
00403     hdtv_sub(&tmp, lt, &coeff.v1div2);
00404     hdtv_muldiv(&tmp, &tmp, &coeff.beta1, &coeff.beta2);
00405     hdtv_add(gt, &tmp, &coeff.w1div2);
00406 }
00407 
00408 void getLocalTime(hdtimeval * gt, hdtimeval * lt)
00409 {
00410     hdtimeval_t tmp;
00411     hdtv_sub(&tmp, gt, &coeff.w1div2);
00412     hdtv_muldiv(&tmp, &tmp, &coeff.beta2, &coeff.beta1);
00413     hdtv_add(lt, &tmp, &coeff.v1div2);
00414 }
00415 
00416 int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
00417 {
00418     uint8_t p = qtail;
00419     if ((qtail+1)%QUEUE_SIZE == qfront) {
00420         return 1;
00421     }
00422     while(p != qfront) {
00423         uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
00424         if (tv->tv_sec > timeq[prev].tv_sec 
00425         || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
00426         timeq[p] = timeq[prev];
00427         funcq[p] = funcq[prev];
00428         p = prev;
00429     } 
00430     timeq[p] = *tv;
00431     funcq[p] = schedFunc;
00432     qtail = (qtail+1)%QUEUE_SIZE;
00433     return 0;
00434 }
00435 
00436 int runAtTime(void (*schedFunc)(void), struct timeval *tv)
00437 {
00438     hdtimeval_t hdtv;
00439     int ret;
00440     hdtv.tv_sec = tv->tv_sec;
00441     hdtv.ticks = tv->tv_usec * 96;
00442     //getLocalTime(&hdtv, &hdtv);
00443     ret = enqueue(schedFunc, &hdtv);
00444     if (qfront != qtail) {
00445         getLocalTime(&timeq[qfront], &front_lt);
00446         TMR->MR1 = front_lt.ticks;
00447         TMR->MCR |= (1<<3);
00448     }
00449     return ret;
00450 }
00451 
00452 
00453 void runAtTrigger(void (*trigFunc)(struct timeval *tv))
00454 {
00455     trigf = trigFunc;
00456 }