Tianji Wu / Mbed 2 deprecated syncMaster_2

syncMaster for problem 2

timesync_master.cpp@0:6a0716eb2e73, 2010-12-03 (annotated)

Committer:
the729
Date:
Fri Dec 03 20:52:00 2010 +0000
Revision:
0:6a0716eb2e73
working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
the729 0:6a0716eb2e73 1 #include "mbed.h"
the729 0:6a0716eb2e73 2 #include "timesync.h"
the729 0:6a0716eb2e73 3 #include "hdtimeval_math.h"
the729 0:6a0716eb2e73 4
the729 0:6a0716eb2e73 5 int32_t clock_sec;
the729 0:6a0716eb2e73 6 DigitalOut led(LED1);
the729 0:6a0716eb2e73 7 Serial uart_sync(p28,p27);
the729 0:6a0716eb2e73 8 Timeout tmr_callback;
the729 0:6a0716eb2e73 9
the729 0:6a0716eb2e73 10 hdtimeval_t timeq[QUEUE_SIZE];
the729 0:6a0716eb2e73 11 void (*funcq[QUEUE_SIZE])(void);
the729 0:6a0716eb2e73 12 uint8_t qfront;
the729 0:6a0716eb2e73 13 uint8_t qtail;
the729 0:6a0716eb2e73 14
the729 0:6a0716eb2e73 15 void (*trigf)(struct timeval *);
the729 0:6a0716eb2e73 16 hdtimeval_t trigt;
the729 0:6a0716eb2e73 17
the729 0:6a0716eb2e73 18 void clock_init();
the729 0:6a0716eb2e73 19 void tmr1_irq();
the729 0:6a0716eb2e73 20 void uart_recv();
the729 0:6a0716eb2e73 21 __INLINE void getHDTime(struct hdtimeval * hdtv);
the729 0:6a0716eb2e73 22 void runTriggerFunc();
the729 0:6a0716eb2e73 23
the729 0:6a0716eb2e73 24 void timesync_init()
the729 0:6a0716eb2e73 25 {
the729 0:6a0716eb2e73 26 wait_ms(100);
the729 0:6a0716eb2e73 27 clock_init();
the729 0:6a0716eb2e73 28 uart_sync.baud(115200);
the729 0:6a0716eb2e73 29 uart_sync.attach(&uart_recv, Serial::RxIrq);
the729 0:6a0716eb2e73 30
the729 0:6a0716eb2e73 31 qfront = 0;
the729 0:6a0716eb2e73 32 qtail = 0;
the729 0:6a0716eb2e73 33 trigf = NULL;
the729 0:6a0716eb2e73 34 }
the729 0:6a0716eb2e73 35
the729 0:6a0716eb2e73 36 void clock_init()
the729 0:6a0716eb2e73 37 {
the729 0:6a0716eb2e73 38 // select PCLK
the729 0:6a0716eb2e73 39 LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
the729 0:6a0716eb2e73 40 // power on timer
the729 0:6a0716eb2e73 41 LPC_SC->PCONP |= (1<<PCTIM2);
the729 0:6a0716eb2e73 42
the729 0:6a0716eb2e73 43 // reset timer
the729 0:6a0716eb2e73 44 TMR->TCR = 2;
the729 0:6a0716eb2e73 45 TMR->CTCR = 0;
the729 0:6a0716eb2e73 46 TMR->PR = 0;
the729 0:6a0716eb2e73 47 TMR->TC = 1;
the729 0:6a0716eb2e73 48 TMR->MR0 = SystemCoreClock - 1;
the729 0:6a0716eb2e73 49 TMR->MCR = 3;
the729 0:6a0716eb2e73 50
the729 0:6a0716eb2e73 51 clock_sec = 0;
the729 0:6a0716eb2e73 52
the729 0:6a0716eb2e73 53 // capture on CAP.0
the729 0:6a0716eb2e73 54 TMR->CCR = 0x07;
the729 0:6a0716eb2e73 55 LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0 | (3<<8);
the729 0:6a0716eb2e73 56
the729 0:6a0716eb2e73 57 // setup interrupt
the729 0:6a0716eb2e73 58 __enable_irq();
the729 0:6a0716eb2e73 59 NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
the729 0:6a0716eb2e73 60 NVIC_EnableIRQ(TIMER2_IRQn);
the729 0:6a0716eb2e73 61
the729 0:6a0716eb2e73 62 NVIC_SetPriority(TIMER2_IRQn, 0);
the729 0:6a0716eb2e73 63 NVIC_SetPriority(UART2_IRQn, 1);
the729 0:6a0716eb2e73 64 NVIC_SetPriority(TIMER3_IRQn, 2);
the729 0:6a0716eb2e73 65 NVIC_SetPriority(UART0_IRQn, 2);
the729 0:6a0716eb2e73 66 NVIC_SetPriority(UART3_IRQn, 1);
the729 0:6a0716eb2e73 67
the729 0:6a0716eb2e73 68 // begin freerun
the729 0:6a0716eb2e73 69 TMR->TCR = 1;
the729 0:6a0716eb2e73 70 }
the729 0:6a0716eb2e73 71
the729 0:6a0716eb2e73 72 void tmr1_irq()
the729 0:6a0716eb2e73 73 {
the729 0:6a0716eb2e73 74 if (TMR->IR & (1<<4)) {
the729 0:6a0716eb2e73 75 if (trigf) {
the729 0:6a0716eb2e73 76 trigt.tv_sec = clock_sec;
the729 0:6a0716eb2e73 77 if ((TMR->IR & 0x01) && TMR->CR0 < 256)
the729 0:6a0716eb2e73 78 trigt.tv_sec = clock_sec+1;
the729 0:6a0716eb2e73 79 trigt.ticks = TMR->CR0;
the729 0:6a0716eb2e73 80 //runTriggerFunc();
the729 0:6a0716eb2e73 81 tmr_callback.attach_us(&runTriggerFunc, 10);
the729 0:6a0716eb2e73 82 }
the729 0:6a0716eb2e73 83 TMR->IR = (1<<4);
the729 0:6a0716eb2e73 84 }
the729 0:6a0716eb2e73 85 if (TMR->IR & 0x1) {
the729 0:6a0716eb2e73 86 // Overflow!
the729 0:6a0716eb2e73 87 clock_sec ++;
the729 0:6a0716eb2e73 88 TMR->IR = 0x1;
the729 0:6a0716eb2e73 89 }
the729 0:6a0716eb2e73 90 if (TMR->IR & 0x2) {
the729 0:6a0716eb2e73 91 if (clock_sec == timeq[qfront].tv_sec) {
the729 0:6a0716eb2e73 92 uint32_t nowtick = timeq[qfront].ticks;
the729 0:6a0716eb2e73 93 funcq[qfront]();
the729 0:6a0716eb2e73 94 qfront = (qfront+1)%QUEUE_SIZE;
the729 0:6a0716eb2e73 95 while(qfront != qtail
the729 0:6a0716eb2e73 96 && clock_sec == timeq[qfront].tv_sec
the729 0:6a0716eb2e73 97 && nowtick == timeq[qfront].ticks) {
the729 0:6a0716eb2e73 98 funcq[qfront]();
the729 0:6a0716eb2e73 99 qfront = (qfront+1)%QUEUE_SIZE;
the729 0:6a0716eb2e73 100 }
the729 0:6a0716eb2e73 101 if (qfront == qtail) {
the729 0:6a0716eb2e73 102 TMR->MCR &= ~(1<<3);
the729 0:6a0716eb2e73 103 } else {
the729 0:6a0716eb2e73 104 TMR->MR1 = timeq[qfront].ticks;
the729 0:6a0716eb2e73 105 }
the729 0:6a0716eb2e73 106 } else if (clock_sec > timeq[qfront].tv_sec) {
the729 0:6a0716eb2e73 107 // we missed it!
the729 0:6a0716eb2e73 108 qfront = (qfront+1)%QUEUE_SIZE;
the729 0:6a0716eb2e73 109 if (qfront == qtail) {
the729 0:6a0716eb2e73 110 TMR->MCR &= ~(1<<3);
the729 0:6a0716eb2e73 111 } else {
the729 0:6a0716eb2e73 112 TMR->MR1 = timeq[qfront].ticks;
the729 0:6a0716eb2e73 113 }
the729 0:6a0716eb2e73 114 }
the729 0:6a0716eb2e73 115 TMR->IR = 0x2;
the729 0:6a0716eb2e73 116 }
the729 0:6a0716eb2e73 117 }
the729 0:6a0716eb2e73 118
the729 0:6a0716eb2e73 119 void runTriggerFunc()
the729 0:6a0716eb2e73 120 {
the729 0:6a0716eb2e73 121 timeval_t t;
the729 0:6a0716eb2e73 122 hdtv_totv(&t, &trigt);
the729 0:6a0716eb2e73 123 trigf(&t);
the729 0:6a0716eb2e73 124 }
the729 0:6a0716eb2e73 125
the729 0:6a0716eb2e73 126 void uart_recv()
the729 0:6a0716eb2e73 127 {
the729 0:6a0716eb2e73 128 hdtimeval_t tstamp;
the729 0:6a0716eb2e73 129 getHDTime(&tstamp);
the729 0:6a0716eb2e73 130
the729 0:6a0716eb2e73 131 //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
the729 0:6a0716eb2e73 132 if (uart_sync.readable()) {
the729 0:6a0716eb2e73 133 sync_pkt64_t pkt64;
the729 0:6a0716eb2e73 134 sync_pkt32_t pkt32;
the729 0:6a0716eb2e73 135 sync_pkt24_t pkt24;
the729 0:6a0716eb2e73 136 char * data = NULL;
the729 0:6a0716eb2e73 137 uint8_t len = 0;
the729 0:6a0716eb2e73 138 char cmd;
the729 0:6a0716eb2e73 139
the729 0:6a0716eb2e73 140 cmd = uart_sync.getc();
the729 0:6a0716eb2e73 141 //cmd = LPC_UART2->RBR;
the729 0:6a0716eb2e73 142 //printf("byte: %x \r\n", cmd);
the729 0:6a0716eb2e73 143 switch (cmd) {
the729 0:6a0716eb2e73 144 case SYNCTYPE_64:
the729 0:6a0716eb2e73 145 pkt64.rx_time = tstamp;
the729 0:6a0716eb2e73 146 pkt64.tx_time = TMR->TC;
the729 0:6a0716eb2e73 147 uart_sync.putc(pkt64.raw[0]);
the729 0:6a0716eb2e73 148 len = 9;
the729 0:6a0716eb2e73 149 data = pkt64.raw + 1;
the729 0:6a0716eb2e73 150 break;
the729 0:6a0716eb2e73 151 case SYNCTYPE_32:
the729 0:6a0716eb2e73 152 pkt32.rx_time = tstamp.ticks;
the729 0:6a0716eb2e73 153 pkt32.tx_time = TMR->TC;
the729 0:6a0716eb2e73 154 uart_sync.putc(pkt32.raw[0]);
the729 0:6a0716eb2e73 155 len = 5;
the729 0:6a0716eb2e73 156 data = pkt32.raw + 1;
the729 0:6a0716eb2e73 157 break;
the729 0:6a0716eb2e73 158 case SYNCTYPE_24:
the729 0:6a0716eb2e73 159 pkt24.rx_time = tstamp.ticks << 8;
the729 0:6a0716eb2e73 160 pkt24.tx_time = TMR->TC;
the729 0:6a0716eb2e73 161 uart_sync.putc(pkt24.raw[0]);
the729 0:6a0716eb2e73 162 len = 4;
the729 0:6a0716eb2e73 163 data = pkt24.raw + 1;
the729 0:6a0716eb2e73 164 break;
the729 0:6a0716eb2e73 165 }
the729 0:6a0716eb2e73 166 while (len-->0) {
the729 0:6a0716eb2e73 167 uart_sync.putc(*(data++));
the729 0:6a0716eb2e73 168 }
the729 0:6a0716eb2e73 169 //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF);
the729 0:6a0716eb2e73 170 }
the729 0:6a0716eb2e73 171 led = !led;
the729 0:6a0716eb2e73 172 }
the729 0:6a0716eb2e73 173
the729 0:6a0716eb2e73 174 __INLINE void getHDTime(struct hdtimeval * hdtv)
the729 0:6a0716eb2e73 175 {
the729 0:6a0716eb2e73 176 __disable_irq();
the729 0:6a0716eb2e73 177 hdtv->tv_sec = clock_sec;
the729 0:6a0716eb2e73 178 hdtv->ticks = TMR->TC;
the729 0:6a0716eb2e73 179 __enable_irq();
the729 0:6a0716eb2e73 180 return;
the729 0:6a0716eb2e73 181 }
the729 0:6a0716eb2e73 182
the729 0:6a0716eb2e73 183 void getTime(struct timeval * tv)
the729 0:6a0716eb2e73 184 {
the729 0:6a0716eb2e73 185 hdtimeval_t tstamp;
the729 0:6a0716eb2e73 186
the729 0:6a0716eb2e73 187 getHDTime(&tstamp);
the729 0:6a0716eb2e73 188 hdtv_totv(tv, &tstamp);
the729 0:6a0716eb2e73 189 return;
the729 0:6a0716eb2e73 190 }
the729 0:6a0716eb2e73 191
the729 0:6a0716eb2e73 192 int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
the729 0:6a0716eb2e73 193 {
the729 0:6a0716eb2e73 194 uint8_t p = qtail;
the729 0:6a0716eb2e73 195 if ((qtail+1)%QUEUE_SIZE == qfront) {
the729 0:6a0716eb2e73 196 return 1;
the729 0:6a0716eb2e73 197 }
the729 0:6a0716eb2e73 198 while(p != qfront) {
the729 0:6a0716eb2e73 199 uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
the729 0:6a0716eb2e73 200 if (tv->tv_sec > timeq[prev].tv_sec
the729 0:6a0716eb2e73 201 || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
the729 0:6a0716eb2e73 202 timeq[p] = timeq[prev];
the729 0:6a0716eb2e73 203 funcq[p] = funcq[prev];
the729 0:6a0716eb2e73 204 p = prev;
the729 0:6a0716eb2e73 205 }
the729 0:6a0716eb2e73 206 timeq[p] = *tv;
the729 0:6a0716eb2e73 207 funcq[p] = schedFunc;
the729 0:6a0716eb2e73 208 qtail = (qtail+1)%QUEUE_SIZE;
the729 0:6a0716eb2e73 209 return 0;
the729 0:6a0716eb2e73 210 }
the729 0:6a0716eb2e73 211
the729 0:6a0716eb2e73 212 int runAtTime(void (*schedFunc)(void), struct timeval *tv)
the729 0:6a0716eb2e73 213 {
the729 0:6a0716eb2e73 214 hdtimeval_t hdtv;
the729 0:6a0716eb2e73 215 int ret;
the729 0:6a0716eb2e73 216 hdtv.tv_sec = tv->tv_sec;
the729 0:6a0716eb2e73 217 hdtv.ticks = tv->tv_usec * 96;
the729 0:6a0716eb2e73 218 ret = enqueue(schedFunc, &hdtv);
the729 0:6a0716eb2e73 219 if (qtail != qfront) {
the729 0:6a0716eb2e73 220 TMR->MR1 = timeq[qfront].ticks;
the729 0:6a0716eb2e73 221 TMR->MCR |= (1<<3);
the729 0:6a0716eb2e73 222 }
the729 0:6a0716eb2e73 223 return ret;
the729 0:6a0716eb2e73 224 }
the729 0:6a0716eb2e73 225
the729 0:6a0716eb2e73 226 void runAtTrigger(void (*trigFunc)(struct timeval *tv))
the729 0:6a0716eb2e73 227 {
the729 0:6a0716eb2e73 228 trigf = trigFunc;
the729 0:6a0716eb2e73 229 }