syncMaster_3 - syncMaster for problem 3

Users » the729 » Code » syncMaster_3

Tianji Wu / Mbed 2 deprecated syncMaster_3

syncMaster for problem 3

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

Committer:: the729
Date:: Fri Dec 03 20:53:21 2010 +0000
Revision:: 0:b9f06a7ae791

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Dec 2010
Imports:	0
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning