/*
 * Created: 2023/03/20 
 *  Author:  wangzk zhengmengxing
 */ 
#include	"..\PrjCommon\CommonDef.h"
#include    "AttMath.h"
/****************************************************
 功能: 将输入值限制在[-1，1]
 输入：
 输出：
*****************************************************/
TYPE_CAL PoseLimitInOne(const TYPE_CAL X)
{
	if (X > 1.0f) 
        return 1.0f;
	if (X < -1.0f) 
        return -1.0f;
	return X;
}
/****************************************************
 功能: 开平方 最小模式使用
 输入：
 输出：
*****************************************************/
//#ifdef MINMODULE_TYPE
//TYPE_CAL Posesqrt( TYPE_CAL m )
//{
	//TYPE_CAL x,y;
	//INT8 Li;
	//
	//if(m<0.0F)	/*小于0，则不求 */
	//{ return 0.0F; }
	//else
	//{
		//x=1.0F;
		//y=(x+m/x)/2.0F;
		//Li = 0x0;
		//while( Li < 20 )
		 //{
			  //x=y;
			  //y=(x+m/x)/2.0F;
			  //Li++;
		  //}
		//return x;
	//}
//}
//#endif
/****************************************************
 功能:绝对值 最小模式使用
 输入： 
 输出： 
*****************************************************/
//#ifdef MINMODULE_TYPE
//TYPE_CAL Poseabsf( TYPE_CAL x )
//{
	//TYPE_CAL y;
	//if( x > 0.0F  )
	//{ y = x; }
	//else
	//{ y = -x; }
	//return y;	/* 返回值 */
//}
//#endif
/****************************************************
 功能:反余弦 最小模式使用
 输入：
 输出：
*****************************************************/
//#ifdef MINMODULE_TYPE
//TYPE_CAL Poseacosf(TYPE_CAL x)
//{	
	//INT8 Li;
	//TYPE_CAL Lx, Ly, Lz;
	//TYPE_CAL LM[11] ={ 3.141593F, 2.498091F, 2.214298F, 1.982313F, 1.772154F, 1.570796F, 1.369438F, 1.159279F, 0.927295F, 0.643501F, 0.0F  };
//
	//Ly = x + 1.0F;
	//Lz = Ly/0.2F;
	//Li = (INT8)(Lz);
	//Lx = LM[Li]+( LM[Li+1]-LM[Li] )*( Lz - Li );
	//
	//return Lx;
//}
//#endif
/****************************************************
 功能: 取符号 1
 输入：浮点数  
 返回：1表示输入值为正，-1表示输入值为负，0表示输入值为零
*****************************************************/
char CalPFlag(TYPE_CAL  Data)
{
	char   tmpRes;

	tmpRes = 0;

	if(POSE_ZERO < Data)
	{
		tmpRes = 1;
	}
	else if(-POSE_ZERO > Data)
	{
		tmpRes = -1;
	}else{;}

	return tmpRes;
}

/*******************************************
 说明：N维矢量点乘 2
 输入： x1，x2:矢量，长度为ucn，返回值re
		ucN：矢量的长度
 返回：点乘结果
 注意： 数组溢出
*******************************************/
void Posedot(TYPE_CAL* x1, TYPE_CAL* x2, UINT8 ucN, TYPE_CAL * pRes)					
{
	UINT8 i;
	TYPE_CAL fReturn;

	fReturn = 0.0F;

	for(i=0;i<ucN;i++)
	{   
		fReturn += x1[i]*x2[i]; 
	}

	*pRes = fReturn;
}

/*******************************************
 说明：3维矢量叉乘 3
 输入： x1，x2:矢量，长度为ucn，返回值re
		ucN：矢量的长度
 输出：x3：叉乘结果
 注意： 数组溢出 ，指针必须有效
*******************************************/
void cross(TYPE_CAL * x1, TYPE_CAL * x2, TYPE_CAL *  x3)
{
	x3[0] = x1[1]*x2[2]-x1[2]*x2[1];
	x3[1] = x1[2]*x2[0]-x1[0]*x2[2];
	x3[2] = x1[0]*x2[1]-x1[1]*x2[0];
}

/*******************************************
 说明：N维矢量模值 4
 输入： x矢量，长度为ucn，返回值re
		ucN：矢量的长度
 返回：矢量模
 注意： 数组溢出
*******************************************/
void CalNormal( TYPE_CAL* x, UINT8 ucN, TYPE_CAL * pRes )
{
	TYPE_CAL ftmp, tmpX;
	UINT8 Li = 0x00;
	ftmp = 0.0;
	tmpX = 0.0;

	for( Li=0; Li<ucN; Li++ )
	{ 
		tmpX = x[Li];
		ftmp += tmpX*tmpX;
    }

	ftmp = POSE_SQRTF(ftmp);
	*pRes = (TYPE_CAL)ftmp;
}

/****************************************************
 功能: 构造对角阵 5
 输入： inLine : 矢量
		iLen： 矢量长度
 输出：outMatrix :矩阵
 注意，指针所包含存储区域长度要足够，避免越界,
*****************************************************/
void 	CalMakeCrossMatrix(TYPE_CAL * inLine, TYPE_CAL * outMatrix, UINT16 iLen)
{
	UINT16  i;
	memset(outMatrix, 0, iLen * iLen * sizeof(TYPE_CAL));
	for(i = 0; i < iLen; ++i)
	{
		outMatrix[i * iLen + i] = inLine[i];
	}
}

/*********************************************
 说明：两个矩阵相乘 [h l]*[l*f] 6
 输入： A1,A2,矩阵
		H,L,F，L为共同维数
 输出：A3矩阵H * F
 注意： 数组溢出
*******************************************/
void MatrixProductHL( TYPE_CAL *A1, TYPE_CAL *A2, TYPE_CAL *A3, UINT8 H, UINT8 L, UINT8 F )
{
	UINT8   i,j,k;

	memset( &A3[0], 0, H*F*4 );
	for (i=0; i<H; i++)
	{
		for(k=0; k<F; k++)
		{
			for (j=0; j<L; j++)
			{   A3[F*i + k] += A1[L*i + j]*A2[F*j+k];   }
		}
	}
}

/********************************************
 说明：H L维矩阵转置  7
 输入： A1,矩阵【H* L】
		H,L,维数
 输出：A2矩阵【L* H】
 注意： 数组溢出
*******************************************/
void MatrixTransposeHL(TYPE_CAL * A1, TYPE_CAL * A2, UINT8 H, UINT8 L )
{
	UINT8 m, n;

	for( m=0; m<L; m++ )
	{
		for( n=0; n<H; n++ )
			{ A2[m*H+n] = A1[n*L+m];}
	}
}

/****************************************************
 功能 : 三维矩阵求逆 8
 输入 ： cI: 原始实矩阵
		 ef: 可逆与否的阈值
 输出 ： cs是否可逆标志
		 cO求逆后矩阵
 注意：
*****************************************************/
void InvsymMatrix3(TYPE_CAL *cI, TYPE_CAL *cO, UINT8 *cs,  TYPE_CAL * ef)
{
	TYPE_CAL A[3][3];
	UINT8 j;
	TYPE_CAL x,n;
	TYPE_CAL tmpef;

	tmpef = *ef;
	n= 0.0F;
	memcpy( &A[0][0], cI, 9 * sizeof(TYPE_CAL));
	*cs = POSE_OK;  

	for(j = 0;j < 3;j++)
	{
		if(j == 2)
			n += A[0][j] * A[1][0] * A[2][1];
		else if(j == 1)
			n += A[0][j] * A[1][j+1] * A[2][0];
		else
			n += A[0][j] * A[1][j+1] * A[2][j+2];
	}

	for(j = 0;j < 3;j++)
	{
		if(j == 2)
			n -= A[2][j] * A[1][0] * A[0][1];
		else if(j==1)
			n -= A[2][j] * A[1][j+1] * A[0][0];
		else
			n -= A[2][j] * A[1][j+1] * A[0][j+2];
	}

	if( POSE_ABSF(n) > tmpef )
	{
		x = 1.0F/n;

		cO[0] = ( A[1][1] * A[2][2] - A[1][2] * A[2][1] )*x;
		cO[4] = ( A[0][0] * A[2][2] - A[0][2] * A[2][0] )*x;
		cO[8] = ( A[0][0] * A[1][1] - A[0][1] * A[1][0] )*x;

		cO[1] = (-A[0][1] * A[2][2] + A[0][2] * A[2][1] )*x;
		cO[2] = ( A[0][1] * A[1][2] - A[0][2] * A[1][1] )*x;
		cO[5] = (-A[0][0] * A[1][2] + A[0][2] * A[1][0] )*x;

		cO[3] = (-A[1][0] * A[2][2] + A[2][0] * A[1][2] )*x;
		cO[6] = ( A[1][0] * A[2][1] - A[2][0] * A[1][1] )*x;
		cO[7] = (-A[0][0] * A[2][1] + A[2][0] * A[0][1] )*x;
	}
	else { *cs = POSE_NO; }
}

/****************************************************
功能: 用线性插值公式计算插值 9
输入：  x ：X轴矢量 必须为递增
		y： Y 轴矢量
		iLen：矢量长度
		Xi： 需要插值的点
返回：得到的插值
注意： 输入x必须为递增
*****************************************************/
//#ifndef MINMODULE_TYPE
//void CalInterPolation(TYPE_CAL * x, TYPE_CAL * y, UINT32 iLen, TYPE_CAL *Xi,TYPE_CAL* tmpTF)
//{
	//TYPE_CAL  result = 0;
	////TYPE_CAL  Xm[3];
	//TYPE_CAL  tmpXi;
	//UINT32      i;
	//UINT32    tmpPos;
//
	//tmpXi = *Xi;
//
	//if(0 == iLen)
	//{
		//result = 0;
	//}
	//else if(1 == iLen)
	//{
		//result = y[0];
	//}
	//else if(2 == iLen)
	//{
		//if(CalPFlag(x[0] - x[1]) != 0)
		//{
			//result = (y[0]*( tmpXi - x[1]) - y[1] * (tmpXi - x[0])) / (x[0] - x[1]);
		//}
		//else
			//result = y[0];
	//}
	//else
	//{
		//if(tmpXi <= x[0])
		//{
			//tmpPos = 0;
		//}
		//else if(tmpXi >= x[iLen - 1])
		//{
			//tmpPos = iLen - 2;
		//}
		//else
		//{
			//tmpPos = 0;
			///*首先找到的位置*/
			//for(i = 1; i < iLen; ++i)
			//{
				//if(tmpXi <= x[i])
				//{
					///*确认最相近的2个点*/
					//tmpPos = i - 1;
					//break;
				//}
			//}
		//}
//
		//if(CalPFlag(x[tmpPos] - x[tmpPos + 1]) != 0)
		//{
			//result = y[tmpPos] * (tmpXi - x[tmpPos + 1]) - y[tmpPos + 1] * (tmpXi - x[tmpPos]);
			//result = result / (x[tmpPos] - x[tmpPos + 1]);
		//}
		//else
			//result = y[tmpPos];
	//}
	//*tmpTF = result;
//}
//#endif

/****************************************************
功能:	计算两个矢量夹角 10
输入：  x ：矢量
		y： 矢量

返回：pRet
*****************************************************/
UINT8	CalVecArc(TYPE_CAL * VecX, TYPE_CAL * VecY, TYPE_CAL *pRet)
{
	TYPE_CAL tmpModL1, tmpModL2, tmpDot, tmpUse;

	if((NULL == VecX) || (NULL == VecY) || (NULL == pRet))
		return (1);
	CalNormal(VecX, 3, &tmpModL1);
	CalNormal(VecY, 3, &tmpModL2);
	Posedot(VecX, VecY, 3, &tmpDot);

	if(POSE_ZERO > tmpModL1 * tmpModL2)
		return (1);

	tmpUse = tmpDot / (tmpModL1 * tmpModL2);

	if(POSE_ABSF(tmpUse) > 1.0F)
	{
		if(1 == CalPFlag(tmpUse))
		{
			tmpUse = 1.0F;
		}
		else if(-1 == CalPFlag(tmpUse) )
		{
			tmpUse = -1.0F;
		}
	}
	
	*pRet = POSE_ACOSF(tmpUse);

	return (0);
}

/****************************************************** 
 功能： N维矢量归一化即单位化
 输入：
 输出：
 描述：在多处被调用
 *****************************************************/
void unitary(TYPE_CAL* pVector, UINT8 ucN)
{
	UINT8 i;
	TYPE_CAL ftemp;

	CalNormal( pVector, ucN, &ftemp);           /* 取矢量的模*/
	if (ftemp > POSE_ZERO)
	{
		for (i=0; i<ucN; i++)
		{   pVector[i] = pVector[i]/ftemp;  }   /* 矢量单位化*/
	}
}

/***********************************************
 说明：构造N维单位阵
 输入：N：矩阵维数
 输出：A：N * N维矩阵
************************************************/
//#ifndef MINMODULE_TYPE
//void MatrixIcons( TYPE_CAL *A, UINT8 N )
//{
	//UINT8 i;
	//memset( &A[0], 0, N*N*4 );
//
	//for( i=0; i<N; i++ )
	//{ A[i*N+i] = 1.0F; }    /*对角线元素*/
//}
//#endif

/****************************************************
功能: n个数按从大到小排序
输入： cd：矢量
	   n：矢量维数
输出： cd:排序完成的矢量
*****************************************************/
void SortUint16( UINT16 *cd, UINT8 n )
{
	UINT8 Li, Lj;
	UINT16 tmpf;
	for( Li=0; Li<n; Li++ )
	{
		for( Lj=0; Lj < n-Li-1; Lj++ )
		{
			if( cd[Lj]<cd[Lj+1] )
			{
				tmpf = cd[Lj];
				cd[Lj] = cd[Lj+1];
				cd[Lj+1] = tmpf;
			}
		}
	}
}

/********************************
功能：MN维矩阵与N矢量相乘 
输入：pMatrix：矩阵M * N    
	 pVector:矢量N维
     pReturn：M维矢量
返回：
*******************************************************/
void MatrixPlusVectorN(TYPE_CAL* pMatrix, TYPE_CAL* pVector, TYPE_CAL* pReturn, UINT8 M, UINT8 N )
{
	UINT16   i,j;
	for (i=0; i<M; i++)
	{
		pReturn[i] = 0.0F;
		for (j=0; j<N; j++)
		{
			pReturn[i] += pMatrix[i*N+j] * pVector[j];
		}
	}
}

/***************************************************
功能：四元数得到姿态角矩阵 
输入：cq：4元素
输出：ca:姿态角矩阵
***************************************************/
void Q2Att( TYPE_CAL *cq, TYPE_CAL *ca )
{
	ca[0] = cq[0]*cq[0]-cq[1]*cq[1]-cq[2]*cq[2]+cq[3]*cq[3];
	ca[1] = 2.0F*( cq[0]*cq[1]+cq[2]*cq[3] );
	ca[2] = 2.0F*( cq[0]*cq[2]-cq[1]*cq[3] );

	ca[3] = 2.0F*( cq[0]*cq[1]-cq[2]*cq[3] );
	ca[4] = -cq[0]*cq[0]+cq[1]*cq[1]-cq[2]*cq[2]+cq[3]*cq[3];
	ca[5] = 2.0F*( cq[1]*cq[2]+cq[0]*cq[3] );

	ca[6] = 2.0F*( cq[0]*cq[2]+cq[1]*cq[3] );
	ca[7] = 2.0*( cq[1]*cq[2]-cq[0]*cq[3] );
	ca[8] = -cq[0]*cq[0]-cq[1]*cq[1]+cq[2]*cq[2]+cq[3]*cq[3];
}


/*****************************************************
 功能：姿态角至姿态矩阵  
 输入：att：姿态角
 输出：A:姿态角矩阵
***************************************************/
void Att2M( TYPE_CAL *att, TYPE_CAL *A )
{
	TYPE_CAL s0, c0, s1, c1, s2, c2;
	s0 = POSE_SINF( att[0] );
	c0 = POSE_COSF( att[0] );
	s1 = POSE_SINF( att[1] );
	c1 = POSE_COSF( att[1] );
	s2 = POSE_SINF( att[2] );
	c2 = POSE_COSF( att[2] );

	A[0] = c1*c2-s0*s1*s2;
	A[1] = c1*s2+s0*s1*c2;
	A[2] = -c0*s1;

	A[3] = -c0*s2;
	A[4] = c0*c2;
	A[5] = s0;

	A[6] = s1*c2+s0*c1*s2;
	A[7] = s1*s2-s0*c1*c2;
	A[8] = c0*c1;
}

/***************************************************
 功能：姿态矩阵至姿态角  
 输入：M:姿态角矩阵
 输出：ca：姿态角
***************************************************/
//#ifndef MINMODULE_TYPE
//void M2Att( TYPE_CAL *M , TYPE_CAL *ca )
//{
	//if(POSE_ABSF(M[5]) > 1.0F)
	//{
		//if(1 == CalPFlag(M[5]))
		//{
			//M[5] = 1.0F;
		//}
		//else if(-1 == CalPFlag(M[5]))
		//{
			//M[5] = -1.0F;
		//}
	//}
//
	//ca[0] = POSE_ASINF( M[5] );        /*A[1][2]*/
//
	//if(POSE_ABSF(POSE_ABSF(ca[0]) - POSE_05PI) < POSE_ZERO)
	//{
		//ca[1] = 0.0;
		//ca[2] = 0.0;
		//return;
	//}
	//
	//ca[1] = POSE_ATAN2F( -M[2], M[8] ); /*2013。12。11更改，需要改动需求*/
	//ca[2] = POSE_ATAN2F( -M[3], M[4] ); /* -A[1][0] A[1][1]*/
//}
//#endif

/******************************************************
 功能：姿态矩阵得到对应的四元数
 输入：iM3：姿态矩阵
 输出：cq:4元素
*******************************************************/
void Att2Q( TYPE_CAL *iM3, TYPE_CAL *cq )
{
	TYPE_CAL Ltmp;
	TYPE_CAL    tmpCp[4];
	//TYPE_CAL tmpUse = 0;
	UINT16  k, tmpNO;

	tmpCp[0] = POSE_ABSF(1.0 + iM3[0]+iM3[4]+iM3[8]);
	tmpCp[1] = POSE_ABSF(1.0 + iM3[0]-iM3[4]-iM3[8]);
	tmpCp[2] = POSE_ABSF(1.0 - iM3[0]+iM3[4]-iM3[8]);
	tmpCp[3] = POSE_ABSF(1.0 - iM3[0]-iM3[4]+iM3[8]);

	Ltmp = 0;
	tmpNO = 0;
	for(k = 0; k < 4; ++k)
	{
		if(Ltmp < tmpCp[k])
		{
			Ltmp = tmpCp[k];
			tmpNO = k;
		}
	}

	Ltmp = POSE_SQRTF(Ltmp) * 0.5;
	if(0 == CalPFlag(Ltmp))
	{
		cq[0] = 0.0;
		cq[1] = 0.0;
		cq[2] = 0.0;
		cq[3] = 1.0;
	}
	else if(0 == tmpNO)
	{
		cq[3] = Ltmp;
		cq[0] = ( iM3[5] - iM3[7] )/4.0F/Ltmp;      /* A^T12 - A^T21 */
		cq[1] = ( iM3[6] - iM3[2] )/4.0F/Ltmp;      /* A^T20 - A^T02 */
		cq[2] = ( iM3[1] - iM3[3] )/4.0F/Ltmp;      /* A^T01 - A^T10 */

	}
	else if(1 == tmpNO)
	{
		cq[0] = Ltmp;
		cq[1] = ( iM3[1] + iM3[3] )/4.0F/Ltmp;
		cq[2] = ( iM3[2] + iM3[6] )/4.0F/Ltmp;
		cq[3] = ( iM3[5] - iM3[7] )/4.0F/Ltmp;
	}
	else if(2 == tmpNO)
	{
		cq[1] = Ltmp;
		cq[2] = ( iM3[5] + iM3[7] )/4.0F/Ltmp;
		cq[3] = ( iM3[6] - iM3[2] )/4.0F/Ltmp;
		cq[0] = ( iM3[1] + iM3[3] )/4.0F/Ltmp;
	}
	else
	{
		cq[2] = Ltmp;
		cq[3] = ( iM3[1] - iM3[3] )/4.0F/Ltmp;
		cq[0] = ( iM3[2] + iM3[6] )/4.0F/Ltmp;
		cq[1] = ( iM3[5] + iM3[7] )/4.0F/Ltmp;
	}
	/*判断q4大于或等于0，不变号。小于0，q1，q2，q3，q4符号取反 20130717 */
	if(cq[3] < 0.0)
	{
		cq[0] = -cq[0];
		cq[1] = -cq[1];
		cq[2] = -cq[2];
		cq[3] = -cq[3];
	}
	/*unitary( cq, 4);*/
}

/********************************************************
 功能：四元数得到四元数矩阵
 输入：p：4元素
	   kind：1为正常次序
 输出：rM4:4元素矩阵
*******************************************************/
//#ifndef MINMODULE_TYPE
//void Q2Q44( TYPE_CAL *p, TYPE_CAL *rM4, UINT8 kind )
//{   /*Q2=Q*Q1 */
	//if( kind == 1 ) /* 正常次序 应用于Q2=A(Q1)*Q */
	//{
		//rM4[0] = p[3];  rM4[1] = p[2];  rM4[2] =-p[1];  rM4[3] = p[0];
		//rM4[4] =-p[2];  rM4[5] = p[3];  rM4[6] = p[0];  rM4[7] = p[1];
		//rM4[8] = p[1];  rM4[9] =-p[0];  rM4[10]= p[3];  rM4[11]= p[2];
		//rM4[12]=-p[0];  rM4[13]=-p[1];  rM4[14]=-p[2];  rM4[15]= p[3];
	//}
	//else             /* 应用于 Q2=A(Q)*Q1 */
	//{
		//rM4[0] = p[3];  rM4[1] =-p[2];  rM4[2] = p[1];  rM4[3] = p[0];
		//rM4[4] = p[2];  rM4[5] = p[3];  rM4[6] =-p[0];  rM4[7] = p[1];
		//rM4[8] =-p[1];  rM4[9] = p[0];  rM4[10]= p[3];  rM4[11]= p[2];
		//rM4[12]=-p[0];  rM4[13]=-p[1];  rM4[14]=-p[2];  rM4[15]= p[3];
	//}
//}
//#endif

/*********************************************************************
功能：转换矩阵Aoi计算 由惯性坐标系到卫星轨道坐标系转换矩阵
输入：uR、uV——位置、速度矢量
输出：Aoi[3]
注意：
*********************************************************************/
void CalTransMatrix_IO( TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL * cAoi )
{
	TYPE_CAL  uvec[3],vec[3],rvec[3];

	uvec[0] = -cuR[0];              
	uvec[1] = -cuR[1];
	uvec[2] = -cuR[2];
	unitary(&uvec[0], 3);
	memcpy( cAoi+6, &uvec[0], 3 * sizeof(TYPE_CAL) );   

	cross( cuV, cuR , &vec[0] );    /*叉乘顺序反向，因此已经取反了*/
	unitary( &vec[0], 3 );
	memcpy( cAoi+3, &vec[0], 3 * sizeof(TYPE_CAL) );   

	cross( &vec[0], &uvec[0] , &rvec[0]);
	memcpy( cAoi, &rvec[0], 3 * sizeof(TYPE_CAL) );  
}

/********************************************************************
 功能：计算经纬度
 输入：cuR :当前位置
 输出：alf:经度
	   dlt：纬度
*********************************************************************/
//#ifndef MINMODULE_TYPE
//BOOL 	CalEarthLonLat(TYPE_CAL * cuR, TYPE_CAL * alf, TYPE_CAL * dlt)
//{
	//TYPE_CAL    tmpNor, tmpUse;
	//BOOL    result = TRUE;
	///*计算经纬度*/
	//CalNormal(cuR, 3, &tmpNor);
//
	///*检查有效性*/
	//if((0 == CalPFlag(cuR[0])) || (0 == CalPFlag(tmpNor)))
		//return FALSE;
//
	//*alf = POSE_ATAN2F(cuR[1] , cuR[0]);
//
	//tmpUse = cuR[2] / tmpNor;
	//if(POSE_ABSF(tmpUse) > 1.0F)
	//{
		//if(1 == CalPFlag(tmpUse))
		//{
			//tmpUse = 1.0F;
		//}
		//else if(-1 == CalPFlag(tmpUse))
		//{
			//tmpUse = -1.0F;
		//}
	//}
//
	//*dlt = POSE_ASINF(tmpUse);
	//return result;
//}
//#endif

/********************************************************************
功能：转换矩阵Aie计算 惯性坐标系到地理坐标系的转换矩阵 
输入：cuR 当前位置
输出：Aie[3] ，转换矩阵
注意：
*********************************************************************/
//#ifndef MINMODULE_TYPE
//void 	CalTransMatrix_IG( TYPE_CAL * cuR, TYPE_CAL *cAie )
//{
	////TYPE_CAL sla, sra;
	//TYPE_CAL sl,cl, sr, cr;
	//TYPE_CAL    tmpAlf, tmpDlt; /*赤道经纬度*/
//
	///*计算经纬度*/
	//if(!CalEarthLonLat(cuR, &tmpAlf, &tmpDlt))
	//{
		//return;
	//}
//
	//sl = POSE_SINF( tmpDlt );
	//cl = POSE_COSF( tmpDlt );
	//sr = POSE_SINF( tmpAlf );
	//cr = POSE_COSF( tmpAlf );
//
	//cAie[0] =-sl*cr;    /* Aie[3][3] */
	//cAie[1] =-sl*sr;
	//cAie[2] = cl;
	//cAie[3] =-sr;
	//cAie[4] = cr;
	//cAie[5] = 0.0;
	//cAie[6] =-cl*cr;
	//cAie[7] =-cl*sr;
	//cAie[8] =-sl;
//}
//#endif
///********************************************************************
//功能：得到轨道系相对于惯性系的角速率
//输入：Omg0:卫星平均轨道角速
//输出：Omgoi, 角速率矢量
//*********************************************************************/
//#ifndef MINMODULE_TYPE
//void CalArcSpdOI(TYPE_CAL Omg0, TYPE_CAL * Omgoi)
//{
	//Omgoi[0] = 0.0;
	//Omgoi[1] = -Omg0;
	//Omgoi[2] = 0.0;
//}
//#endif

/********************************************************************
功能：四元数相乘
输入：Qb1：Qb2：输入4元数，4维向量
输出：Qres输出四元数
*********************************************************************/
//#ifndef MINMODULE_TYPE
//void 	CalQPlusQ(TYPE_CAL * Qb1, TYPE_CAL * Qb2, TYPE_CAL * Qres)
//{
	//Qres[0] = Qb1[3] * Qb2[0] + Qb1[0] * Qb2[3] + Qb1[1] * Qb2[2] - Qb1[2] * Qb2[1];
	//Qres[1] = Qb1[3] * Qb2[1] + Qb1[1] * Qb2[3] + Qb1[2] * Qb2[0] - Qb1[0] * Qb2[2];
	//Qres[2] = Qb1[3] * Qb2[2] + Qb1[2] * Qb2[3] + Qb1[0] * Qb2[1] - Qb1[1] * Qb2[0];
	//Qres[3] = Qb1[3] * Qb2[3] - Qb1[0] * Qb2[0] - Qb1[1] * Qb2[1] - Qb1[2] * Qb2[2];
//}
//#endif

/********************************************************************
功能：四元数求逆
输入：Qb1：输入4元数，4维向量
输出：Qres输出四元数
*********************************************************************/
void 	CalQuatInv(TYPE_CAL *Qb1, TYPE_CAL *Qres)
{
	Qres[0] = -Qb1[0];
	Qres[1] = -Qb1[1];
	Qres[2] = -Qb1[2];
	Qres[3] =  Qb1[3];
}

/************************ZMXADD****************************************/

/********************************************************************
功能：CRC余式表
*********************************************************************/
const UINT8 CRC_TABLE_8[256]={ 
	0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
	0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
	0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
	0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
	0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
	0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
	0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
	0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
	0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
	0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
	0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
	0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
	0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
	0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
	0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
	0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3
};

/****************************************************************
说明：获取CRC校验
返回：crc校验值
注意：
****************************************************************/
UINT8 DoGetCrcCheckU8(UINT8 * BeginAddr,UINT8 CheckLength)
{
	UINT8	result;
	UINT8   i;

	result = 0xFF;
	for(i = 0; i < CheckLength; ++i)	/* culculate the CRC */
	{
		result = CRC_TABLE_8[(UINT8)(result ^ BeginAddr[i])];
	}
	return result;
}

/****************************************************
 功能：从24位大端数据获取有符号整数
 输入：无符号8位
 返回：返回32位有符号整数
*****************************************************/
float PoseGetInt32From24(UINT8* Ldata)
{
	UINT32 L24,tmpL24;
	float tmp_f =0.0f;
	L24 = ((*(Ldata+0))*256+(*(Ldata+1)))*256+(*(Ldata+2));
	if( (*(Ldata+0) & 0x80) != 0x0)
	{		
		tmpL24 = ~L24 ;
		L24 = (tmpL24 &0xffffff)+1;
		tmp_f = ((float)L24)*(-1.0f);
	}
	else
	{
		tmp_f = (float)L24;
	}
	return(tmp_f);
}


/********************************************************************
功能：得到轨道系相对于惯性系的角速率
输入：cuR位置，cuV速度
输出：Woi轨道系相对于惯性系的角速度     GMEARTH
*********************************************************************/
void AglSpdOofICal(TYPE_CAL *cuR, TYPE_CAL *cuV, TYPE_CAL *Woi)
{
	UINT8 i;
	TYPE_CAL RR;
	TYPE_CAL tmpw[3];
	TYPE_CAL tmpwo;
	TYPE_CAL tmpRXV[3];
	CalNormal(cuR, 3, &RR);
	RR=RR*RR;
	cross(cuR,cuV,tmpRXV);
	if (RR > POSE_ZERO)
	{
		for(i=0;i<3;i++)
		{
			tmpw[i] =tmpRXV[i]/RR;
		}
		CalNormal(tmpw, 3, &tmpwo);
	}
	else
	{
		Woi[1]=-0.00096466;                 //zmx20231010
		return;
	}
	memset(Woi,0,3*sizeof(TYPE_CAL));
	Woi[1]=-tmpwo;
}

/****************************************************
功能:将弧度转化为 0----2PI
输入：源弧度值
*****************************************************/
//TYPE_CAL CalTo2PAI(TYPE_CAL  inData)
//{
//UINT32 	tmpCt;
//TYPE_CAL result;
//
//if(inData > POSE_ZERO)
//{
//tmpCt = inData / POSE_2PI;
//result = inData - tmpCt * POSE_2PI;
//}
//else
//{
//tmpCt = (-inData) / POSE_2PI;
//result = inData + POSE_2PI * (tmpCt + 1);
//}
//if(result < 0.0)
//result = 0.0;
//if(result > POSE_2PI)
//result = POSE_2PI;
//
//return result;
//}

/******************************************************************
*函数名： AttLongKutaCal
*输    入: 上一步角速率,上一步姿态四元数
*输    出: 姿态四元数
*说    明: 龙格-库塔定步长积分计算
*******************************************************************/
void AttLongKutaCal(TYPE_CAL * pWbxl, TYPE_CAL * pQbxl, TYPE_CAL * pQbx)
{
	TYPE_CAL	tmpK[4][4], tmpOmg[16];
	TYPE_CAL	tmpUse[4];
	TYPE_CAL	tmpDT;
	UINT32 i;

	tmpOmg[0] = 0.0;
	tmpOmg[1] = pWbxl[2];
	tmpOmg[2] = -pWbxl[1];
	tmpOmg[3] = pWbxl[0];
	tmpOmg[4] = -pWbxl[2];
	tmpOmg[5] = 0.0;
	tmpOmg[6] = pWbxl[0];
	tmpOmg[7] = pWbxl[1];
	tmpOmg[8] = pWbxl[1];
	tmpOmg[9] = -pWbxl[0];
	tmpOmg[10] = 0.0;
	tmpOmg[11] = pWbxl[2];
	tmpOmg[12] = -pWbxl[0];
	tmpOmg[13] = -pWbxl[1];
	tmpOmg[14] = -pWbxl[2];
	tmpOmg[15] = 0.0;

	//K1
	MatrixPlusVectorN(tmpOmg, pQbxl, tmpK[0], 4, 4);
	for (i = 0; i < 4; ++i)
	{
		tmpK[0][i] /= 2.0;
	}
	//K2
	for (i = 0; i < 4; ++i)
	{
		tmpUse[i] = pQbxl[i] + 0.5 * tmpK[0][i];
	}
	MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[1], 4, 4);
	for (i = 0; i < 4; ++i)
	{
		tmpK[1][i] /= 2.0;
	}
	//K3
	for (i = 0; i < 4; ++i)
	{
		tmpUse[i] = pQbxl[i] + 0.5 * tmpK[1][i];
	}
	MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[2], 4, 4);
	for (i = 0; i < 4; ++i)
	{
		tmpK[2][i] /= 2.0;
	}

	//K4
	for (i = 0; i < 4; ++i)
	{
		tmpUse[i] = pQbxl[i] + tmpK[2][i];
	}
	MatrixPlusVectorN(tmpOmg, tmpUse, tmpK[3], 4, 4);
	for (i = 0; i < 4; ++i)
	{
		tmpK[3][i] /= 2.0;
	}
	//计算结果
	tmpDT = ATTPERIOD_S;
	for (i = 0; i < 4; ++i)
	{
		pQbx[i] = pQbxl[i] +
		tmpDT * (tmpK[0][i] + 2.0 * tmpK[1][i] + 2.0 * tmpK[2][i] + tmpK[3][i]) / 6.0;
	}
}

/***************************************************
功能：3-1-2欧拉角转四元数
输入：欧拉角
输出：四元数（矢量在前，标量在后）
***************************************************/
void Eul312A2Quat( TYPE_CAL *ca, TYPE_CAL *cq )
{
	cq[0] = POSE_SINF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
	-POSE_COSF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
	cq[1] = POSE_COSF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
	+POSE_SINF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
	cq[2] = POSE_SINF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
	+POSE_COSF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
	cq[3] = POSE_COSF(ca[0]/2.0)*POSE_COSF(ca[1]/2.0)*POSE_COSF(ca[2]/2.0)
	-POSE_SINF(ca[0]/2.0)*POSE_SINF(ca[1]/2.0)*POSE_SINF(ca[2]/2.0);
}

/***************************************************
 功能：姿态矩阵至姿态角
 输入：M:姿态角矩阵
 输出：ca：姿态角
***************************************************/
void Matrix2Eul312A( TYPE_CAL *M , TYPE_CAL *ca )
{
	if(POSE_ABSF(M[5]) > 1.0F)
	{
		if(1 == CalPFlag(M[5]))
		{
			M[5] = 1.0F;
		}
		else if(-1 == CalPFlag(M[5]))
		{
			M[5] = -1.0F;
		}
	}

	ca[0] = POSE_ASINF( M[5] );

	if(POSE_ABSF(POSE_ABSF(ca[0]) - POSE_05PI) < POSE_ZERO)
	{
		ca[1] = 0.0;
		ca[2] = 0.0;
		return;
	}

	ca[1] = POSE_ATAN2F( -M[2], M[8] );
	ca[2] = POSE_ATAN2F( -M[3], M[4] );
}

void DeviAngleLimit(TYPE_CAL * Ang)
{
	if(POSE_ABSF(*Ang)>(270.0*ANG2RADIAN))
	{
		if(*Ang>0) *Ang-=360.0*ANG2RADIAN;
		else *Ang+=360.0*ANG2RADIAN;
	}
}

/********************************
功能：计算二进制数据中取1的位数
输入：n为输入数据
     eff_bit为有效位数（只从低位算）
返回：
*******************************************************/
UINT8 count_onebit(UINT8 n,UINT8 eff_bit)
{
	UINT8 count = 0;
	UINT8 i = 0;
	for(i = 0; i < eff_bit; i++)
	{
		if (((n >> i) & 1) == 1)
		{
			count++;
		}
	}
	return count;
}


///**********************************
 //* \fn uint32_t get_uint32from8_be(const uint8_t *psrc_data)
 //* \brief 从任意地址获取32bit数据(大端)；由于某些架构访问存储有对齐要求，所以不能直接将8位指针强制转换为32位
 //* \param psrc_data 需要处理的数据
 //* \return 32位无符号整型数据
 ////*********************************/
//UINT32 get_uint32from8_be(const UINT8 *psrc_data)
//{
	//UINT32 result;
//
	//result = *psrc_data;
	//result = result << 8;
	//result |= *(psrc_data + 1);
	//result = result << 8;
	//result |= *(psrc_data + 2);
	//result = result << 8;
	//result |= *(psrc_data + 3);
//
	//return result;
//}
//
/////********************************
 ////* 功能说明：从四字节大端数据pData，转换成float类型数据，并检查float类型是否合法；只有有效的float类型才返回TRUE,不合法返回FALSE。
 ////* 输入说明：
////*******************************************************/
//BOOL get_floatto8_be(UINT8 * pData, TYPE_CAL * Data)
//{
	//UNN un;
	//UINT32 tDenCheck = get_uint32from8_be(pData);
//
	////un.result[3] = *pData & 0xff;
	////un.result[2] = (*(pData + 1) & 0xff);
	////un.result[1] = (*(pData + 2) & 0xff);
	////un.result[0] = (*(pData + 3) & 0xff);
	//
	//un.result[0] = *pData & 0xff;
	//un.result[1] = (*(pData + 1) & 0xff);
	//un.result[2] = (*(pData + 2) & 0xff);
	//un.result[3] = (*(pData + 3) & 0xff);	
    //memcpy(&un.Lf, &un.result, 4*sizeof(UINT8));
///* 需增加对非法数据NAN INF等的判断 */
	//if( !FLOAT_NAN_CHECK(un.Lf) || !FLOAT_INF_CHECK(un.Lf) 	/* 当为NAN时，结果为TRUE */
			//|| ( ( (tDenCheck&0x7f800000) == 0) && ( (tDenCheck&0x807fffff) != 0) ) /* Denormal float check */
			//)
	//{ return FALSE; }
	//else
	//{
		//if ( ( 0 < un.Lf ) && (un.Lf < 1.0E-15F ) )
		//{
			//un.Lf = 1.0E-15F;
		//}
		//else if (( 0 > un.Lf ) && (un.Lf > -1.0E-15F ))
		//{
			//un.Lf = -1.0E-15F;
		//}
		//else
		//{
			//;
		//}
		//*Data = un.Lf;
		//return TRUE;
	//}
//}