toam.nc

一种改进的节点间测距算法

includes ToAMsg;

module ToAM
{
    provides interface StdControl;
	
	uses 
	{   interface Timer as TimerToA;
		interface Timer as TimerSounder;
		interface Timer as TimerSample;	
		interface MicroTimer as MicroTimerSample;
		interface StdControl as MicCtrl;
		interface StdControl as SounderCtrl;		
		//interface StdControl as CommCtrl;
		interface SysTime;		
		interface SendMsg as SendBeacon;
		interface SendMsg as SendResult;
		interface ReceiveMsg as ReceiveBeacon;
		interface Mic;
		interface MicInterrupt;
		interface Leds;		
	}
}

implementation
{
    uint8_t in,count,i,base_addr;
	TOS_Msg data;	
	bool first_tone_detect;
	uint32_t TimeSound, TimeSoundReceive,difference;	
	uint32_t distance, Vsound , TimeSoundOver;	
	command result_t StdControl.init()
	{
	    call Leds.init();	
		call SounderCtrl.init();	
		call MicCtrl.init();
		call MicCtrl.init();
		//call Mic.muxSel(0);
    	call Mic.gainAdjust(255);  
		return SUCCESS;
	}
	
	command result_t StdControl.start()
	{
	   	in = 1;		
		base_addr = 0;
		Vsound = 340;		
		call Leds.redToggle();
		call MicCtrl.start(); 
		call MicCtrl.start();
		call MicroTimerSample.start(1000000);		//开机延时
		return SUCCESS;
	}
	
	command result_t StdControl.stop()
	{
	    call TimerToA.stop();
		call MicCtrl.stop();
		call SounderCtrl.stop();		
		return SUCCESS;
	}

	async event result_t MicroTimerSample.fired()
	{
		call MicroTimerSample.stop();
		if(TOS_LOCAL_ADDRESS == 1)
		{	
			TDoAMsg *sndmsg = (TDoAMsg *) data.data;	
			atomic
			{   
				sndmsg->address = TOS_LOCAL_ADDRESS;
			}
			if(call SendBeacon.send(TOS_BCAST_ADDR, sizeof(TDoAMsg), &data))  
			{    
				return SUCCESS;
			}
			
		}		
		return SUCCESS;
	}

	task void sendresult();

	event result_t SendBeacon.sendDone(TOS_MsgPtr m, result_t ok)
	{
		TimeSound = call SysTime.getTime32();
	    call TimerToA.start(TIMER_REPEAT, 3072);
		
		return SUCCESS;	
	}
	
	event result_t TimerToA.fired()
	{    
		//call Leds.greenToggle();		
		call SounderCtrl.start();		
		call TimerSounder.start(TIMER_ONE_SHOT, 1024);
		first_tone_detect = TRUE;
		while(1)
		{
		in = call Mic.readToneDetector();
	  	if(in == 0)
				{   		
					if(first_tone_detect)
					{
					TimeSoundReceive = call SysTime.getTime32();					
					first_tone_detect = FALSE;
					}
					
					count++;
				}
				else
				{
					count = 0;
					first_tone_detect = TRUE;
				}
				if(count > 10)
				{			
					call Leds.greenToggle();
					post sendresult();					
					break;
				}
		}	
		/*while(1)
		{
			in = call Mic.readToneDetector();
	  		if(in == 0)
			{   		
				TimeSound = call SysTime.getTime32();				
				break;
			}		
		}*/			
		 return FAIL;	
	}

	task void sendresult();

	event result_t TimerSounder.fired()
	{	 
		call SounderCtrl.stop();
		call MicInterrupt.enable();
		return SUCCESS;
	}

	event TOS_MsgPtr ReceiveBeacon.receive(TOS_MsgPtr m)
	{ 
		call MicInterrupt.enable();
		return m;
	}

			
	task void sendresult()							 //发送结果
	{
	    ResultMsg * sndmsg = (ResultMsg *) data.data;
		/*difference = TimeSound - TimeRadio;
		if(TOS_LOCAL_ADDRESS == 2){
			
			distance = difference /921.6 * Vsound;
		}
		else
			if(TimeSound>TimeRadio)
				distance = difference /921.6 * Vsound;*/
			
		call Leds.yellowToggle();
		atomic
		{
		    sndmsg->TimeSound = TimeSound;
			sndmsg->TimeSoundReceive = TimeSoundReceive;
			sndmsg->distance = distance;
			sndmsg->address = TOS_LOCAL_ADDRESS;
		}
		if(call SendResult.send(base_addr, sizeof(ResultMsg), &data))
		{
		    //call Leds.yellowToggle();
		}
	}

	async event result_t MicInterrupt.toneDetected()
	{	
		if(TOS_LOCAL_ADDRESS == 2)
		{
			TimeSoundReceive = call SysTime.getTime32();
			call Leds.greenToggle();
			while(1)
					{
						in = call Mic.readToneDetector();
						if(in == 1)
						{
							TimeSoundOver = call SysTime.getTime32();
							call Leds.redToggle();	
							call TimerSample.start(TIMER_ONE_SHOT,3072);
							break;
											
						}
					}
				
		}
		else
		{
			TimeSoundReceive = call SysTime.getTime32();
			while(1)
					{
						in = call Mic.readToneDetector();
						if(in == 1)
						{
							TimeSoundOver = call SysTime.getTime32();
							call Leds.redToggle();	
							call TimerSample.start(TIMER_ONE_SHOT,3072);
							break;
											
						}
					}
			post sendresult();
		}
		return SUCCESS;
	}
	
	event result_t TimerSample.fired()
	{		
		call SounderCtrl.start();		
		call TimerSounder.start(TIMER_ONE_SHOT, 1024);
		while(1)
		{
		in = call Mic.readToneDetector();
	  	if(in == 0)
		{   		
			TimeSound = call SysTime.getTime32();
			post sendresult();		
			break;
		}		
		}	
		return SUCCESS;
	}	
	
	event result_t SendResult.sendDone(TOS_MsgPtr m, result_t ok)
	{
		call MicInterrupt.disable();
	    return SUCCESS;	
	}
	
	}