syncMaster for problem 2
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 void (*trigf)(struct timeval *); 00016 hdtimeval_t trigt; 00017 00018 void clock_init(); 00019 void tmr1_irq(); 00020 void uart_recv(); 00021 __INLINE void getHDTime(struct hdtimeval * hdtv); 00022 void runTriggerFunc(); 00023 00024 void timesync_init() 00025 { 00026 wait_ms(100); 00027 clock_init(); 00028 uart_sync.baud(115200); 00029 uart_sync.attach(&uart_recv, Serial::RxIrq); 00030 00031 qfront = 0; 00032 qtail = 0; 00033 trigf = NULL; 00034 } 00035 00036 void clock_init() 00037 { 00038 // select PCLK 00039 LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER); 00040 // power on timer 00041 LPC_SC->PCONP |= (1<<PCTIM2); 00042 00043 // reset timer 00044 TMR->TCR = 2; 00045 TMR->CTCR = 0; 00046 TMR->PR = 0; 00047 TMR->TC = 1; 00048 TMR->MR0 = SystemCoreClock - 1; 00049 TMR->MCR = 3; 00050 00051 clock_sec = 0; 00052 00053 // capture on CAP.0 00054 TMR->CCR = 0x07; 00055 LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0 | (3<<8); 00056 00057 // setup interrupt 00058 __enable_irq(); 00059 NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq); 00060 NVIC_EnableIRQ(TIMER2_IRQn); 00061 00062 NVIC_SetPriority(TIMER2_IRQn, 0); 00063 NVIC_SetPriority(UART2_IRQn, 1); 00064 NVIC_SetPriority(TIMER3_IRQn, 2); 00065 NVIC_SetPriority(UART0_IRQn, 2); 00066 NVIC_SetPriority(UART3_IRQn, 1); 00067 00068 // begin freerun 00069 TMR->TCR = 1; 00070 } 00071 00072 void tmr1_irq() 00073 { 00074 if (TMR->IR & (1<<4)) { 00075 if (trigf) { 00076 trigt.tv_sec = clock_sec; 00077 if ((TMR->IR & 0x01) && TMR->CR0 < 256) 00078 trigt.tv_sec = clock_sec+1; 00079 trigt.ticks = TMR->CR0; 00080 //runTriggerFunc(); 00081 tmr_callback.attach_us(&runTriggerFunc, 10); 00082 } 00083 TMR->IR = (1<<4); 00084 } 00085 if (TMR->IR & 0x1) { 00086 // Overflow! 00087 clock_sec ++; 00088 TMR->IR = 0x1; 00089 } 00090 if (TMR->IR & 0x2) { 00091 if (clock_sec == timeq[qfront].tv_sec) { 00092 uint32_t nowtick = timeq[qfront].ticks; 00093 funcq[qfront](); 00094 qfront = (qfront+1)%QUEUE_SIZE; 00095 while(qfront != qtail 00096 && clock_sec == timeq[qfront].tv_sec 00097 && nowtick == timeq[qfront].ticks) { 00098 funcq[qfront](); 00099 qfront = (qfront+1)%QUEUE_SIZE; 00100 } 00101 if (qfront == qtail) { 00102 TMR->MCR &= ~(1<<3); 00103 } else { 00104 TMR->MR1 = timeq[qfront].ticks; 00105 } 00106 } else if (clock_sec > timeq[qfront].tv_sec) { 00107 // we missed it! 00108 qfront = (qfront+1)%QUEUE_SIZE; 00109 if (qfront == qtail) { 00110 TMR->MCR &= ~(1<<3); 00111 } else { 00112 TMR->MR1 = timeq[qfront].ticks; 00113 } 00114 } 00115 TMR->IR = 0x2; 00116 } 00117 } 00118 00119 void runTriggerFunc() 00120 { 00121 timeval_t t; 00122 hdtv_totv(&t, &trigt); 00123 trigf(&t); 00124 } 00125 00126 void uart_recv() 00127 { 00128 hdtimeval_t tstamp; 00129 getHDTime(&tstamp); 00130 00131 //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) { 00132 if (uart_sync.readable()) { 00133 sync_pkt64_t pkt64; 00134 sync_pkt32_t pkt32; 00135 sync_pkt24_t pkt24; 00136 char * data = NULL; 00137 uint8_t len = 0; 00138 char cmd; 00139 00140 cmd = uart_sync.getc(); 00141 //cmd = LPC_UART2->RBR; 00142 //printf("byte: %x \r\n", cmd); 00143 switch (cmd) { 00144 case SYNCTYPE_64: 00145 pkt64.rx_time = tstamp; 00146 pkt64.tx_time = TMR->TC; 00147 uart_sync.putc(pkt64.raw[0]); 00148 len = 9; 00149 data = pkt64.raw + 1; 00150 break; 00151 case SYNCTYPE_32: 00152 pkt32.rx_time = tstamp.ticks; 00153 pkt32.tx_time = TMR->TC; 00154 uart_sync.putc(pkt32.raw[0]); 00155 len = 5; 00156 data = pkt32.raw + 1; 00157 break; 00158 case SYNCTYPE_24: 00159 pkt24.rx_time = tstamp.ticks << 8; 00160 pkt24.tx_time = TMR->TC; 00161 uart_sync.putc(pkt24.raw[0]); 00162 len = 4; 00163 data = pkt24.raw + 1; 00164 break; 00165 } 00166 while (len-->0) { 00167 uart_sync.putc(*(data++)); 00168 } 00169 //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF); 00170 } 00171 led = !led; 00172 } 00173 00174 __INLINE void getHDTime(struct hdtimeval * hdtv) 00175 { 00176 __disable_irq(); 00177 hdtv->tv_sec = clock_sec; 00178 hdtv->ticks = TMR->TC; 00179 __enable_irq(); 00180 return; 00181 } 00182 00183 void getTime(struct timeval * tv) 00184 { 00185 hdtimeval_t tstamp; 00186 00187 getHDTime(&tstamp); 00188 hdtv_totv(tv, &tstamp); 00189 return; 00190 } 00191 00192 int enqueue(void (*schedFunc)(void), hdtimeval_t *tv) 00193 { 00194 uint8_t p = qtail; 00195 if ((qtail+1)%QUEUE_SIZE == qfront) { 00196 return 1; 00197 } 00198 while(p != qfront) { 00199 uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE; 00200 if (tv->tv_sec > timeq[prev].tv_sec 00201 || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break; 00202 timeq[p] = timeq[prev]; 00203 funcq[p] = funcq[prev]; 00204 p = prev; 00205 } 00206 timeq[p] = *tv; 00207 funcq[p] = schedFunc; 00208 qtail = (qtail+1)%QUEUE_SIZE; 00209 return 0; 00210 } 00211 00212 int runAtTime(void (*schedFunc)(void), struct timeval *tv) 00213 { 00214 hdtimeval_t hdtv; 00215 int ret; 00216 hdtv.tv_sec = tv->tv_sec; 00217 hdtv.ticks = tv->tv_usec * 96; 00218 ret = enqueue(schedFunc, &hdtv); 00219 if (qtail != qfront) { 00220 TMR->MR1 = timeq[qfront].ticks; 00221 TMR->MCR |= (1<<3); 00222 } 00223 return ret; 00224 } 00225 00226 void runAtTrigger(void (*trigFunc)(struct timeval *tv)) 00227 { 00228 trigf = trigFunc; 00229 }
Generated on Sun Jul 17 2022 21:17:49 by
1.7.2