cmt2.c.svn-base

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

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : cmt2.c
 * Version    : 1.0
 * Brief      :
 * Code by    : leaf
 * Date       : Apr 11, 2009
 * Note       :
 *
 *
 * ------------------------------------------------------------------------
 */
#include "lib_bupt.h"

#ifdef CMT2_H_INCLUDED
/* 调整角度值，使它在[-180, 180)之间（佗螺仪返回的角度范围） */
Angle adjustAngle(Angle a){
	Angle ret = a;
	while( ret >= 180 )
		ret -= 360;
	while( ret < -180 )
		ret += 360;
	return ret;
}

/* 从佗螺仪返回的消息中提取角度信息(F1220格式) */
int getOrient( pCmtMsg msg, size_t idx, pOrient orient){
	orient->roll = cmtMsgGetIntAng( msg, idx );
	orient->pitch = cmtMsgGetIntAng( msg, idx + 4 );
	orient->yaw = cmtMsgGetIntAng( msg, idx + 8 );
	return 1;
}

void cmtReset( Cmt *cmt, CmtResetType type)
{
	CmtMsg msg;
	cmtMsgInit( &msg, 3 );
	cmtRequest( cmt, &msg, CmtMidReset, 2, 0, type );
	cmtMsgFreeData( &msg );
}

#endif