player/Project_App_Calendar/App_Src/win/mywin_user_9axis.c

#include "mywin_inc.h"
#include "mywin_user_9axis.h"
#include "qmc6310.h"
#include "math.h"






WIN_9axisStruct *WIN_Creat9axis (WIN_WindowStruct *base,
			void (*msgLoop)(struct _WIN_WindowStruct *win,WIN_MsgStruct *msg),
			int x,int y,int x_size,int y_size)
{
	//重设消息循环
	if (msgLoop==0)
	{
		msgLoop=(void (*)(struct _WIN_WindowStruct *win,WIN_MsgStruct *msg))AXIS_defaultMsgLoop; 
	}
	
	WIN_9axisStruct *ret=mymalloc (sizeof ( WIN_9axisStruct));
	//调用父类的构造函数
	if (ret)
	{
		mymemset (ret,0,sizeof ( WIN_9axisStruct));
		if (0==WIN_CreatTouchEx((WIN_TouchWinStruct *)ret,base,msgLoop,x,y,x_size,y_size))
		{
			//创建失败
			myfree (ret);
			ret=0;
		}
		else
		{
			((WIN_WindowStruct *)ret)->winType="WIN_9axisStruct";
			((WIN_WindowStruct *)ret)->intercept=1; 
//			((WIN_WindowStruct *)ret)->deleteWindow=(void (*)(struct _WIN_WindowStruct *win))WIN_DeleteSpectrum;
			//构造一个消息框
			qmc6310_init();
			ret->timer_20ms=WIN_CreatTimer ((WIN_WindowStruct *)ret,20);
			ret->timer_refresh=WIN_CreatTimer ((WIN_WindowStruct *)ret,300);
			ret->fitt_on=1;
			
			for(int i=0;i<WIN_GET_ARRAY_SIZE(ret->kal);i++)
			{
				kalman_init(&ret->kal[i]);
			}
		}
	}
	
	
	return ret;
}




//消息框的绘制函数
void AXIS_DefaultPaint (WIN_9axisStruct *axis)
{
	int x=0;
	int y=0;
	int x_size=((WIN_WindowStruct *)axis)->x_size;
	int y_size=((WIN_WindowStruct *)axis)->y_size;
	char *txt=mymalloc(128);
	
	WIN_PaintBackGround ((WIN_WindowStruct *)axis);
	
	int font=WIN_SetFontType(24);
	WIN_SetLcdColor(0);
	
	sprintf(txt,"point num:%d",axis->ell.num);
	WIN_DrawTxtAt(txt,x,y);y+=30;
	
	sprintf(txt,"mag_raw:%.2f,%.2f,%.2f",axis->mag_raw[0],axis->mag_raw[1],axis->mag_raw[2]);
	WIN_DrawTxtAt(txt,x,y);y+=30;
	
	sprintf(txt,"A:%.2f,B:%.2f,C:%.2f\nX0:%.2f,Y0:%.2f,Z0:%.2f",axis->ell.a,axis->ell.b,axis->ell.c,
			axis->ell.x,axis->ell.y,axis->ell.z);
	WIN_DrawTxtAtRect(txt,x,y,x_size,y_size-y);y+=60;
	
	float *mag=axis->fitt_raw;
	
	sprintf(txt,"fitt_raw:%.2f,%.2f,%.2f",mag[0],mag[1],mag[2]);
	WIN_DrawTxtAt(txt,x,y);y+=30;

	sprintf(txt,"mod:%.2f",axis->mod);
	WIN_DrawTxtAt(txt,x,y);y+=30;
	sprintf(txt,"angle:%.2f",axis->angle);
	WIN_DrawTxtAt(txt,x,y);y+=30;
		
	WIN_SetFontType(font);
	myfree(txt);
}






void AXIS_Refresh(WIN_9axisStruct *axis)
{
	//椭圆拟合
	if(axis->ell.num>10&&axis->ell.num%10==0)
	{
		if(axis->fitt_on)
			ellipsoid_fitting(&axis->ell);
	}
	
	//计算校准后的磁力值
	if(axis->ell.a!=0.0f)
		axis->fitt_raw[0]=(axis->mag_raw[0]-axis->ell.x)/axis->ell.a;
	if(axis->ell.b!=0.0f)
		axis->fitt_raw[1]=(axis->mag_raw[1]-axis->ell.y)/axis->ell.b;
	if(axis->ell.c!=0.0f)
		axis->fitt_raw[2]=(axis->mag_raw[2]-axis->ell.z)/axis->ell.c;
	
	//计算模
	axis->mod=sqrtf(axis->fitt_raw[0]*axis->fitt_raw[0]+axis->fitt_raw[1]*axis->fitt_raw[1]+axis->fitt_raw[2]*axis->fitt_raw[2]);
	
	//判断是否需要重新校准
	if(axis->mod-1>=-0.1f&&axis->mod-1<=0.1f)
	{
		axis->suss_num++;
		if(axis->suss_num>50) axis->fitt_on=0;
	}
	else 
	{
		axis->suss_num=0;
		axis->fitt_on=1;
	}
	
	//计算方向
	axis->angle=atan2(axis->fitt_raw[1],axis->fitt_raw[0])*180.0f/3.14159f;
}









//消息处理函数
void AXIS_defaultMsgLoop (WIN_9axisStruct *axis,WIN_MsgStruct *msg)
{
	WIN_KeyStruct *k=0;
	switch (msg->msg)
	{	
		case WIN_MSG_PAINT:
			AXIS_DefaultPaint(axis);
		break;
		case WIN_MSG_TIMER:
			if(msg->data.v==axis->timer_20ms)
			{
				qmc6310_read_mag_xyz(axis->mag_raw);
				for(int i=0;i<WIN_GET_ARRAY_SIZE(axis->kal);i++)
				{
					axis->mag_raw[i]=kalman_filter(&axis->kal[i],axis->mag_raw[i]);
				}
				if(axis->fitt_on)
					ellipsoid_add_point(&axis->ell,axis->mag_raw[0],axis->mag_raw[1],axis->mag_raw[2]);
			}
			else if(msg->data.v==axis->timer_refresh)
			{
				AXIS_Refresh(axis);
				WIN_SetInvalidWhenTop((WIN_WindowStruct *)axis);
			}
		break;
		case WIN_MSG_KEY:
			k=msg->data.p;
			if (k->shortPress&KEY_VALUE_UP)//短按
			{
			}
			else if (k->shortPress&KEY_VALUE_DOWN)
			{
			}
			else if (k->shortPress&KEY_VALUE_HOME)
			{
					((WIN_WindowStruct *)axis)->deleteWindow((WIN_WindowStruct *)axis);
			}
			if (k->shortPress)
			{
				WIN_SetInvalid ((WIN_WindowStruct *)axis);
			}
			break;//case WIN_MSG_KEY:
		default:
			WIN_DefaultMsgLoop((WIN_WindowStruct *)axis,msg);
		break;
	}
}



WIN_9axisStruct *AXIS_StartTest(WIN_WindowStruct *base)
{
	
	WIN_9axisStruct *ret=WIN_Creat9axis (base,0,0,0,base->x_size,base->y_size);
	return ret;
}