syncSlave_3 - syncSlave for problem 3

Users » the729 » Code » syncSlave_3

Tianji Wu / Mbed 2 deprecated syncSlave_3

syncSlave for problem 3

timesync_slave.cpp@0:362932d519c6, 2010-12-03 (annotated)

Committer:: the729
Date:: Fri Dec 03 20:53:52 2010 +0000
Revision:: 0:362932d519c6

Who changed what in which revision?

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

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:362932d519c6, 2010-12-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning