1142 lines
27 KiB
C
1142 lines
27 KiB
C
/*
|
||
* 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;
|
||
//}
|
||
//}
|