radar.c.svn-base

来自「AVR单片机下」· SVN-BASE 代码 · 共 76 行

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : radar.c
 * Version    : 2.0
 * Brief      :
 * Code by    : leaf
 * Date       : May 6, 2009
 * Note       :
 *
 *
 * ------------------------------------------------------------------------
 */

#include "lib_bupt.h"

#ifdef RADAR_H_INCLUDED
UINT8
calcSum( const UINT8 *data, size_t cnt )
{
	UINT8 ret = 0;
	while( cnt )
	{
		ret += data[ --cnt ];
	}
	return ret;
}

void
radarWrite( RadarPort p, UINT8 b1, UINT8 b2, UINT8 b3 )
{
	usartPutc( p, b1 );
	usartPutc( p, b2 );
	usartPutc( p, b3 );
	usartPutc( p, (b1 + b2 + b3) );
}

Bool
radarRead( RadarPort p, void *buf )
{
	usartRead( p, buf, 4 );
	UINT8 *recv = buf;
	return PARSE_BOOL( calcSum(recv, 3) == recv[3] );
}

int
radarDist( RadarMsg msg )
{
	UINT16 ret;
	ret = msg[1];
	ret <<= 8;
	ret += msg[2];
	if( ret == 0xFF ){
		return -1;
	}
	else{
		return ret;
	}
}

int
radarTemp( RadarMsg msg )
{
	return radarDist( msg );
}

#ifdef _STDIO_H_
void
printRadarMsg( RadarMsg msg )
{
	UINT8 *buf = msg;
	printf( "R: %02X %02X %02X %02X\n", buf[0], buf[1], buf[2], buf[3] );
}
#endif

#endif