syncSlave for problem 2

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