/***************************************************************************************************
Motion.c
****************************************************************************************************/
#define THIS_FILE_ID 19

#include "includes.h"

#define LSM303D_WHO_AM_I_VAL    0x49
#define LSM6DS3_WHO_AM_I_VAL    0x69

MOTION_SENSOR_TYPE MotionSensorType;

SUT_MOTION sutMotion;
/**********************************************************************
DelayNs
***********************************************************************/
void DelayNs(unsigned short ns)
{
	while(ns--);
}
/**********************************************************************
DelayUs
SystemCoreClock=72 Mhz  t=70
SystemCoreClock=60 Mhz  t=60
***********************************************************************/
//void DelayUs(unsigned short us)
//{
//	int t;
//	while(us--){
//		t=60;
//		while(t--);
//	}
//}
//void DelayMs(unsigned short ms)
//{
//	while(ms--){
//		DelayUs(1000);
//		IWDG_ReloadCounter();
//	}
//}


static int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp,
                              uint16_t len)
{
	unsigned char data;
	unsigned short i,j;
	data=Reg;
	
	if(handle==(void *)LSM303AGR_I2C_ADD_XL){
		MOTION_CS_XL_LOW;
	}else MOTION_CS_MAG_LOW;
	DELAY_NOP; 

	for(i=0;i<8;i++)
  { 
		MOTION_CLK_LOW;
		DELAY_NOP;
		DELAY_NOP;
		if((data<<i)&0x80) MOTION_SDA_HIGH;
		else MOTION_SDA_LOW;
		DELAY_NOP;
		MOTION_CLK_HIGH;
		DELAY_NOP;
  }	
	for(j=0;j<len;j++){
		data=Bufp[j];
		for(i=0;i<8;i++)
		{ 
			MOTION_CLK_LOW;
			DELAY_NOP;
			if((data<<i)&0x80) MOTION_SDA_HIGH;
			else MOTION_SDA_LOW;
			DELAY_NOP;
			MOTION_CLK_HIGH;
			DELAY_NOP;
		}	
	}
	
	if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
	else MOTION_CS_MAG_HIGH;
	DELAY_NOP;
  return 0;
}

static int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp,
                             uint16_t len)
{
	unsigned char data;
	unsigned short dat=0,i,j;
	data=0x80|Reg;
	if(handle==(void *)LSM303AGR_I2C_ADD_XL){
		MOTION_CS_XL_LOW;
		if(len>1)data|=0x40;//地址自增
	}else MOTION_CS_MAG_LOW;
	
	DELAY_NOP;
	
	//发地址
  for(i=0;i<8;i++)
  { 
		DELAY_NOP;
		MOTION_CLK_LOW;
		DELAY_NOP;
		if((data<<i)&0x80) MOTION_SDA_HIGH;
		else MOTION_SDA_LOW;
		DELAY_NOP;
		MOTION_CLK_HIGH;
		DELAY_NOP;
  }	 	

	//读数据
	for(j=0;j<len;j++){
		for(i=0;i<8;i++)
		{ 
			data<<=1;
			DELAY_NOP;
			MOTION_CLK_LOW;
			DELAY_NOP;
			MOTION_CLK_HIGH;
			DELAY_NOP;
			if(MOTION_SDA_READ) data|=0x01;
		}
		Bufp[j]=data;
	}

	if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
	else MOTION_CS_MAG_HIGH;
	
	DELAY_NOP;
  return 0;
}



/**********************************************************************************************
*
***********************************************************************************************/
void SPI_LSM_Init()                
{
	
}
void Motion_PortInit()                
{
	//SPI 初始化
	GPIO_InitTypeDef GPIO_InitStructure;                  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);                 
	//IP模拟SPI三线式  SCK--PB13   SDA(MISO/MOSI)--PB15
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13  ;   
	GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;            
	GPIO_Init(GPIOB, &GPIO_InitStructure);       
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15 ;   
	GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;            
	GPIO_Init(GPIOB, &GPIO_InitStructure);  
	
	//片选IO设置           
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);                    
	
	GPIO_InitStructure.GPIO_Pin =  MOTION_CS_XL_PIN|MOTION_CS_MAG_PIN;
	GPIO_InitStructure.GPIO_Speed =GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 
	GPIO_Init(MOTION_CS_XL_PORT, &GPIO_InitStructure);      

	MOTION_CS_XL_HIGH;
	MOTION_CS_MAG_HIGH;
}


//lsm303agr_ctx_t dev_ctx_xl;
//lsm303agr_ctx_t dev_ctx_mg;

static uint8_t whoamI, rst;
int MotionInit(void)
{
	unsigned char whoAmI = 0,r,a;
	status_t response;                              // mems error variable
	Motion_PortInit();
	os_dly_wait(10);
	//配置为SPI模式
	a=1;
	LSM303C_ACC_WriteReg(0,0x23,&a,1);
	a=4;
	LSM303C_MAG_WriteReg(0,0x22,&a,1);
	whoAmI=0;
	//读WHO AM I
  LSM303C_MAG_R_WHO_AM_I_(0, &whoAmI);
  if(whoAmI !=MAG_WHO_AM_I_VALUE){
		SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
		return 0;
	}
	whoAmI=0;
  LSM303C_ACC_ReadReg(0, LSM303C_ACC_WHO_AM_I_REG, &whoAmI, 1);
  if(whoAmI !=ACC_WHO_AM_I_VALUE){
		SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
		return 0;
	}
	
 //Initialize Magnetometer 
 //set ODR (turn ON device)
 response = LSM303C_MAG_W_OutputDataRate(0, LSM303C_MAG_DO_10Hz);
 if(response==MEMS_ERROR) while(1); //manage here comunication error

 //set Fullscale
 response = LSM303C_MAG_W_FullScale(0, LSM303C_MAG_FS_16Ga);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
   //set Block Data Update
 response = LSM303C_MAG_W_BlockDataUpdate(0, LSM303C_MAG_BDU_ENABLE);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
     //set XY Axis Operation Mode
 response = LSM303C_MAG_W_OperatingModeXY(0, LSM303C_MAG_OM_HIGH);
 if(response==MEMS_ERROR) while(1) //manage here comunication error
  
     //set Z Axis Operation Mode
 response = LSM303C_MAG_W_OperatingModeZ(0, LSM303C_MAG_OMZ_HIGH);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
     //set Continuous Mode
 response = LSM303C_MAG_W_SystemOperatingMode(0, LSM303C_MAG_MD_CONTINUOUS);
 if(response==MEMS_ERROR) while(1); //manage here comunication error  
  
 //this flag is used for reading data only when there are new data
 
 //Initialize Accelerometer 
 //set Fullscale
 response = LSM303C_ACC_SetFullScale(0, LSM303C_ACC_FS_2g);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
   //set Block Data Update
 response = LSM303C_ACC_BlockDataUpdate(0, LSM303C_ACC_BDU_ENABLE);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
 //Enable Axis 
 response = LSM303C_ACC_EnableAxis(0, LSM303C_ACC_X_ENABLE|LSM303C_ACC_Y_ENABLE|LSM303C_ACC_Z_ENABLE);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
  
 //set ODR (turn ON device)
 response = LSM303C_ACC_SetODR(0, LSM303C_ACC_ODR_10_Hz);
 if(response==MEMS_ERROR) while(1); //manage here comunication error
 
 SlwTrace(DEBUG,"Lsm303C Init OK.\r\n");
	return 1;
}


int MotionReadAccel(short *pX,short *pY,short *pZ)
{
	u8_t flag_LSM303C_ACC_STATUS_FLAGS;
	Type3Axis16bit_U data;	// accelerometer row data
	status_t response;	// mems error variable
	response = LSM303C_ACC_Status_Flags(0, &flag_LSM303C_ACC_STATUS_FLAGS);
	if(response==MEMS_ERROR)return 0;
	else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE){
		//read only if new data are available from accelerometer  
		response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
		*pX=data.i16bit[0];
		*pY=data.i16bit[1];
		*pZ=data.i16bit[2];
	}
	return 1;
}

int MotionReadMagne(short *pX,short *pY,short *pZ)
{
	LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA; 
	Type3Axis16bit_U data;	// accelerometer row data
	status_t response;	// mems error variable
	response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
	if(response==MEMS_ERROR)return 0;
	else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE){
		//read only if new data are available from accelerometer  
		response = LSM303C_MAG_Get_Magnetic(0, data.u8bit); 
		*pX=data.i16bit[0];
		*pY=data.i16bit[1];
		*pZ=data.i16bit[2];
	}
	return 1;
}
/*
循环采样，1秒一个点
*/
void MotionLoop(void)
{
	int i,i1,i2;
	char flag;
	short Ax,Ay,Az,Mx,My,Mz;
	int Axp,Ayp,Axm,Aym;
	static unsigned long lastTick=50;
	unsigned long curTick;
	static short Axx[10],Ayy[10],Azz[10],Mxx[10],Myy[10],Mzz[10];
	static unsigned char sucCt=0;
	//控制执行频率
	curTick=os_time_get();
	if(curTick>lastTick)lastTick=curTick+50;
	else return;

	flag=0;
	if(MotionReadAccel(&Ax,&Ay,&Az)){
		flag=1;
	}
	if(MotionReadMagne(&Mx,&My,&Mz)){
		flag=1;
	}
	if(flag){
		if(++sucCt>9)sucCt=0;
		//先存起来
		Axx[sucCt]=Ax;
		Ayy[sucCt]=Ay;
		Azz[sucCt]=Az;
		Mxx[sucCt]=Mx;
		Myy[sucCt]=My;
		Mzz[sucCt]=Mz;
		//再处理
		if(sucCt==9){//计算处理
			//求平均
			Axp=0;Ayp=0;
			for(i=0;i<10;i++){
				Axp+=Axx[i];
				Ayp+=Ayy[i];
			}
			Axp/=10;
			Ayp/=10;
			Axm=0;Aym=0;
			//找到最大偏差点
			for(i=0;i<10;i++){
				Ax=Axx[i]-Axp;
				Ay=Ayy[i]-Ayp;

				if(Ax<0)Ax*=-1;
				if(Ay<0)Ay*=-1;

				if(Ax>Axm){
					Axm=Ax;
					i1=i;
				}
				if(Ay>Aym){
					Aym=Ay;
					i2=i;
				}

			}
			tsk_lock();
			sutMotion.Flag=0;
			//判断Ax还是Ay偏差最大
			if(Axm>Aym){
				//Ax偏差最大，取Ax
				sutMotion.Ax=Axx[i1];
				sutMotion.Ay=Ayy[i1];
				sutMotion.Az=Azz[i1];
				sutMotion.Mx=Mxx[i1];
				sutMotion.My=Myy[i1];
				sutMotion.Mz=Mzz[i1];
				if(Axm>(Axp+ACC_THRESHOLD))sutMotion.Flag=1;
			}else{
				//Ay偏差最大，取Ay
				sutMotion.Ax=Axx[i2];
				sutMotion.Ay=Ayy[i2];
				sutMotion.Az=Azz[i2];
				sutMotion.Mx=Mxx[i2];
				sutMotion.My=Myy[i2];
				sutMotion.Mz=Mzz[i2];
				if(Aym>(Ayp+ACC_THRESHOLD))sutMotion.Flag=2;
			}
			tsk_unlock();
			//打印
#if 0
			float fAx,fAy,fAz,fMx,fMy,fMz;
			fAx=sutMotion.Ax*SENSITIVITY_ACC;
			fAy=sutMotion.Ay*SENSITIVITY_ACC;
			fAz=sutMotion.Az*SENSITIVITY_ACC;
			fMx=sutMotion.Mx*SENSITIVITY_MAG;
			fMy=sutMotion.My*SENSITIVITY_MAG;
			fMz=sutMotion.Mz*SENSITIVITY_MAG;
			printf("\r\n");	
			printf("Ax=%10.3f\r\n",fAx);
			printf("Ay=%10.3f\r\n",fAy);
			printf("Az=%10.3f\r\n",fAz);
			printf("Mx=%10.3f\r\n",fMx);
			printf("My=%10.3f\r\n",fMy);
			printf("Mz=%10.3f\r\n",fMz);	
			printf("----Flag=%d---\r\n",sutMotion.Flag);	
#endif
		}
	}else{
		SlwTrace(DEBUG,"Motion Test Error!\r\n");	
		sucCt=0;
		tsk_lock();
		sutMotion.Ax=0;
		sutMotion.Ay=0;
		sutMotion.Az=0;
		sutMotion.Mx=0;
		sutMotion.My=0;
		sutMotion.Mz=0;
		sutMotion.Flag=0;
		tsk_unlock();
	}
}

/***************************************************
*每20ms调用一次
****************************************************/
void MotionTest(void)
{
	static unsigned char ct=0;
	int Ax,Ay,Az,Mx,My,Mz;
	float mgData_X, mgData_Y,mgData_Z;              //data in mg
	float gaussData_X, gaussData_Y, gaussData_Z;    //data in Gauss
	
	u8_t flag_LSM303C_ACC_STATUS_FLAGS;
	LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA; 
	Type3Axis16bit_U data;                                 // accelerometer row data
	status_t response;                              // mems error variable
	
		if(++ct>100)ct=0;
		else return;
	 //get Acceleration Raw data  
	 response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
	 if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR111"); //manage here comunication error
	 //read only if new data are available from accelerometer  
	 else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE)
	 {
			response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
			//convert from LSB to mg
			mgData_X=data.i16bit[0]*SENSITIVITY_ACC; 
			mgData_Y=data.i16bit[1]*SENSITIVITY_ACC;
			mgData_Z=data.i16bit[2]*SENSITIVITY_ACC;
		 Ax=data.i16bit[0];
		 Ax=data.i16bit[1];
		 Ax=data.i16bit[2];
	 }
	//get Magnetic Raw data 

	 response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
	 if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR222"); //manage here comunication error
	 //read only if new data are available  
	 else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE)
	 {
			response = LSM303C_MAG_Get_Magnetic(0, data.u8bit); 
			//convert from LSB to Gauss
			gaussData_X=data.i16bit[0]*SENSITIVITY_MAG;
			gaussData_Y=data.i16bit[1]*SENSITIVITY_MAG;
			gaussData_Z=data.i16bit[2]*SENSITIVITY_MAG;
		 Mx=data.i16bit[0];
		 My=data.i16bit[1];
		 Mz=data.i16bit[2];
	 }

	 printf("\r\n");	
	 printf("Ax=%10.3f\r\n",mgData_X);
	 printf("Ay=%10.3f\r\n",mgData_Y);
	 printf("Az=%10.3f\r\n",mgData_Z);
	 printf("Mx=%10.3f\r\n",gaussData_X);
	 printf("My=%10.3f\r\n",gaussData_Y);
	 printf("Mz=%10.3f\r\n",gaussData_Z);	
	 printf("----\r\n");	

	
}