syncSlave for problem 3
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 //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 }
Generated on Fri Jul 15 2022 14:57:22 by
1.7.2