syncSlave for problem 2
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 28 2022 23:22:05 by
1.7.2