cmt.c.svn-base

AVR单片机下

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : cmt.c
 * Version    : 1.0
 * Brief      : MTi通信协议C语言实现
 * Code by    : leaf
 * Date       : Apr 3, 2009
 * Note       :
 *
 *
 * ------------------------------------------------------------------------
 */
#include "lib_bupt.h"

#ifdef CMT_H_INCLUDED
static int(*cmtRead)(CmtPort, void *, size_t) = usartRead;
static int(*cmtWrite)(CmtPort, const void *, size_t) = usartWrite;
/*初始化Cmt对象*/
void cmtInit(Cmt *cmt, CmtPort port, CmtId id){
	cmt->port = port;
	cmt->id   = id;
}

/*计算校验和（消息中除了0xFA之外的部分通能加起来，如果低字节为0则是正确的消息）*/
UINT8 computeChecksum(const CmtMsg *msg){
	UINT8 cs = 0;
	if(msg->size < 255){
		cs += (UINT8)msg->size;
	}
	else{
		cs += 0xff;
		cs += (UINT8)(msg->size>>8);
		cs += (UINT8)msg->size;
	}
	cs += msg->mid;
	int i;
	for(i = 0; i < msg->size; ++i){
		cs += (UINT8)vectorGet(&msg->data, i);
	}
	return cs;
}

/*计算校验位*/
UINT8 computeCheckbyte(const CmtMsg *msg)
{
	UINT8 cs = 0;
	if(msg->size < 255){
		cs -= (UINT8)msg->size;
	}
	else{
		cs -= 0xff;
		cs -= (UINT8)(msg->size>>8);
		cs -= (UINT8)msg->size;
	}
	cs -= msg->mid;
	size_t i;
	for(i = 0; i < msg->size; ++i){
		cs -= (UINT8)vectorGet(&msg->data, i);
	}
	return cs;
}

/*接收一条消息，返回消息的ID，如果返回-1表示校验失败，收到错误消息*/
int cmtRecvMsg(Cmt *cmt, CmtMsg *msg){
	char preamble;
	CmtId id;
	UINT8 checksum;
	cmtMsgClearData(msg);
	do{
		do{
			cmtRead(cmt->port, &preamble, 1);
		}while(preamble != 0xfa);   //等待消息头
		cmtRead(cmt->port, &id, 1);
	}while( id != cmt->id ); //等待正确的node id
	cmtRead(cmt->port, &msg->mid, 1);
	msg->size = 0;
	void *sz  = &msg->size;
	cmtRead(cmt->port, sz, 1);
	if(msg->size == 255){
		cmtRead(cmt->port, sz + 1, 1);
		cmtRead(cmt->port, sz,     1);
	}
	vectorExtends(&msg->data, msg->size);
	int rc = cmtRead(cmt->port, vectorGetBuffer(&msg->data, 0), msg->size);
	if( rc != msg->size ){
		return -1;
	}
	cmtRead(cmt->port, &checksum, 1);
	if( (UINT8)(computeChecksum(msg) + checksum + cmt->id) )
	{
		return -2;
	}
	return msg->mid;
}

/*发送一条消息，附加校验字节*/
void cmtSendMsg(Cmt *cmt, CmtMsg *msg){
	UINT8 msgHeader[6];
	msgHeader[0] = 0xfa;
	msgHeader[1] = cmt->id;
	msgHeader[2] = msg->mid;
	if(msg->size < 255){
		msgHeader[3] = (UINT8)msg->size;
		cmtWrite(cmt->port, msgHeader, 4);
	}
	else{
		msgHeader[3] = 255;
		msgHeader[4] = (UINT8)(msg->size >> 8);
		msgHeader[5] = (UINT8)(msg->size);
		cmtWrite(cmt->port, msgHeader, 6);
	}
	cmtWrite(cmt->port, vectorGetBuffer(&msg->data, 0), msg->size);
	UINT8 cs = computeCheckbyte(msg) - cmt->id;
	cmtWrite(cmt->port, &cs, 1);
}

/* 快捷的消息发送方式，占用较多栈空间
 * 传入send指针为NULL的话，动态分配内存空间，创建新的消息体
 * len是数据长度，若不为0，后面应加上要发送的数据
 * e.g: cmtRequest(cmt, msg, 48, 0);
 *      cmtRequest(cmt, NULL, 48, 0);
 *      cmtRequest(cmt, NULL, 208, 2, 0, 4	);
 *      cmtRequest(cmt, msg, 208, 2, 0, 4	);
 * */
void cmtRequest(Cmt *cmt, CmtMsg *send, CmtMsgId mid,size_t len, ...){
	Bool createMsg = PARSE_BOOL(send == NULL);
	if(createMsg){
		send = (CmtMsg *)malloc(sizeof(CmtMsg));
		cmtMsgInit(send, len);
	}
	cmtMsgClearData(send);
	cmtMsgSetId(send, mid);
	if(len > 0){
		va_list params;
		va_start(params, len);
		while(len-- > 0){
			cmtMsgAppendByte(send, va_arg( params, int ));
		}
		va_end(params);
	}
	cmtSendMsg(cmt, send);
	if(createMsg){
		cmtMsgFreeData(send);
		free(send);
	}
}

void cmtSetReadMethod( int(* readmethod)( CmtPort, void *, size_t ) )
{
	cmtRead = readmethod;
}
void cmtSetWriteMethod( int(* writemethod)( CmtPort, const void *, size_t sz ) )
{
	cmtWrite = writemethod;
}

#endif