syncSlave_2 - syncSlave for problem 2

Users » the729 » Code » syncSlave_2

Tianji Wu / Mbed 2 deprecated syncSlave_2

syncSlave for problem 2

timesync_slave.cpp@0:988132ee7271, 2010-12-03 (annotated)

Committer:: the729
Date:: Fri Dec 03 20:52:44 2010 +0000
Revision:: 0:988132ee7271

Who changed what in which revision?

User	Revision	Line number	New contents of line
the729	0:988132ee7271	1	#include "mbed.h"
the729	0:988132ee7271	2	#include "timesync.h"
the729	0:988132ee7271	3	#include "timesync_slave.h"
the729	0:988132ee7271	4	#include "hdtimeval_math.h"
the729	0:988132ee7271	5
the729	0:988132ee7271	6	int32_t clock_sec;
the729	0:988132ee7271	7	DigitalOut led(LED1);
the729	0:988132ee7271	8	Serial uart_sync(p28,p27);
the729	0:988132ee7271	9	Timeout tmr_sync;
the729	0:988132ee7271	10	Timeout tmr_callback;
the729	0:988132ee7271	11
the729	0:988132ee7271	12	hdtimeval_t front_lt;
the729	0:988132ee7271	13	hdtimeval_t timeq[QUEUE_SIZE];
the729	0:988132ee7271	14	void (*funcq[QUEUE_SIZE])(void);
the729	0:988132ee7271	15	uint8_t qfront;
the729	0:988132ee7271	16	uint8_t qtail;
the729	0:988132ee7271	17
the729	0:988132ee7271	18	void (trigf)(struct timeval );
the729	0:988132ee7271	19	hdtimeval_t trigt;
the729	0:988132ee7271	20	//bool trigactive;
the729	0:988132ee7271	21
the729	0:988132ee7271	22	//syncdata_t syncdata[2];
the729	0:988132ee7271	23	//state_t state;
the729	0:988132ee7271	24	synctype_t syncbusy;
the729	0:988132ee7271	25	synctype_t tmp_synctype;
the729	0:988132ee7271	26	hdtimeval_t txstamp;
the729	0:988132ee7271	27
the729	0:988132ee7271	28	coeff_t coeff;
the729	0:988132ee7271	29
the729	0:988132ee7271	30	void clock_init();
the729	0:988132ee7271	31	void tmr1_irq();
the729	0:988132ee7271	32	void uart_recv();
the729	0:988132ee7271	33	__INLINE void getHDTime(struct hdtimeval * hdtv);
the729	0:988132ee7271	34	void sync_req();
the729	0:988132ee7271	35	void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp);
the729	0:988132ee7271	36	void getLocalTime(hdtimeval * gt, hdtimeval * lt);
the729	0:988132ee7271	37	void getGlobalTime(hdtimeval * lt, hdtimeval * gt);
the729	0:988132ee7271	38	void runTriggerFunc();
the729	0:988132ee7271	39
the729	0:988132ee7271	40	void timesync_init()
the729	0:988132ee7271	41	{
the729	0:988132ee7271	42	wait_ms(100);
the729	0:988132ee7271	43	coeff.v1div2.tv_sec = 0;
the729	0:988132ee7271	44	coeff.v1div2.ticks = 0;
the729	0:988132ee7271	45	coeff.w1div2.tv_sec = 0;
the729	0:988132ee7271	46	coeff.w1div2.ticks = 0;
the729	0:988132ee7271	47	coeff.beta1.tv_sec = 0;
the729	0:988132ee7271	48	coeff.beta1.ticks = 1;
the729	0:988132ee7271	49	coeff.beta2.tv_sec = 0;
the729	0:988132ee7271	50	coeff.beta2.ticks = 1;
the729	0:988132ee7271	51	coeff.size = 0;
the729	0:988132ee7271	52	coeff.front = 0;
the729	0:988132ee7271	53	trigf = NULL;
the729	0:988132ee7271	54
the729	0:988132ee7271	55	clock_init();
the729	0:988132ee7271	56	uart_sync.baud(115200);
the729	0:988132ee7271	57	uart_sync.attach(&uart_recv, Serial::RxIrq);
the729	0:988132ee7271	58
the729	0:988132ee7271	59	syncbusy = SYNCTYPE_NONE;
the729	0:988132ee7271	60	tmp_synctype = SYNCTYPE_64;
the729	0:988132ee7271	61	tmr_sync.attach(&sync_req, 0.1);
the729	0:988132ee7271	62	}
the729	0:988132ee7271	63	/*
the729	0:988132ee7271	64	void timesync_poll()
the729	0:988132ee7271	65	{
the729	0:988132ee7271	66	//static last_poll;
the729	0:988132ee7271	67	tmp_synctype = SYNCTYPE_24;
the729	0:988132ee7271	68	sync_req();
the729	0:988132ee7271	69	}
the729	0:988132ee7271	70	*/
the729	0:988132ee7271	71	void clock_init()
the729	0:988132ee7271	72	{
the729	0:988132ee7271	73	// select PCLK
the729	0:988132ee7271	74	LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) \| (PCLK_DEVIDER<<CLK_TIMER);
the729	0:988132ee7271	75	// power on timer
the729	0:988132ee7271	76	LPC_SC->PCONP \|= (1<<PCTIM2);
the729	0:988132ee7271	77
the729	0:988132ee7271	78	// reset timer
the729	0:988132ee7271	79	TMR->TCR = 2;
the729	0:988132ee7271	80	TMR->CTCR = 0;
the729	0:988132ee7271	81	TMR->PR = 0;
the729	0:988132ee7271	82	TMR->TC = 1;
the729	0:988132ee7271	83	TMR->MR0 = SystemCoreClock - 1;
the729	0:988132ee7271	84	TMR->MCR = 3;
the729	0:988132ee7271	85
the729	0:988132ee7271	86	clock_sec = 0;
the729	0:988132ee7271	87
the729	0:988132ee7271	88	// capture on CAP.0
the729	0:988132ee7271	89	TMR->CCR = 0x07;
the729	0:988132ee7271	90	LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0 \| (3<<8);
the729	0:988132ee7271	91
the729	0:988132ee7271	92	// setup interrupt
the729	0:988132ee7271	93	__enable_irq();
the729	0:988132ee7271	94	NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
the729	0:988132ee7271	95	NVIC_EnableIRQ(TIMER2_IRQn);
the729	0:988132ee7271	96
the729	0:988132ee7271	97	NVIC_SetPriority(TIMER2_IRQn, 0);
the729	0:988132ee7271	98	NVIC_SetPriority(UART2_IRQn, 1);
the729	0:988132ee7271	99	NVIC_SetPriority(TIMER3_IRQn, 2);
the729	0:988132ee7271	100	NVIC_SetPriority(UART0_IRQn, 2);
the729	0:988132ee7271	101	NVIC_SetPriority(UART3_IRQn, 1);
the729	0:988132ee7271	102
the729	0:988132ee7271	103	// begin freerun
the729	0:988132ee7271	104	TMR->TCR = 1;
the729	0:988132ee7271	105	}
the729	0:988132ee7271	106
the729	0:988132ee7271	107	void tmr1_irq()
the729	0:988132ee7271	108	{
the729	0:988132ee7271	109	if (TMR->IR & (1<<4)) {
the729	0:988132ee7271	110	if (trigf) {
the729	0:988132ee7271	111	trigt.tv_sec = clock_sec;
the729	0:988132ee7271	112	if ((TMR->IR & 0x01) && TMR->CR0 < 256)
the729	0:988132ee7271	113	trigt.tv_sec = clock_sec+1;
the729	0:988132ee7271	114	trigt.ticks = TMR->CR0;
the729	0:988132ee7271	115	//runTriggerFunc();
the729	0:988132ee7271	116	tmr_callback.attach_us(&runTriggerFunc, 10);
the729	0:988132ee7271	117	}
the729	0:988132ee7271	118	TMR->IR = (1<<4);
the729	0:988132ee7271	119	}
the729	0:988132ee7271	120	if (TMR->IR & 0x1) {
the729	0:988132ee7271	121	// Overflow!
the729	0:988132ee7271	122	clock_sec ++;
the729	0:988132ee7271	123	TMR->IR = 0x1;
the729	0:988132ee7271	124	}
the729	0:988132ee7271	125	if (TMR->IR & 0x2) {
the729	0:988132ee7271	126	if (clock_sec == front_lt.tv_sec) {
the729	0:988132ee7271	127	hdtimeval_t * nowtv = timeq+qfront;
the729	0:988132ee7271	128	funcq[qfront]();
the729	0:988132ee7271	129	qfront = (qfront+1)%QUEUE_SIZE;
the729	0:988132ee7271	130	while(qfront != qtail
the729	0:988132ee7271	131	&& nowtv->tv_sec == timeq[qfront].tv_sec
the729	0:988132ee7271	132	&& nowtv->ticks == timeq[qfront].ticks) {
the729	0:988132ee7271	133	funcq[qfront]();
the729	0:988132ee7271	134	qfront = (qfront+1)%QUEUE_SIZE;
the729	0:988132ee7271	135	}
the729	0:988132ee7271	136	if (qfront == qtail) {
the729	0:988132ee7271	137	TMR->MCR &= ~(1<<3);
the729	0:988132ee7271	138	} else {
the729	0:988132ee7271	139	getLocalTime(&timeq[qfront], &front_lt);
the729	0:988132ee7271	140	TMR->MR1 = front_lt.ticks;
the729	0:988132ee7271	141	}
the729	0:988132ee7271	142	} else if (clock_sec > front_lt.tv_sec) {
the729	0:988132ee7271	143	// we missed it!
the729	0:988132ee7271	144	qfront = (qfront+1)%QUEUE_SIZE;
the729	0:988132ee7271	145	if (qfront == qtail) {
the729	0:988132ee7271	146	TMR->MCR &= ~(1<<3);
the729	0:988132ee7271	147	} else {
the729	0:988132ee7271	148	getLocalTime(&timeq[qfront], &front_lt);
the729	0:988132ee7271	149	TMR->MR1 = front_lt.ticks;
the729	0:988132ee7271	150	}
the729	0:988132ee7271	151	}
the729	0:988132ee7271	152	TMR->IR = 0x2;
the729	0:988132ee7271	153	}
the729	0:988132ee7271	154	}
the729	0:988132ee7271	155
the729	0:988132ee7271	156	void runTriggerFunc()
the729	0:988132ee7271	157	{
the729	0:988132ee7271	158	timeval_t t;
the729	0:988132ee7271	159	getGlobalTime(&trigt, &trigt);
the729	0:988132ee7271	160	hdtv_totv(&t, &trigt);
the729	0:988132ee7271	161	trigf(&t);
the729	0:988132ee7271	162	}
the729	0:988132ee7271	163
the729	0:988132ee7271	164	void uart_recv()
the729	0:988132ee7271	165	{
the729	0:988132ee7271	166	hdtimeval_t tstamp;
the729	0:988132ee7271	167	getHDTime(&tstamp);
the729	0:988132ee7271	168
the729	0:988132ee7271	169	//if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
the729	0:988132ee7271	170	if (uart_sync.readable()) {
the729	0:988132ee7271	171	sync_pkt64_t pkt64;
the729	0:988132ee7271	172	sync_pkt32_t pkt32;
the729	0:988132ee7271	173	sync_pkt24_t pkt24;
the729	0:988132ee7271	174	char * data = NULL;
the729	0:988132ee7271	175	void * pkt_p = NULL;
the729	0:988132ee7271	176	uint8_t len = 0;
the729	0:988132ee7271	177
the729	0:988132ee7271	178	if (!syncbusy) {
the729	0:988132ee7271	179	len = uart_sync.getc();
the729	0:988132ee7271	180	return;
the729	0:988132ee7271	181	}
the729	0:988132ee7271	182
the729	0:988132ee7271	183	switch(syncbusy) {
the729	0:988132ee7271	184	case SYNCTYPE_64:
the729	0:988132ee7271	185	data = pkt64.raw;
the729	0:988132ee7271	186	pkt_p = (void *)&pkt64;
the729	0:988132ee7271	187	len = 10;
the729	0:988132ee7271	188	break;
the729	0:988132ee7271	189	case SYNCTYPE_32:
the729	0:988132ee7271	190	data = pkt32.raw;
the729	0:988132ee7271	191	pkt_p = (void *)&pkt32;
the729	0:988132ee7271	192	len = 6;
the729	0:988132ee7271	193	break;
the729	0:988132ee7271	194	case SYNCTYPE_24:
the729	0:988132ee7271	195	pkt24.no_use = 0;
the729	0:988132ee7271	196	pkt_p = (void *)&pkt24;
the729	0:988132ee7271	197	data = pkt24.raw;
the729	0:988132ee7271	198	len = 5;
the729	0:988132ee7271	199	break;
the729	0:988132ee7271	200	}
the729	0:988132ee7271	201	while(len-->0) {
the729	0:988132ee7271	202	//while(!(LPC_UART2->LSR & 0x01));
the729	0:988132ee7271	203	*(data++)= uart_sync.getc(); //LPC_UART2->RBR;
the729	0:988132ee7271	204	}
the729	0:988132ee7271	205
the729	0:988132ee7271	206	update_data(syncbusy, pkt_p, &tstamp);
the729	0:988132ee7271	207	}
the729	0:988132ee7271	208	syncbusy = SYNCTYPE_NONE;
the729	0:988132ee7271	209	}
the729	0:988132ee7271	210
the729	0:988132ee7271	211	void sync_req()
the729	0:988132ee7271	212	{
the729	0:988132ee7271	213	//if (syncbusy) return;
the729	0:988132ee7271	214	__disable_irq();
the729	0:988132ee7271	215	txstamp.tv_sec = clock_sec;
the729	0:988132ee7271	216	txstamp.ticks = TMR->TC;
the729	0:988132ee7271	217	uart_sync.putc(tmp_synctype);
the729	0:988132ee7271	218	//LPC_UART2->THR = tmp_synctype;
the729	0:988132ee7271	219	__enable_irq();
the729	0:988132ee7271	220	syncbusy = tmp_synctype;
the729	0:988132ee7271	221	}
the729	0:988132ee7271	222
the729	0:988132ee7271	223	void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp)
the729	0:988132ee7271	224	{
the729	0:988132ee7271	225	hdtimeval_t v, w;
the729	0:988132ee7271	226	hdtimeval_t master_rxt, master_txt;
the729	0:988132ee7271	227	int32_t ticks_diff;
the729	0:988132ee7271	228	hdtv_add(&v, &txstamp, rxstamp);
the729	0:988132ee7271	229	switch (t) {
the729	0:988132ee7271	230	case SYNCTYPE_64: {
the729	0:988132ee7271	231	sync_pkt64_t * p = (sync_pkt64_t *)pkt_p;
the729	0:988132ee7271	232	master_rxt = p->rx_time;
the729	0:988132ee7271	233	master_txt.tv_sec = master_rxt.tv_sec;
the729	0:988132ee7271	234	master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
the729	0:988132ee7271	235	} break;
the729	0:988132ee7271	236	case SYNCTYPE_32: {
the729	0:988132ee7271	237	sync_pkt32_t * p = (sync_pkt32_t *)pkt_p;
the729	0:988132ee7271	238	hdtimeval_t tmp;
the729	0:988132ee7271	239	getGlobalTime(&txstamp, &tmp);
the729	0:988132ee7271	240	if (tmp.ticks > MAX_TICK/2) {
the729	0:988132ee7271	241	if (p->rx_time > tmp.ticks-MAX_TICK/2) {
the729	0:988132ee7271	242	master_rxt.tv_sec = tmp.tv_sec;
the729	0:988132ee7271	243	} else {
the729	0:988132ee7271	244	master_rxt.tv_sec = tmp.tv_sec +1;
the729	0:988132ee7271	245	}
the729	0:988132ee7271	246	} else {
the729	0:988132ee7271	247	if (p->rx_time > tmp.ticks+MAX_TICK/2) {
the729	0:988132ee7271	248	master_rxt.tv_sec = tmp.tv_sec-1;
the729	0:988132ee7271	249	} else {
the729	0:988132ee7271	250	master_rxt.tv_sec = tmp.tv_sec;
the729	0:988132ee7271	251	}
the729	0:988132ee7271	252	}
the729	0:988132ee7271	253	master_rxt.ticks = p->rx_time;
the729	0:988132ee7271	254	master_txt.tv_sec = master_rxt.tv_sec;
the729	0:988132ee7271	255	master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
the729	0:988132ee7271	256	} break;
the729	0:988132ee7271	257	case SYNCTYPE_24: {
the729	0:988132ee7271	258	sync_pkt24_t * p = (sync_pkt24_t *)pkt_p;
the729	0:988132ee7271	259	hdtimeval_t tmp;
the729	0:988132ee7271	260	getGlobalTime(&txstamp, &tmp);
the729	0:988132ee7271	261	p->rx_time >>= 8;
the729	0:988132ee7271	262	if (tmp.ticks > (1<<23)) {
the729	0:988132ee7271	263	tmp.ticks = tmp.ticks-(1<<23);
the729	0:988132ee7271	264	master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
the729	0:988132ee7271	265	master_rxt.tv_sec = tmp.tv_sec;
the729	0:988132ee7271	266	} else {
the729	0:988132ee7271	267	tmp.ticks = tmp.ticks+MAX_TICK-(1<<23);
the729	0:988132ee7271	268	master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
the729	0:988132ee7271	269	master_rxt.tv_sec = tmp.tv_sec-1;
the729	0:988132ee7271	270	}
the729	0:988132ee7271	271	if (master_rxt.ticks < tmp.ticks) {
the729	0:988132ee7271	272	master_rxt.ticks += 0x1000000;
the729	0:988132ee7271	273	if (master_rxt.ticks > MAX_TICK) {
the729	0:988132ee7271	274	master_rxt.ticks &= 0xFFFFFF;
the729	0:988132ee7271	275	master_rxt.tv_sec ++;
the729	0:988132ee7271	276	}
the729	0:988132ee7271	277	}
the729	0:988132ee7271	278	master_txt.tv_sec = master_rxt.tv_sec;
the729	0:988132ee7271	279	master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
the729	0:988132ee7271	280	} break;
the729	0:988132ee7271	281	}
the729	0:988132ee7271	282	//printf("t1,t2: %d,%d, %d,%d \r\n",master_rxt.tv_sec, master_rxt.ticks>>24, master_rxt.ticks & 0xFFFFFF, master_txt.ticks);
the729	0:988132ee7271	283	if (master_rxt.ticks > master_txt.ticks) {
the729	0:988132ee7271	284	master_txt.ticks += 0x10000;
the729	0:988132ee7271	285	if (master_txt.ticks >= MAX_TICK) {
the729	0:988132ee7271	286	master_txt.ticks &= 0xFFFF;
the729	0:988132ee7271	287	master_txt.tv_sec ++;
the729	0:988132ee7271	288	}
the729	0:988132ee7271	289	}
the729	0:988132ee7271	290	hdtv_add(&w, &master_rxt, &master_txt);
the729	0:988132ee7271	291
the729	0:988132ee7271	292	hdtv_div2(&v, &v);
the729	0:988132ee7271	293	hdtv_div2(&w, &w);
the729	0:988132ee7271	294	// evaluate performance
the729	0:988132ee7271	295	{
the729	0:988132ee7271	296	hdtimeval_t tmp;
the729	0:988132ee7271	297	getGlobalTime(&v, &tmp);
the729	0:988132ee7271	298	hdtv_sub(&tmp, &tmp, &w);
the729	0:988132ee7271	299	if (tmp.tv_sec > 10)
the729	0:988132ee7271	300	ticks_diff = 0x7FFFFFFF;
the729	0:988132ee7271	301	else {
the729	0:988132ee7271	302	ticks_diff = tmp.tv_sec * MAX_TICK + tmp.ticks;
the729	0:988132ee7271	303	}
the729	0:988132ee7271	304	//printf("perf: %d \r\n", ticks_diff);
the729	0:988132ee7271	305	}
the729	0:988132ee7271	306	if (coeff.size)
the729	0:988132ee7271	307	if (coeff.size<HISTORY_LEN) {
the729	0:988132ee7271	308	hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[0]));
the729	0:988132ee7271	309	hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[0]));
the729	0:988132ee7271	310	} else {
the729	0:988132ee7271	311	hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[coeff.front]));
the729	0:988132ee7271	312	hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[coeff.front]));
the729	0:988132ee7271	313	}
the729	0:988132ee7271	314	/*if(coeff.size>=8) {
the729	0:988132ee7271	315	hdtimeval_t tmp;
the729	0:988132ee7271	316	hdtv_div8(&tmp, &(coeff.hisw[coeff.front]));
the729	0:988132ee7271	317	hdtv_sub(&coeff.w1div2, &coeff.w1div2, &tmp);
the729	0:988132ee7271	318	hdtv_div8(&tmp, &(coeff.hisv[coeff.front]));
the729	0:988132ee7271	319	hdtv_sub(&coeff.v1div2, &coeff.v1div2, &tmp);
the729	0:988132ee7271	320	hdtv_div8(&tmp, &w);
the729	0:988132ee7271	321	hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
the729	0:988132ee7271	322	hdtv_div8(&tmp, &v);
the729	0:988132ee7271	323	hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
the729	0:988132ee7271	324	} else {*/
the729	0:988132ee7271	325	coeff.v1div2 = v;
the729	0:988132ee7271	326	coeff.w1div2 = w;
the729	0:988132ee7271	327	//}
the729	0:988132ee7271	328	coeff.hisw[coeff.front] = w;
the729	0:988132ee7271	329	coeff.hisv[coeff.front] = v;
the729	0:988132ee7271	330	coeff.front = (coeff.front+1) % HISTORY_LEN;
the729	0:988132ee7271	331	if (coeff.size<=HISTORY_LEN) coeff.size++;
the729	0:988132ee7271	332	/*if (coeff.size==8) {
the729	0:988132ee7271	333	hdtimeval_t tmp;
the729	0:988132ee7271	334	coeff.w1div2.tv_sec = 0;
the729	0:988132ee7271	335	coeff.w1div2.ticks = 0;
the729	0:988132ee7271	336	coeff.v1div2.tv_sec = 0;
the729	0:988132ee7271	337	coeff.v1div2.ticks = 0;
the729	0:988132ee7271	338	for (uint8_t i=0; i<8; i++) {
the729	0:988132ee7271	339	hdtv_div8(&tmp, &(coeff.hisw[i]));
the729	0:988132ee7271	340	hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
the729	0:988132ee7271	341	hdtv_div8(&tmp, &(coeff.hisv[i]));
the729	0:988132ee7271	342	hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
the729	0:988132ee7271	343	}
the729	0:988132ee7271	344	printf("updated!\r\n");
the729	0:988132ee7271	345	}*/
the729	0:988132ee7271	346	//printf("w/v: %d, %d \r\n", coeff.w1div2.ticks, coeff.v1div2.ticks);
the729	0:988132ee7271	347	if (qfront != qtail) {
the729	0:988132ee7271	348	getLocalTime(&timeq[qfront], &front_lt);
the729	0:988132ee7271	349	TMR->MR1 = front_lt.ticks;
the729	0:988132ee7271	350	TMR->MCR \|= (1<<3);
the729	0:988132ee7271	351	}
the729	0:988132ee7271	352	// schedule next sync
the729	0:988132ee7271	353	ticks_diff = abs(ticks_diff);
the729	0:988132ee7271	354	if (ticks_diff < 200) {
the729	0:988132ee7271	355	tmp_synctype = SYNCTYPE_24;
the729	0:988132ee7271	356	tmr_sync.attach(&sync_req, 60);
the729	0:988132ee7271	357	} else if (ticks_diff < 500) {
the729	0:988132ee7271	358	tmp_synctype = SYNCTYPE_24;
the729	0:988132ee7271	359	tmr_sync.attach(&sync_req, 30);
the729	0:988132ee7271	360	} else if (ticks_diff < 800) {
the729	0:988132ee7271	361	tmp_synctype = SYNCTYPE_24;
the729	0:988132ee7271	362	tmr_sync.attach(&sync_req, 20);
the729	0:988132ee7271	363	} else if (ticks_diff < 5000) {
the729	0:988132ee7271	364	tmp_synctype = SYNCTYPE_32;
the729	0:988132ee7271	365	tmr_sync.attach(&sync_req, 10);
the729	0:988132ee7271	366	} else {
the729	0:988132ee7271	367	tmp_synctype = SYNCTYPE_64;
the729	0:988132ee7271	368	tmr_sync.attach(&sync_req, 5);
the729	0:988132ee7271	369	}
the729	0:988132ee7271	370	led = !led;
the729	0:988132ee7271	371	}
the729	0:988132ee7271	372
the729	0:988132ee7271	373	__INLINE void getHDTime(struct hdtimeval * hdtv)
the729	0:988132ee7271	374	{
the729	0:988132ee7271	375	__disable_irq();
the729	0:988132ee7271	376	hdtv->tv_sec = clock_sec;
the729	0:988132ee7271	377	hdtv->ticks = TMR->TC;
the729	0:988132ee7271	378	__enable_irq();
the729	0:988132ee7271	379	return;
the729	0:988132ee7271	380	}
the729	0:988132ee7271	381
the729	0:988132ee7271	382	void getTime(struct timeval * tv)
the729	0:988132ee7271	383	{
the729	0:988132ee7271	384	hdtimeval_t tstamp, gstamp, tstamp1;
the729	0:988132ee7271	385	static hdtimeval_t comp_delay = {0,0};
the729	0:988132ee7271	386
the729	0:988132ee7271	387	getHDTime(&tstamp);
the729	0:988132ee7271	388
the729	0:988132ee7271	389	hdtv_add(&gstamp, &tstamp, &comp_delay);
the729	0:988132ee7271	390	getGlobalTime(&gstamp, &gstamp);
the729	0:988132ee7271	391
the729	0:988132ee7271	392	getHDTime(&tstamp1);
the729	0:988132ee7271	393	hdtv_sub(&comp_delay, &tstamp1, &tstamp);
the729	0:988132ee7271	394	//printf("delay: %d\r\n", comp_delay.ticks);
the729	0:988132ee7271	395
the729	0:988132ee7271	396	tv->tv_sec = gstamp.tv_sec;
the729	0:988132ee7271	397	tv->tv_usec = ((gstamp.ticks+48) >> 5) / 3;
the729	0:988132ee7271	398	return;
the729	0:988132ee7271	399	}
the729	0:988132ee7271	400
the729	0:988132ee7271	401	void getGlobalTime(hdtimeval * lt, hdtimeval * gt)
the729	0:988132ee7271	402	{
the729	0:988132ee7271	403	hdtimeval_t tmp;
the729	0:988132ee7271	404	hdtv_sub(&tmp, lt, &coeff.v1div2);
the729	0:988132ee7271	405	hdtv_muldiv(&tmp, &tmp, &coeff.beta1, &coeff.beta2);
the729	0:988132ee7271	406	hdtv_add(gt, &tmp, &coeff.w1div2);
the729	0:988132ee7271	407	}
the729	0:988132ee7271	408
the729	0:988132ee7271	409	void getLocalTime(hdtimeval * gt, hdtimeval * lt)
the729	0:988132ee7271	410	{
the729	0:988132ee7271	411	hdtimeval_t tmp;
the729	0:988132ee7271	412	hdtv_sub(&tmp, gt, &coeff.w1div2);
the729	0:988132ee7271	413	hdtv_muldiv(&tmp, &tmp, &coeff.beta2, &coeff.beta1);
the729	0:988132ee7271	414	hdtv_add(lt, &tmp, &coeff.v1div2);
the729	0:988132ee7271	415	}
the729	0:988132ee7271	416
the729	0:988132ee7271	417	int enqueue(void (schedFunc)(void), hdtimeval_t tv)
the729	0:988132ee7271	418	{
the729	0:988132ee7271	419	uint8_t p = qtail;
the729	0:988132ee7271	420	if ((qtail+1)%QUEUE_SIZE == qfront) {
the729	0:988132ee7271	421	return 1;
the729	0:988132ee7271	422	}
the729	0:988132ee7271	423	while(p != qfront) {
the729	0:988132ee7271	424	uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
the729	0:988132ee7271	425	if (tv->tv_sec > timeq[prev].tv_sec
the729	0:988132ee7271	426	\|\| (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
the729	0:988132ee7271	427	timeq[p] = timeq[prev];
the729	0:988132ee7271	428	funcq[p] = funcq[prev];
the729	0:988132ee7271	429	p = prev;
the729	0:988132ee7271	430	}
the729	0:988132ee7271	431	timeq[p] = *tv;
the729	0:988132ee7271	432	funcq[p] = schedFunc;
the729	0:988132ee7271	433	qtail = (qtail+1)%QUEUE_SIZE;
the729	0:988132ee7271	434	return 0;
the729	0:988132ee7271	435	}
the729	0:988132ee7271	436
the729	0:988132ee7271	437	int runAtTime(void (schedFunc)(void), struct timeval tv)
the729	0:988132ee7271	438	{
the729	0:988132ee7271	439	hdtimeval_t hdtv;
the729	0:988132ee7271	440	int ret;
the729	0:988132ee7271	441	hdtv.tv_sec = tv->tv_sec;
the729	0:988132ee7271	442	hdtv.ticks = tv->tv_usec * 96;
the729	0:988132ee7271	443	//getLocalTime(&hdtv, &hdtv);
the729	0:988132ee7271	444	ret = enqueue(schedFunc, &hdtv);
the729	0:988132ee7271	445	if (qfront != qtail) {
the729	0:988132ee7271	446	getLocalTime(&timeq[qfront], &front_lt);
the729	0:988132ee7271	447	TMR->MR1 = front_lt.ticks;
the729	0:988132ee7271	448	TMR->MCR \|= (1<<3);
the729	0:988132ee7271	449	}
the729	0:988132ee7271	450	return ret;
the729	0:988132ee7271	451	}
the729	0:988132ee7271	452
the729	0:988132ee7271	453
the729	0:988132ee7271	454	void runAtTrigger(void (trigFunc)(struct timeval tv))
the729	0:988132ee7271	455	{
the729	0:988132ee7271	456	trigf = trigFunc;
the729	0:988132ee7271	457	}

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_slave.cpp@0:988132ee7271, 2010-12-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning