syncMaster for problem 3
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 29 2022 08:06:44 by
1.7.2