/*
 * Created: 2022/11/4 11:02:40
 *  Author:  wangzk zhengmengxing
 */ 
//#include	"..\PrjCommon\CommonDef.h"
//#include	"..\PrjCommon\DataStorDefine.h"
#include    "AttCtrlMain.h"
#include    "AttMath.h"
/*****************************************************************/

/*初始化函数*/
void	ZKEnvInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttEnvPara_t *pEnv =NULL;	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pEnv = &tmpAtt->sEnvPara;
	
	pEnv->BetaAngCntIn	= 0;
	pEnv->OrbAttRate_Wo = 9.6466E-4f;		
	//地影判断
	pEnv->BetaAng=0.0f;							//beta角
	pEnv->BetaShadow=0x55;						//卫星进出影设置，0x55阴影区0阳照区
	pEnv->BetaAngCntIn=0;						//卫星进影判断计数  初始值为0
	pEnv->BetaAngCntOut=0;						//卫星出影判断计数   初始值为0
	tmpConst->BetaAngLimit=57.5f*ANG2RADIAN;		//卫星进出影判断值，初始值为57.5，（可注数修改）
	pEnv->SatTime_s=0; 							//星上时
	pEnv->SatTime_ms=0;
}

/*初始化函数*/
void	ZKEnvCycInit(void)
{
	UINT8 i =0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttEnvPara_t *pEnv =NULL;
	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	pEnv = &tmpAtt->sEnvPara;
	
#ifdef  MINMODULE_TYPE
//太阳矢量
	for (i = 0; i < 3 ; ++i)
	{
		pEnv->SunVecI[i]=0.0f;						//惯性系太阳矢量XYZ（单位矢量）
		pEnv->SunPosVecI[i]=0.0f;					//惯性系太阳位置矢量XYZ
	}
//pEnv->bInOutShadow =0x00;
#else
	//太阳矢量
	for (i = 0; i < 3 ; ++i)
	{
		pEnv->SunVecI[i]=0.0f;						//惯性系太阳矢量XYZ（单位矢量）
		pEnv->SunPosVecI[i]=0.0f;					//惯性系太阳位置矢量XYZ
	}
	pEnv->bInOutShadow =0x00;

	//磁场
	for (i = 0; i < 3 ; ++i)
	{
		pEnv->fvBe[i]=0.0f;
		pEnv->fvBi[i]=0.0f;
		pEnv->fvBo[i]=0.0f;
		pEnv->fvBb[i]=0.0f;
		pEnv->fLonLat[i]=0.0f;
	}
	pEnv->fvIn_Valid=POSE_OK;
#endif
}


/***********************************************
*说    明: 计算儒略日ZYT
***********************************************/
void MJD_CAL_ATT(UINT32 sat_time, TYPE_CAL *pMjd)
{
	*pMjd = (TYPE_CAL)sat_time/86400.0F + 7304.5F;
}

/***********************************************
*说    明: 计算儒略世纪
***********************************************/
void MJC_CAL_ATT(TYPE_CAL mjd, TYPE_CAL *pMjc,   UINT8 *MJCLv)
{
	*pMjc = mjd / 36525.0F;
	*MJCLv = POSE_NO;
	//if (*pMjc >= 0.024)
	if (*pMjc > 0.24)
	{
		*MJCLv = POSE_OK;
	}
}

/******************************************************************
*说    明: 计算惯性系太阳位置矢量
*输    入: 儒略世纪
*输    出: 惯性系太阳矢量，惯性系太阳位置矢量
*******************************************************************/
void ObtSunVecI(TYPE_CAL JulC, TYPE_CAL *pSunVI, TYPE_CAL *pSunPVI)
{
	TYPE_CAL tmpM =0.0f, tmpL =0.0f;
	TYPE_CAL tmpeps =0.0f;//平黄赤交角
	TYPE_CAL tmpZodR =0.0f;

	if ((NULL == pSunVI) || (NULL == pSunPVI))
	{return;}

	/*计算平近点角*/
	//tmpM = 6.2400599667 + 628.3019551515 * JulC;
	tmpM = 6.2401f + 628.30196f * JulC;
	//平黄赤交角
	//tmpeps = 0.4090928042 +0.0002269655* JulC;
	tmpeps = 0.4090928f;    
	/*计算平春分点的几何平黄经*/
	//tmpL = 4.8950629939 + 628.3319667861 * JulC + 0.0334160739 * POSE_SINF(tmpM);
	tmpL = 4.8951f + 628.3076f * JulC + 0.03342f * POSE_SINF(tmpM);
	/*黄道半径（m）*/
	tmpZodR = (1.49619E11) - (2.499E9)*POSE_COSF(tmpM) - (2.1E7)*POSE_COSF(2.0f * tmpM);
	
    //惯性系太阳矢量
    pSunVI[0] = POSE_COSF(tmpL);
    pSunVI[1] = POSE_SINF(tmpL) * POSE_COSF(tmpeps);
    pSunVI[2] = POSE_SINF(tmpL) * POSE_SINF(tmpeps);

    pSunPVI[0] = tmpZodR * pSunVI[0];
    pSunPVI[1] = tmpZodR * pSunVI[1];
    pSunPVI[2] = tmpZodR * pSunVI[2];
}


/******************************************************************
*说    明: 计算轨道系轨道角速度
*输    入: J2000卫星位置，J2000卫星速度
*输    出: 轨道系轨道角速度
*******************************************************************/
void Cal_OrbAttRate_Wo(void)
{
	TYPE_CAL tmpWoi[3] ={0.0f};	
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttOrbitGetPara_t *pOrbit =NULL;
	sAttEnvPara_t *pEnv =NULL;
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL ==tmpAtt)
	{return;}
    
	pOrbit = &tmpAtt->sOrbitPara;
	pEnv = &tmpAtt->sEnvPara;
	
	if(0 == pOrbit->OrbitVld)
	{
		return;
	}
	AglSpdOofICal(pOrbit->JPos, pOrbit->JVel, tmpWoi);
	pEnv->OrbAttRate_Wo = tmpWoi[1];
}

/******************************************************************
*说    明: 卫星光照阴影设置
*******************************************************************/

void ShadowFlgCal(void)
{	
	TYPE_CAL tmpV=0.0f;
	TYPE_CAL tmpR=0.0f;
	TYPE_CAL tmpa = 0.0f;
	TYPE_CAL tmpLamd0 = 0.0f;
	TYPE_CAL tmpLamd = 0.0f;
	TYPE_CAL tmpRXV = 0.0f;
	//TYPE_CAL tmpSunVeco[3];
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttOrbitGetPara_t *pOrbit =NULL;
	sAttEnvPara_t *pEnv =NULL;
	sAttDeterPara_t *pDeter =NULL;
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	
	pOrbit = &tmpAtt->sOrbitPara;
	pEnv = &tmpAtt->sEnvPara;
	pDeter=&tmpAtt->sDeterPara;

	if(0 == pOrbit->OrbitVld)
	{
		pEnv->bInOutShadow =0x00;
		return;
	}
	
	CalNormal(pOrbit->JPos, 3, &tmpR);
	CalNormal(pOrbit->JVel, 3, &tmpV);
	tmpRXV = 2.0f - tmpR * tmpV *tmpV/GMEARTH;
	
	if (POSE_ABSF(tmpRXV) > POSE_ZERO)
	{
		tmpa = tmpR / tmpRXV;
		if (POSE_ABSF(tmpa) > POSE_ZERO)
		{
			tmpLamd0 = POSE_ASINF(EarthRad/tmpa);
		}
		else
		{
			return;
		}
	}
	else
	{
		return;
	}
	unitary(pDeter->SunVeco, 3);
	
	tmpLamd =POSE_ACOSF(pDeter->SunVeco[2]);

	if (tmpLamd < tmpLamd0)
	{
		pEnv->bInOutShadow =0x55;
	}
	else
	{
		pEnv->bInOutShadow =0x00;
	}
}

/******************************************************************
*说    明: 卫星进出影设置
*输    入: J2000卫星位置
*输    出: 卫星地影区标识
*******************************************************************/
void BetaShadowCal(void)
{
	UINT8 i =0;
	TYPE_CAL tmpVecN[3] ={0.0f,0.0f,0.0f};
	TYPE_CAL tmpVecNMod =0.0f;
	TYPE_CAL tmpRXV[3] ={0.0f,0.0f,0.0f};
	TYPE_CAL tmpV=0.0f;
	TYPE_CAL tmpR=0.0f;
	TYPE_CAL tmpNS = 0.0f;
	TYPE_CAL tmpSunVecI = 0.0f;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttOrbitGetPara_t *pOrbit =NULL;
	sAttEnvPara_t *pEnv =NULL;
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pOrbit = &tmpAtt->sOrbitPara;
	pEnv = &tmpAtt->sEnvPara;
		
	if(0 == pOrbit->OrbitVld)
	{
		return;
	}
	//Beta角计算
	CalNormal(pOrbit->JPos, 3, &tmpR);
	CalNormal(pOrbit->JVel, 3, &tmpV);
	cross(pOrbit->JPos,pOrbit->JVel,tmpRXV);
	
	if(POSE_ABSF(tmpR*tmpV) < POSE_ZERO)
	{		return;}	
	for(i=0;i<3;i++)
	{
		tmpVecN[i] =tmpRXV[i]/(tmpR*tmpV);
	}
	
	Posedot(tmpVecN, pEnv->SunVecI, 3, &tmpNS);
	CalNormal(tmpVecN, 3, &tmpVecNMod);
	CalNormal(pEnv->SunVecI, 3, &tmpSunVecI);
	if(POSE_ABSF(tmpVecNMod*tmpSunVecI) < POSE_ZERO)
	{		return;}
	pEnv->BetaAng = POSE_05PI - POSE_ACOSF(tmpNS/(tmpVecNMod*tmpSunVecI));
	
	//卫星进出影设置
	if (POSE_ABSF(pEnv->BetaAng) < tmpConst->BetaAngLimit)
	{
		pEnv->BetaAngCntOut = 0;
		if(pEnv->BetaAngCntIn<(60*2))
		{
			pEnv->BetaAngCntIn++;
		}
		else
		{
			pEnv->BetaShadow =0x55;
		}
	}
	
	if (POSE_ABSF(pEnv->BetaAng) > tmpConst->BetaAngLimit)
	{
		pEnv->BetaAngCntIn = 0;
		if(pEnv->BetaAngCntOut<(60*2))
		{
			pEnv->BetaAngCntOut++;
		}
		else
		{
			pEnv->BetaShadow =0x00;
		}
	}
}


/******************************************************************
*说    明: 地磁场矢量计算公式
*输    入: J2000卫星位置、
*输    出: 惯性系地磁矢量
 -----------------------------------------------------------------------------
 计算给定点空间磁场。
 输入  r  ——到地心的距离（m）
 Lon——地心经度（弧度 -pi～pi）
 Lat——地心纬度（弧度 -pi/2～pi/2）
 输出B={X Y Z} 依次为北、东、地三个方向的空间磁场,单位：nT
 ------------------------------------------------------------------------------
*******************************************************************/
void MagMath(TYPE_CAL *rLonLat, TYPE_CAL cgst, TYPE_CAL *B)
{
	//static TYPE_CAL   g2012[104] = {-29399.1,	-1443.5, -2510.6, 2975, 1674.9, 1365.4, -2387.1, 1239.3, 513.7, 901.8, 807.9, 80.4, -304.2, 42.9, -234.6, 363.7, 187.2, -140.5, -149.9, 14.4, 65.5, 65.2, 73.3, -120.2, -37.6, 13.5, -63.8, 80.5, -76.9, -8.2, 57.2, 15.9, 5.9, -8, 10.6, 23.7, 9.8, -17.7, -0.1, -21.2, 15.7, 14, -16.6, -0.1,	5, 8.4, 2.9, -1.5, -1.1, -13.2, -1.1, 8.8, -9.3, -11.9, -1.9, -6.2, -0.1, 1.7, -0.9, 0.7, -0.9, 1.9, 1.4, -2.4,	-3.8};
	//static TYPE_CAL   h2012[104] = {0, 4626.6, 0, -3021.8, -757, 0, -76.1, 240.8, -542.9, 0, 281.8, -151.9, 203.3, -354.7, 0, 47.7, 210.8, -121.8, 35.3, 99.2, 0, -19.1, 23.5, 51.5, -63.7, 8.9, 69.1, 0, -50.9, -16.3, 1.4, 23.3, -3.3, -5.1, -6.3, 7.8, 0.4, -1.4, 9.6, 0, 3.4, -0.2, 3.6, 4.8, -8.6, -0.1, -4.3, -3.4, -0.1, -8.8, 0, 0, 2.5, -0.6, -0.4, 0.6, -0.2, -1.7, -1.6, -3, -2, -2.6, 0, -1.2, 0.5, 1.4, -1.8, 0.1, 0.8, 0.2, 0.6, 0.2, -0.9, 0, 0.5, 0, -0.9, 0.6, 1.4, -0.4, -1.3, -0.1, 0.3, -0.1, 0.5, 0.5, -0.4, -0.4, -0.6};
	static TYPE_CAL   g2020[65] = {-29404.8f,-1450.9f, -2499.6f, 2982.0f, 1677.0f, 1363.2f, -2381.2f, 1236.2f, 525.7f, 903.0f, 809.5f, 86.3f, -309.4f, 48.0f, -234.3f, 363.2f, 187.8f, -140.7f, -151.2f, 13.5f, 66.0f, 65.5f, 72.9f, -121.5f, -36.2f, 13.5f, -64.7f, 80.6f, -76.7f, -8.2f, 56.5f, 15.8f, 6.4f, -7.2f, 9.8f, 23.7f, 9.7f, -17.6f, -0.5f, -21.1f, 15.3f, 13.7f, -16.5f, -0.3f, 5.0f, 8.4f, 2.9f, -1.5f, -1.1f, -13.2f, -1.1f, 8.8f, -9.3f, -11.9f, -1.9f, -6.2f, -0.1f, 1.7f, -0.9f, 0.7f, -0.9f, 1.9f, 1.4f, -2.4f, -3.8f};
	static TYPE_CAL   h2020[65] = {0.0f, 4652.5f, 0.0f, -2991.6f, -734.6f, 0.0f, -82.1f, 241.9f, -543.4f, 0.0f, 281.9f, -158.4f, 199.7f, -349.7f, 0.0f, 47.7f, 208.3f, -121.2f, 32.3f, 98.9f, 0.0f, -19.1f, 25.1f, 52.8f, -64.5f, 8.9f, 68.1f, 0.0f, -51.5f, -16.9f, 2.2f, 23.5f, -2.2f, -27.2f, -1.8f, 0.0f, 8.4f, -15.3f, 12.8f, -11.7f, 14.9f, 3.6f, -6.9f, 2.8f, 0.0f, -23.4f, 11.0f, 9.8f, -5.1f, -6.3f, 7.8f, 0.4f, -1.4f, 9.6f, 0.0f, 3.4f, -0.2f, 3.6f, 4.8f, -8.6f, -0.1f, -4.3f, -3.4f, -0.1f, -8.8f};

	UINT8   nMax = 10;
	TYPE_CAL   r =0.0f,Lon =0.0f,Lat =0.0f;
	
	TYPE_CAL   theta =0.0f,Sintp[10] ={0.0f},Costp[10] ={0.0f};
	TYPE_CAL   P[65] ={0.0f},dP[65] ={0.0f};
	TYPE_CAL   Psin_mLon[11] ={0.0f},Pcos_mLon[11] ={0.0f};
	TYPE_CAL   kk =0.0f;
	
	TYPE_CAL   X =0.0f,Y =0.0f,Z =0.0f;
	TYPE_CAL   B_north =0.0f,B_east =0.0f,B_earth =0.0f;
	TYPE_CAL   temp1 =0.0f,temp2 =0.0f;
	
	UINT8   i =0,j=0;
	UINT8   num = 0;
	if(NULL==rLonLat)
	{return;}		
	r   = rLonLat[2];
	Lon = rLonLat[0] - cgst;
	
	if (Lon > POSE_PI)
	{
		Lon = Lon - POSE_2PI;
	}
	else if (Lon < -POSE_PI)
	{
		Lon = Lon + POSE_2PI;
	}
	else
	{
		;
	}
	Lat = rLonLat[1];
	theta = POSE_PI/2 - Lat;
	Sintp[0]  = POSE_SINF(theta) + 1e-8*(sin(theta<1e-8));
	Costp[0]  = POSE_COSF(theta);
	for (i=1; i<10; i++)
	{
		Sintp[i] = POSE_POW( Sintp[0],(i+1));
		Costp[i] = POSE_POW( Costp[0],(i+1));
	}

	// 计算让德拉函数
	P[0]  = Costp[0];
	P[1]  = Sintp[0];
	P[2]  = 1.5*Costp[1] - 0.5;
	P[3]  = 1.7320508075688772935*Costp[0]*Sintp[0];
	P[4]  = 0.86602540378443864677 - 0.86602540378443864677*Costp[1];
	P[5]  = 2.5*Costp[2] - 1.5*Costp[0];
	P[6]  = Sintp[0] * (3.061862178478972622775*Costp[1] - 0.612372435695794524555);
	P[7]  = 1.9364916731037084426*Sintp[1]*Costp[0];
	P[8]  = 0.79056941504209483300*Sintp[2];
	P[9]  = 4.375*Costp[3] - 3.75*Costp[1] + 0.375;
	P[10] = Sintp[0]*Costp[0]*(5.533985905294663831*Costp[1]-2.371708245126284499);
	P[11] = Sintp[1]*(3.9131189606246319686475*Costp[1]-0.55901699437494742409250000000000);
	P[12] = 2.0916500663351888700*Sintp[2]*Costp[0];
	P[13] = 0.73950997288745200533*Sintp[3];
	P[14] = 7.875*Costp[4]-8.75*Costp[2]+1.875*Costp[0];
	P[15] = Sintp[0]*(10.16658128379446932365*Costp[3]-6.7777208558629795491*Costp[1]+0.48412291827592711065);
	P[16] = (7.6852130744696987873025*Costp[2]-2.5617376914898995957675*Costp[0])*Sintp[1];
	P[17] = Sintp[2]*(4.7062126492541749574055*Costp[1]-0.5229125165837972174895);
	P[18] = 2.2185299186623561344*Sintp[3]*Costp[0];
	P[19] = 0.70156076002011401131*Sintp[4];
	P[20] = 14.4375*Costp[5]-19.6875*Costp[3]+6.5625*Costp[1]-0.3125;
	P[21] = Sintp[0]*Costp[0]*(18.90312474169284002751375*Costp[3]-17.1846588560844000250125*Costp[1]+2.86410980934740000416875);
	P[22] = Sintp[1]*(14.944232269507858529664375000000*Costp[3]-8.1513994197315591979987500000000*Costp[1]+0.452855523318419955444375);
	P[23]=	Sintp[2]*Costp[0]*(9.9628215130052390196607500000000*Costp[1]-2.71713313991051973263475);
	P[24]=	Sintp[3]*(5.4568620790707178652805000000000*Costp[1]-0.49607837082461071502550000000000);
	P[25]=	2.3268138086232857013*Sintp[4]*Costp[0];
	P[26]=	0.67169328938139610979*Sintp[5];
	P[27]=	26.8125000000000000000*Costp[6]-43.3125000000000000000*Costp[4]+19.6875000000000000000*Costp[2]-2.1875000000000000000*Costp[0];
	P[28]=	Sintp[0]*(35.469603513959667604426875000000*Costp[5]-40.926465593030385697415625000000*Costp[3]+11.161763343553741553840625000000*Costp[1]-0.41339864235384227977187500000000);
	P[29]=	(28.960809996010125407674500000000*Costp[4]-22.277546150777019544365000000000*Costp[2]+3.0378472023786844833225000000000*Costp[0])*Sintp[1];
	P[30]=	Sintp[2]*(20.478385136833910029618875000000*Costp[3]-9.4515623708464200136702500000000*Costp[1]+0.42961647140211000062137500000000);
	P[31]=	(12.348930874776164558539650000000*Costp[2]-2.8497532787944995135091500000000*Costp[0])*Sintp[3];
	P[32]=	Sintp[4]*(6.1744654373880822792923475000000*Costp[1]-0.47495887979908325225325750000000);
	P[33]=	2.4218245962496950923*Sintp[5]*Costp[0];
	P[34]=  0.64725984928774932488*Sintp[6];
	P[35]=  50.27343750000000000000*Costp[7]-93.84375000000000000000*Costp[5]+54.14062500000000000000*Costp[3]-9.84375000000000000000*Costp[1]+0.27343750000000000000;
	P[36]=  Sintp[0]*Costp[0]*(67.031250000000000001340625000000*Costp[5]-93.843750000000000001876875000000*Costp[3]+36.093750000000000000721875000000*Costp[1]-3.2812500000000000000656250000000);
	P[37]=	Sintp[1]*(56.082367403612251576875000000000*Costp[5]-56.082367403612251576875000000000*Costp[3]+12.942084785448981133125000000000*Costp[1]-0.39218438743784791312500000000000);
	P[38]=	Sintp[2]*Costp[0]*(41.419573328168170771135500000000*Costp[3]-27.613048885445447180757000000000*Costp[1]+3.1861210252437054439335000000000);
	P[39]=	Sintp[3]*(26.736219617835372314388468750000*Costp[3]-10.694487847134148925755387500000*Costp[1]+0.41132645565900572791366875000000);
	P[40]=	Sintp[4]*Costp[0]*(14.830586268334101760745250000000*Costp[1]-2.9661172536668203521490500000000);
	P[41]=	Sintp[5]*(6.8652274293172542798025200000000*Costp[1]-0.45768182862115028532016800000000);
	P[42]=	2.5068266169601757490*Sintp[6]*Costp[0];
	P[43]=	0.62670665424004393725*Sintp[7];
	P[44]=	94.9609375000000000000*Costp[8]-201.0937500000000000000*Costp[6]+140.7656250000000000000*Costp[4]-36.0937500000000000000*Costp[2]+2.4609375000000000000*Costp[0];
	P[45]=	Sintp[0]*(127.40346687426536137379062500000*Costp[7]-209.84100426349588932153750000000*Costp[5]+104.92050213174794466076875000000*Costp[3]-16.141615712576606870887500000000*Costp[1]+0.36685490255855924706562500000000);
	P[46]=	(108.65004161512663681809468750000*Costp[6]-134.21475728927408077529343750000*Costp[4]+44.738252429758026925097812500000*Costp[2]-3.4414040330583097634690625000000*Costp[0])*Sintp[1];
	P[47]=	Sintp[2]*(82.982839993569974752544250000000*Costp[5]-73.220152935502918899303750000000*Costp[3]+14.644030587100583779860750000000*Costp[1]-0.37548796377180984050925000000000);
	P[48]=	(56.375738371688976466782806250000*Costp[4]-33.162199042169986156931062500000*Costp[2]+3.3162199042169986156931062500000*Costp[0])*Sintp[3];
	P[49]=	Sintp[4]*(33.690947687096713796739206250000*Costp[3]-11.890922713092957810613837500000*Costp[1]+0.39636409043643192702046125000000);
	P[50]=	(17.397930574676114140811883750000*Costp[2]-3.0702230425899024954373912500000*Costp[0])*Sintp[5];
	P[51]=	Sintp[6]*(7.5335249254737569233685298750000*Costp[1]-0.44314852502786805431579587500000);
	P[52]=	2.5839777317091473341*Sintp[7]*Costp[0];
	//P[53]= .60904939217552372718*Sintp[8];
	P[53]=  0.60904939217552372718*Sintp[8];
	P[54]=  180.42578125000000000000*Costp[9]-427.32421875000000000000*Costp[7]+351.91406250000000000000*Costp[5]-117.30468750000000000000*Costp[3]+13.53515625000000000000*Costp[1]-0.24609375000000000000;
	P[55]=	Sintp[0]*Costp[0]*(243.28607380714600563907226562500*Costp[7]-460.96308721353980015824218750000*Costp[5]+284.71249504365693539185546875000*Costp[3]-63.269443343034874531523437500000*Costp[1]+3.6501601928673966075878906250000);
	P[56]=	Sintp[1]*(210.69192030396436232168027343750*Costp[7]-310.49335623742116552668671875000*Costp[5]+136.98236304592110243824414062500*Costp[3]-18.264315072789480325099218750000*Costp[1]+0.35123682832287462163652343750000);
	P[57]=	Sintp[2]*Costp[0]*(165.28034045942311008961040625000*Costp[5]-182.67827103409922694114834375000*Costp[3]+53.728903245323302041514218750000*Costp[1]-3.5819268830215534694342812500000);
	P[58]=	Sintp[3]*(116.87084953567937084175066562500*Costp[5]-92.266460159746871717171578125000*Costp[3]+16.282316498778859714794984375000*Costp[1]-0.36182925552841910477322187500000);
	P[59]=	Sintp[4]*Costp[0]*(73.915615322315771078115916875000*Costp[3]-38.902955432797774251639956250000*Costp[1]+3.4326137146586271398505843750000);
	P[60]=	Sintp[5]*(41.320085114855771085792242812500*Costp[3]-13.048447931007085606039655625000*Costp[1]+0.38377788032373781194234281250000);
	P[61]=	Sintp[6]*Costp[0]*(20.043185339772047739390229125000*Costp[1]-3.1647134747008496430616151250000);
	P[62]=	Sintp[7]*(8.1825961504122995301302910750000*Costp[1]-0.43066295528485787000685742500000);
	P[63]=	2.6547847521179797908*Sintp[8]*Costp[0];
	//P[64]=	.59362791713657325695*Sintp[9];
	P[64]=	0.59362791713657325695*Sintp[9];
	
	dP[0]=	-1*Sintp[0];
	dP[1]=	Costp[0];
	dP[2]=	-3*Costp[0]*Sintp[0];
	dP[3]=	-1.7320508075688772935*Sintp[1]+1.7320508075688772935*Costp[1];
	dP[4]=	1.7320508075688772935*Costp[0]*Sintp[0];
	dP[5]=	-Sintp[0]*(7.5000000000000000000*Costp[1]-1.5000000000000000000);
	dP[6]=	3.0618621784789726227750000000000*Costp[2]-0.61237243569579452455500000000000*Costp[0]-6.1237243569579452455*Sintp[1]*Costp[0];
	dP[7]=	Sintp[0]*(3.8729833462074168852*Costp[1]-1.9364916731037084426*Sintp[1]);
	dP[8]=	2.3717082451262844990*Sintp[1]*Costp[0];
	dP[9]=	-Sintp[0]*Costp[0]*(17.5000000000000000000*Costp[1]-7.5000000000000000000);
	dP[10]=	(-16.601957715883991493000000000000*Costp[1]+2.3717082451262844990000000000000)*Sintp[1]+5.5339859052946638310000000000000*Costp[3]-2.3717082451262844990000000000000*Costp[1];
	dP[11]=	Sintp[0]*Costp[0]*(7.8262379212492639374000000000000*Costp[1]-1.1180339887498948482000000000000-7.8262379212492639374*Sintp[1]);
	dP[12]=	6.2749501990055666099*Costp[1]*Sintp[1]-2.0916500663351888700*Sintp[3];
	dP[13]=	2.9580398915498080213*Sintp[2]*Costp[0];
	dP[14]=	-Sintp[0]*(39.3750000000000000000*Costp[3]-26.2500000000000000000*Costp[1]+1.8750000000000000000);
	dP[15]=	(-40.666325135177877294600000000000*Costp[2]+13.555441711725959098200000000000*Costp[0])*Sintp[1]+10.166581283794469323650000000000*Costp[4]-6.7777208558629795491000000000000*Costp[2]+0.48412291827592711065000000000000*Costp[0];
	dP[16]=	Sintp[0]*(15.370426148939397574447500000000*Costp[3]-5.1234753829797991914825000000000*Costp[1]-23.055639223409096361907500000000*Costp[1]*Sintp[1]+2.5617376914898995957675000000000*Sintp[1]);
	dP[17]=	-9.4124252985083499148*Sintp[3]*Costp[0]+(14.118637947762524872500000000000*Costp[2]-1.5687375497513916525000000000000*Costp[0])*Sintp[1];
	dP[18]=	Sintp[2]*(8.8741196746494245377*Costp[1]-2.2185299186623561344*Sintp[1]);
	dP[19]=	3.5078038001005700566*Sintp[3]*Costp[0];
	dP[20]=	-Sintp[0]*Costp[0]*(86.625000000000000000*Costp[3]-78.750000000000000000*Costp[1]+13.125000000000000000);
	dP[21]=	(-94.515623708464200137568750000000*Costp[3]+51.553976568253200075037500000000*Costp[1]-2.8641098093474000041687500000000)*Sintp[1]+18.903124741692840027513750000000*Costp[5]-17.184658856084400025012500000000*Costp[3]+2.8641098093474000041687500000000*Costp[1];
	dP[22]=	Sintp[0]*Costp[0]*(29.888464539015717059328750000000*Costp[3]-16.302798839463118395997500000000*Costp[1]+0.90571104663683991088875000000000-59.776929078031434118657500000000*Costp[1]*Sintp[1]+16.302798839463118395997500000000*Sintp[1]);
	dP[23]=	(-29.888464539015717058982250000000*Costp[1]+2.7171331399105197326347500000000)*Sintp[3]+(29.888464539015717058462500000000*Costp[3]-8.1513994197315591977625000000000*Costp[1])*Sintp[1];
	dP[24]=	Sintp[2]*Costp[0]*(21.827448316282871462161500000000*Costp[1]-1.9843134832984428601965000000000-10.913724158141435731*Sintp[1]);
	dP[25]=	11.634069043116428506*Costp[1]*Sintp[3]-2.3268138086232857013*Sintp[5];
	dP[26]=	4.0301597362883766588*Sintp[4]*Costp[0];
	dP[27]=	-Sintp[0]*(187.6875000000000000000*Costp[5]-216.5625000000000000000*Costp[3]+59.0625000000000000000*Costp[1]-2.1875000000000000000);
	dP[28]=	(-212.81762108375800562656125000000*Costp[4]+163.70586237212154278966250000000*Costp[2]-22.323526687107483107681250000000*Costp[0])*Sintp[1]+35.469603513959667604426875000000*Costp[6]-40.926465593030385697415625000000*Costp[4]+11.161763343553741553840625000000*Costp[2]-0.41339864235384227977187500000000*Costp[0];
	dP[29]=	Sintp[0]*(57.921619992020250814222875000000*Costp[5]-44.555092301554039087863750000000*Costp[3]+6.0756944047573689665268750000000*Costp[1]-144.80404998005062703837250000000*Costp[3]*Sintp[1]+66.832638452331058633095000000000*Costp[1]*Sintp[1]-3.0378472023786844833225000000000*Sintp[1]);
	dP[30]=	(-81.913540547335640118475500000000*Costp[2]+18.903124741692840027340500000000*Costp[0])*Sintp[3]+(61.435155410501730086604375000000*Costp[4]-28.354687112539260039971250000000*Costp[2]+1.2888494142063300018168750000000*Costp[0])*Sintp[1];
	dP[31]=	Sintp[2]*(49.395723499104658235059500000000*Costp[3]-11.399013115177998054244500000000*Costp[1]-37.046792624328493675618950000000*Costp[1]*Sintp[1]+2.8497532787944995135091500000000*Sintp[1]);
	dP[32]=	-12.348930874776164559*Sintp[5]*Costp[0]+(30.872327186940411396123900000000*Costp[2]-2.3747943989954162612403000000000*Costp[0])*Sintp[3];
	dP[33]=	Sintp[4]*(14.530947577498170554*Costp[1]-2.4218245962496950923*Sintp[1]);
	dP[34]=	4.5308189450142452742*Sintp[5]*Costp[0];
	dP[35]=	-Sintp[0]*Costp[0]*(402.187500000000000000*Costp[5]-563.062500000000000000*Costp[3]+216.562500000000000000*Costp[1]-19.687500000000000000);
	dP[36]=	(-469.21875000000000000938437500000*Costp[5]+469.21875000000000000938437500000*Costp[3]-108.28125000000000000216562500000*Costp[1]+3.2812500000000000000656250000000)*Sintp[1]+67.031250000000000001340625000000*Costp[7]-93.843750000000000001876875000000*Costp[5]+36.093750000000000000721875000000*Costp[3]-3.2812500000000000000656250000000*Costp[1];
	dP[37]=	Sintp[0]*Costp[0]*(112.16473480722450315093468750000*Costp[5]-112.16473480722450315093468750000*Costp[3]+25.884169570897962265600312500000*Costp[1]-0.78436877487569582623031250000000-336.49420442167350946125000000000*Costp[3]*Sintp[1]+224.32946961444900630750000000000*Costp[1]*Sintp[1]-25.884169570897962266250000000000*Sintp[1]);
	dP[38]=	(-207.09786664084085385567750000000*Costp[3]+82.839146656336341542271000000000*Costp[1]-3.1861210252437054439335000000000)*Sintp[3]+(124.25871998450451231171731250000*Costp[5]-82.839146656336341541144875000000*Costp[3]+9.5583630757311163316705625000000*Costp[1])*Sintp[1];
	dP[39]=	Sintp[2]*Costp[0]*(106.94487847134148925586468750000*Costp[3]-42.777951388536595702345875000000*Costp[1]+1.6453058226360229116286875000000-106.94487847134148925755387500000*Costp[1]*Sintp[1]+21.388975694268297851510775000000*Sintp[1]);
	dP[40]=	(-44.491758805002305282235750000000*Costp[1]+2.9661172536668203521490500000000)*Sintp[5]+(74.152931341670508803726250000000*Costp[3]-14.830586268334101760745250000000*Costp[1])*Sintp[3];
	dP[41]=	Sintp[4]*Costp[0]*(41.191364575903525679220525000000*Costp[1]-2.7460909717269017119480350000000-13.730454858634508560*Sintp[1]);
	dP[42]= 17.547786318721230243*Sintp[5]*Costp[1]-2.5068266169601757490*Sintp[7];
	dP[43]= 5.0136532339203514980*Sintp[6]*Costp[0];
	dP[44]= -Sintp[0]*(854.6484375000000000000*Costp[7]-1407.6562500000000000000*Costp[5]+703.8281250000000000000*Costp[3]-108.2812500000000000000*Costp[1]+2.4609375000000000000);
	dP[45]=	(-1019.2277349941228909903250000000*Costp[6]+1259.0460255809753359292250000000*Costp[4]-419.68200852699177864307500000000*Costp[2]+32.283231425153213741775000000000*Costp[0])*Sintp[1]+127.40346687426536137379062500000*Costp[8]-209.84100426349588932153750000000*Costp[6]+104.92050213174794466076875000000*Costp[4]-16.141615712576606870887500000000*Costp[2]+0.36685490255855924706562500000000*Costp[0];
	dP[46]=	Sintp[0]*(217.30008323025327364302656250000*Costp[7]-268.42951457854816155903281250000*Costp[5]+89.476504859516053853010937500000*Costp[3]-6.8828080661166195271546875000000*Costp[1]-760.55029130588645772666281250000*Costp[5]*Sintp[1]+671.07378644637040387646718750000*Costp[3]*Sintp[1]-134.21475728927408077529343750000*Costp[1]*Sintp[1]+3.4414040330583097634690625000000*Sintp[1]);
	dP[47]=	(-497.89703996141984851526550000000*Costp[4]+292.88061174201167559721500000000*Costp[2]-29.288061174201167559721500000000*Costp[0])*Sintp[3]+(248.94851998070992425284671875000*Costp[6]-219.66045880650875669368828125000*Costp[4]+43.932091761301751338737656250000*Costp[2]-1.1264638913154295215060937500000*Costp[0])*Sintp[1];
	dP[48]=	Sintp[2]*(225.50295348675590586425960625000*Costp[5]-132.64879616867994462603506250000*Costp[3]+13.264879616867994462603506250000*Costp[1]-281.87869185844488233391403125000*Costp[3]*Sintp[1]+99.486597126509958470793187500000*Costp[1]*Sintp[1]-3.3162199042169986156931062500000*Sintp[1]);
	dP[49]=	(-134.76379074838685518695682500000*Costp[2]+23.781845426185915621227675000000*Costp[0])*Sintp[5]+(168.45473843548356898369603125000*Costp[4]-59.454613565464789053069187500000*Costp[2]+1.9818204521821596351023062500000*Costp[0])*Sintp[3];
	dP[50]=	Sintp[4]*(104.38758344805668484601995000000*Costp[3]-18.421338255539414972827050000000*Costp[1]-52.193791724028342422435651250000*Costp[1]*Sintp[1]+3.0702230425899024954373912500000*Sintp[1]);
	dP[51]=	-15.067049850947513847*Sintp[7]*Costp[0]+(52.734674478316298463752006250000*Costp[2]-3.1020396751950763802207062500000*Costp[0])*Sintp[5];
	dP[52]=	Sintp[6]*(20.671821853673178673*Costp[1]-2.5839777317091473341*Sintp[1]);
	dP[53]=	5.4814445295797135446*Sintp[7]*Costp[0];
	dP[54]=	-Sintp[0]*Costp[0]*(1804.257812500000000000*Costp[7]-3418.593750000000000000*Costp[5]+2111.484375000000000000*Costp[3]-469.218750000000000000*Costp[1]+27.070312500000000000);
	dP[55]=	(-2189.5746642643140507516503906250*Costp[7]+3226.7416104947786011076953125000*Costp[5]-1423.5624752182846769592773437500*Costp[3]+189.80833002910462359457031250000*Costp[1]-3.6501601928673966075878906250000)*Sintp[1]+243.28607380714600563907226562500*Costp[9]-460.96308721353980015824218750000*Costp[7]+284.71249504365693539185546875000*Costp[5]-63.269443343034874531523437500000*Costp[3]+3.6501601928673966075878906250000*Costp[1];
	dP[56]=	Sintp[0]*Costp[0]*(421.38384060792872464336054687500*Costp[7]-620.98671247484233105337343750000*Costp[5]+273.96472609184220487648828125000*Costp[3]-36.528630145578960650198437500000*Costp[1]+0.70247365664574924327304687500000-1685.5353624317148985734421875000*Costp[5]*Sintp[1]+1862.9601374245269931601203125000*Costp[3]*Sintp[1]-547.92945218368440975297656250000*Costp[1]*Sintp[1]+36.528630145578960650198437500000*Sintp[1]);
	dP[57]=	(-1156.9623832159617706272728437500*Costp[5]+913.39135517049613470574171875000*Costp[3]-161.18670973596990612454265625000*Costp[1]+3.5819268830215534694342812500000)*Sintp[3]+(495.84102137826933026883121875000*Costp[7]-548.03481310229768082344503125000*Costp[5]+161.18670973596990612454265625000*Costp[3]-10.745780649064660408302843750000*Costp[1])*Sintp[1];
	dP[58]=	Sintp[2]*Costp[0]*(467.48339814271748335790920312500*Costp[5]-369.06584063898748686150726562500*Costp[3]+65.129265995115438857913046875000*Costp[1]-1.4473170221136764190647343750000-701.22509721407622505050399375000*Costp[3]*Sintp[1]+369.06584063898748686868631250000*Costp[1]*Sintp[1]-32.564632997557719429589968750000*Sintp[1]);
	dP[59]=	(-369.57807661157885539057958437500*Costp[3]+116.70886629839332275491986875000*Costp[1]-3.4326137146586271398505843750000)*Sintp[5]+(369.57807661157885539603566000000*Costp[5]-194.51477716398887126107140000000*Costp[3]+17.163068573293135699506300000000*Costp[1])*Sintp[3];
	dP[60]=	Sintp[4]*Costp[0]*(247.92051068913462650929738125000*Costp[3]-78.290687586042513634514962500000*Costp[1]+2.3026672819424268716033812500000-165.28034045942308434316897125000*Costp[1]*Sintp[1]+26.096895862014171212079311250000*Sintp[1]);
	dP[61]=	(-60.129556019316143218170687375000*Costp[1]+3.1647134747008496430616151250000)*Sintp[7]+(140.30229737840433417464038875000*Costp[3]-22.152994322905947501259008750000*Costp[1])*Sintp[5];
	dP[62]=	Sintp[6]*Costp[0]*(65.460769203298396242351786750000*Costp[1]-3.4453036422788629601237782500000-16.365192300824599061*Sintp[1]);
	dP[63]=	23.893062769061818117*Sintp[7]*Costp[1]-2.6547847521179797908*Sintp[9];
	dP[64]=	5.9362791713657325695*Sintp[8]*Costp[0];
	
	for(j=0; j<= nMax; j++)
	{
		Psin_mLon[j] = sin(j*Lon);
		Pcos_mLon[j] = cos(j*Lon);
	}
	
	X=0; Y=0; Z=0;
	B_north = 0; B_east = 0; B_earth = 0;
	
	for(i=1; i<=nMax;i++)        //n
	{
		kk = POSE_POW( (EarthRad2MAG/r),(i+2));
		
		for(j=0; j<=i;j++)       //m
		{
			temp1 = g2020[num]*Pcos_mLon[j] + h2020[num]*Psin_mLon[j];
			temp2 = g2020[num]*Psin_mLon[j] - h2020[num]*Pcos_mLon[j];
			
			X = X + temp1*dP[num];
			Y = Y + temp2*j*P[num]/Sintp[0];
			Z = Z + temp1*P[num];
			num = num + 1;
		}
		
		B_north = B_north + kk*X;
		B_east =  B_east  + kk*Y;
		B_earth = B_earth + kk*Z*(i+1);
		X=0; Y=0; Z=0;
	}

	B[0]=  B_north ;    //北  nT
	B[1]=  B_east  ;    //东  nT
	B[2]= -B_earth ;    //地  nT
}


/****************************************************************
功能：计算星下点经纬度
输入：cuR :当前位置
输出：
******************************************************************/

BOOL 	EarthLonLatCal(TYPE_CAL *cuR, TYPE_CAL *clonlat)
{
	TYPE_CAL	tmpNor =0.0f, tmpUse=0.0f;
	BOOL		result = TRUE;
	if ((NULL == cuR) || (NULL == clonlat))
	{return FALSE;}
	/*计算经纬度*/
	CalNormal(cuR, 3, &tmpNor);
	clonlat[2] = tmpNor;

	/*检查有效性*/
	if ((0 == CalPFlag(cuR[0])) || (0 == CalPFlag(tmpNor)))
	{return FALSE;}
	//赤经
	clonlat[0] = 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;
		}else{;}
	}
	//赤纬
	clonlat[1] = POSE_ASINF(tmpUse);
	return result;
}

/********************************************************************
功能：转换矩阵计算 惯性坐标系到地理坐标系的转换矩阵
输入：当前位置
输出：转换矩阵
注意：
*********************************************************************/
//void TransMatrixIE(TYPE_CAL *clonlat, TYPE_CAL cgst, TYPE_CAL *cAie)
void TransMatrixIE(TYPE_CAL *clonlat,  TYPE_CAL *cAie)
{
	float tempSla =0.0f, tempSra =0.0f;
	float tempSl =0.0f, tempCl =0.0f, tempSr =0.0f, tempCr =0.0f;
	if ((NULL == cAie) || (NULL == clonlat))
	{return;}
	tempSla = clonlat[1];
	//tempSra = clonlat[0] - cgst ;
	tempSra = clonlat[0] ;

	if (tempSra > POSE_PI)
	{
		tempSra = tempSra - POSE_2PI;
	}
	else if (tempSra < -POSE_PI)
	{
		tempSra = tempSra + POSE_2PI;
	}
	else
	{
		;
	}
	//纬度
	tempSl = POSE_SINF( tempSla );
	tempCl = POSE_COSF( tempSla );
	//经度
	tempSr = POSE_SINF( tempSra );
	tempCr = POSE_COSF( tempSra );

	cAie[0] = -tempSl * tempCr;
	cAie[1] = -tempSl * tempSr;
	cAie[2] = tempCl;
	cAie[3] = -tempSr ;
	cAie[4] = tempCr;
	cAie[5] = 0.0f;
	cAie[6] = -tempCl * tempCr;
	cAie[7] = -tempCl*tempSr;
	cAie[8] = -tempSl;	

}


/****************************************************************
功能：计算环境模型模块接口，每秒调用一次
输入：	
输出：	
******************************************************************/
void 	ZKEnvAct(void)
{
	UINT32		SatTime[2] ={0,0};
	TYPE_CAL    Aie[3][3] ={{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}}; 		//地东南坐标系至惯性系转换矩阵
	TYPE_CAL    AieT[3][3] ={{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}}; 		//地东南坐标系至惯性系转换矩阵转置
	TYPE_CAL    Aoi[3][3] ={{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}};			//惯性坐标系到卫星轨道坐标系转换矩阵
	TYPE_CAL	tmpRbo[9]={0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
	
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttOrbitGetPara_t *pObtIn =NULL;	
	sAttEnvPara_t *pEnv =NULL;
	sAttDeterPara_t *pDeter =NULL;
	//sAttOrbitGetPara_t *pOrbit = NULL;
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}

	pObtIn = &tmpAtt->sOrbitPara;	
	pEnv = &tmpAtt->sEnvPara;
	pDeter=&tmpAtt->sDeterPara;
	//pOrbit = &tmpAtt->sOrbitPara;	
		
	//初始化
	ZKEnvCycInit();	
	GetTime(SatTime);		    //获取当前星上时
	//MJD_CAL_ATT(SatTime[0], &MJD);	//计算儒略日
	//MJC_CAL_ATT(MJD, &MJC);			//计算儒略世纪
	//CalGst(SatTime, &(pEnv->fGST));    //计算格林尼治角
	
#ifdef  MINMODULE_TYPE
	TYPE_CAL 	MJD, MJC;
	UINT8 MJCLv;
	sAttOrbitGetPara_t *pOrbit = NULL;
	pOrbit = &tmpAtt->sOrbitPara;	
	MJD_CAL_ATT(SatTime[0], &MJD);	//计算儒略日
	MJC_CAL_ATT(MJD, &MJC, &MJCLv);			//计算儒略世纪
	pOrbit->MJCLv = MJCLv;
	ObtSunVecI(MJC, pEnv->SunVecI, pEnv->SunPosVecI);//计算惯性系太阳位置
#else	
	pEnv->SatTime_s=SatTime[0];
	pEnv->SatTime_ms=SatTime[1];
	//pEnv->MJD=MJD;
	//pEnv->MJC=MJC;
	pEnv->fvIn_Valid = POSE_NO;
	
	ObtSunVecI(pObtIn->MJC, pEnv->SunVecI, pEnv->SunPosVecI);//计算惯性系太阳位置

	ShadowFlgCal();  //计算阴影标志
	BetaShadowCal();  //Beta角
	if(0 == pObtIn->OrbitVld)
	{
		return;
	}

	Cal_OrbAttRate_Wo();//计算轨道系轨道角速度
		
	if(!EarthLonLatCal(pObtIn->JPos,  pEnv->fLonLat))
	{
		return;
	}
		
	//从惯性系到北东地地理系
	TransMatrixIE( pEnv->fLonLat, Aie[0]);  
	//* 磁场公式 */
	MagMath( pEnv->fLonLat, pObtIn->GAST, pEnv->fvBe );
	//* 惯性系磁矢量 */
	MatrixTransposeHL(Aie[0], AieT[0],3, 3);
	MatrixPlusVectorN( AieT[0], pEnv->fvBe, pEnv->fvBi, 3, 3 );
	//从惯性系到轨道系
	CalTransMatrix_IO(pObtIn->JPos, pObtIn->JVel, Aoi[0]);	
	/* 轨道系磁矢量 */
	MatrixPlusVectorN( Aoi[0], pEnv->fvBi, pEnv->fvBo, 3, 3 );
	
	if(POSE_NO == pDeter->Qi_Vld)
	{
		return;
	}
	
	//由卫星轨道系四元数Qbo得到转换矩阵Rbo
	Q2Att(pDeter->Qo, tmpRbo);
	//本体系磁矢量
	MatrixPlusVectorN( tmpRbo, pEnv->fvBo, pEnv->fvBb, 3, 3 );
	pEnv->fvIn_Valid = POSE_OK;
#endif
}