cmtmsg.c.svn-base

AVR单片机下

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : cmtmsg.c
 * Version    : 1.0
 * Brief      : Mti消息相关的操作
 * Code by    : leaf
 * Date       : Apr 3, 2009
 * Note       : 消息的数据空间会自动增加，但是在消息不用时应该调用CmtMsgFreeData()
 *              释放数据空间，以防止内存泄漏
 *              向消息添加数据推荐使用appendData/appendByte方法，不要手动更改size
 *
 * ------------------------------------------------------------------------
 */
#include "lib_bupt.h"

#ifdef CMTMSG_H_INCLUDED
/*初始化一条消息，datasize为初始分配的数据空间，推荐为2*/
void cmtMsgInit(CmtMsg *msg, size_t datasize){
	msg->size = 0;
	msg->mid  = 0;
	vectorInit(&msg->data, datasize);
}

/*设置消息ID，推荐使用这个函数而不是直接设置*/
void cmtMsgSetId(CmtMsg *msg, CmtMsgId id){
	msg->mid = id;
}

/*向消息中添加参数len指定长度的数据，自动改变size变量*/
void cmtMsgAppendData(CmtMsg *msg, const void *data, size_t len){
	msg->size += vectorWrite(&msg->data, msg->size, (const VectorDataType *)data, len);
}

/*向消息中添加一字节的数据*/
void cmtMsgAppendByte(CmtMsg *msg, UINT8 data){
	vectorSet(&msg->data, msg->size, data);
	if( msg->size < msg->data.capacity )
		++ msg->size;
}

/*复位size变量，相当于使当前消息体中的数据无效，但是不会释放数据空间*/
void cmtMsgClearData(CmtMsg *msg){
	msg->size = 0;
}

/*复位size变量，同时释放数据空间*/
void cmtMsgFreeData(CmtMsg *msg){
	msg->size = 0;
	vectorFree(&msg->data);
}

/*显示一条消息，使用标准的输入/输出*/
void cmtMsgDisplay(CmtMsg *msg, FILE *dev){
	fprintf(dev, "I_%3u ", msg->mid);
	fprintf(dev, "S_%3u\n", msg->size);
	int i;
	fprintf(dev, "D_");
	if(msg->size <= 0){
		fprintf(dev, "N/A \n");
	}
	else{
		for(i = 0; i < msg->size; ++i){
			fprintf(dev, "%02x ", (UINT8)vectorGet(&msg->data, i));
		}
		fprintf(dev, "\n");
	}
}

/*从消息中读取一字节的数据*/
UINT8 cmtMsgGetByte(const CmtMsg *msg, size_t idx){
	return vectorGet(&msg->data, idx);
}

/* 从消息中提取一个浮点数，但是必需知道这个浮点数从数据区哪个字节开始
 * (如果只返回欧拉角度，则分别从0, 4, 8开始）
 * eg: pitch = cmtMsgGetFloat(msg, 4);
 * */
float cmtMsgGetFloat(const CmtMsg *msg, size_t idx){
	float ret;
	UINT8 *dest = (UINT8 *)&ret;
	dest[0] = vectorGet(&msg->data, idx + 3);
	dest[1] = vectorGet(&msg->data, idx + 2);
	dest[2] = vectorGet(&msg->data, idx + 1);
	dest[3] = vectorGet(&msg->data, idx + 0);
	return ret;
}

/*
 *  当MTi返回数据以1220定点数形式时，可以用这个函数取得这个32位定点数的前16位
 * （1220的意思：前12位是符号位＋整数部分，后20位是小数部分）
 */
INT16 cmtMsgGetInt16(const CmtMsg *msg, size_t idx){
	INT16 ret;
	UINT8 *dest = (UINT8 *)&ret;
	dest[0] = vectorGet(&msg->data, idx + 1);
	dest[1] = vectorGet(&msg->data, idx);
	return ret;
}

/* 当MTi返回数据是1220格式时，取得其符号和整数部分*/
INT16 cmtMsgGetIntAng(const CmtMsg *msg, size_t idx){
	INT16 raw = cmtMsgGetInt16(msg, idx);
	Angle ang = raw >> 4;
	ang += ( raw % 16 / 8 );
	return adjustAngle( ang );
}

/* 当MTi返回数据是1220格式时，转化为浮点格式*/
double cmtMsgGetF1220(const CmtMsg *msg, size_t idx){
	double ret;
	INT32 tmp;
	UINT8* dest = (UINT8*) &tmp;
	dest[0] = vectorGet(&msg->data, idx + 3);
	dest[1] = vectorGet(&msg->data, idx + 2);
	dest[2] = vectorGet(&msg->data, idx + 1);
	dest[3] = vectorGet(&msg->data, idx + 0);

	ret = ((double) tmp)/1048576.0;
	return ret;
}

#endif