zhoujl/rag_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
rag_agent/RAG-TEST-TOOLS/PrjAttCtrlMng/AttEnvMod.c

720 lines
34 KiB
C
Raw Permalink Normal View History

增加代码知识库；修复文档处理内容；增加API设置
2026-05-16 20:20:10 +08:00
/*
* Created: 2022/11/4 11:02:40
* Author: wangzk zhengmengxing
*/
//#include "..\PrjCommon\CommonDef.h"
//#include "..\PrjCommon\DataStorDefine.h"
#include "AttCtrlMain.h"
#include "AttMath.h"
/*****************************************************************/
/*<2A><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
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;
//<2F><>Ӱ<EFBFBD>ж<EFBFBD>
pEnv->BetaAng=0.0f; //beta<74><61>
pEnv->BetaShadow=0x55; //<2F><><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD>Ӱ<EFBFBD><D3B0><EFBFBD>ã<EFBFBD>0x55<35><35>Ӱ<EFBFBD><D3B0>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pEnv->BetaAngCntIn=0; //<2F><><EFBFBD>ǽ<EFBFBD>Ӱ<EFBFBD>жϼ<D0B6><CFBC><EFBFBD> <20><>ʼֵΪ0
pEnv->BetaAngCntOut=0; //<2F><><EFBFBD>dz<EFBFBD>Ӱ<EFBFBD>жϼ<D0B6><CFBC><EFBFBD> <20><>ʼֵΪ0
tmpConst->BetaAngLimit=57.5f*ANG2RADIAN; //<2F><><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD>Ӱ<EFBFBD>ж<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ʼֵΪ57.5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD>޸ģ<EFBFBD>
pEnv->SatTime_s=0; //<2F><><EFBFBD><EFBFBD>ʱ
pEnv->SatTime_ms=0;
}
/*<2A><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
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
//̫<><CCAB>ʸ<EFBFBD><CAB8>
for (i = 0; i < 3 ; ++i)
{
pEnv->SunVecI[i]=0.0f; //<2F><><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ<59><5A><EFBFBD><EFBFBD>λʸ<CEBB><CAB8><EFBFBD><EFBFBD>
pEnv->SunPosVecI[i]=0.0f; //<2F><><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>λ<EFBFBD><CEBB>ʸ<EFBFBD><CAB8>XYZ
}
//pEnv->bInOutShadow =0x00;
#else
//̫<><CCAB>ʸ<EFBFBD><CAB8>
for (i = 0; i < 3 ; ++i)
{
pEnv->SunVecI[i]=0.0f; //<2F><><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ<59><5A><EFBFBD><EFBFBD>λʸ<CEBB><CAB8><EFBFBD><EFBFBD>
pEnv->SunPosVecI[i]=0.0f; //<2F><><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>λ<EFBFBD><CEBB>ʸ<EFBFBD><CAB8>XYZ
}
pEnv->bInOutShadow =0x00;
//<2F>ų<EFBFBD>
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
}
/***********************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ZYT
***********************************************/
void MJD_CAL_ATT(UINT32 sat_time, TYPE_CAL *pMjd)
{
*pMjd = (TYPE_CAL)sat_time/86400.0F + 7304.5F;
}
/***********************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
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;
}
}
/******************************************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
*******************************************************************/
void ObtSunVecI(TYPE_CAL JulC, TYPE_CAL *pSunVI, TYPE_CAL *pSunPVI)
{
TYPE_CAL tmpM =0.0f, tmpL =0.0f;
TYPE_CAL tmpeps =0.0f;//ƽ<>Ƴཻ<C6B3><E0BDBB>
TYPE_CAL tmpZodR =0.0f;
if ((NULL == pSunVI) || (NULL == pSunPVI))
{return;}
/*<2A><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
//tmpM = 6.2400599667 + 628.3019551515 * JulC;
tmpM = 6.2401f + 628.30196f * JulC;
//ƽ<>Ƴཻ<C6B3><E0BDBB>
//tmpeps = 0.4090928042 +0.0002269655* JulC;
tmpeps = 0.4090928f;
/*<2A><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ֵ<EFBFBD><D6B5>ļ<EFBFBD><C4BC><EFBFBD>ƽ<EFBFBD>ƾ<EFBFBD>*/
//tmpL = 4.8950629939 + 628.3319667861 * JulC + 0.0334160739 * POSE_SINF(tmpM);
tmpL = 4.8951f + 628.3076f * JulC + 0.03342f * POSE_SINF(tmpM);
/*<2A>Ƶ<EFBFBD><C6B5>뾶<EFBFBD><EBBEB6>m<EFBFBD><6D>*/
tmpZodR = (1.49619E11) - (2.499E9)*POSE_COSF(tmpM) - (2.1E7)*POSE_COSF(2.0f * tmpM);
//<2F><><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>
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];
}
/******************************************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: J2000<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>J2000<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
*******************************************************************/
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];
}
/******************************************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************************************/
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;
}
}
/******************************************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: J2000<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>Ӱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʶ
*******************************************************************/
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<74>Ǽ<EFBFBD><C7BC><EFBFBD>
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));
//<2F><><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD>Ӱ<EFBFBD><D3B0><EFBFBD><EFBFBD>
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;
}
}
}
/******************************************************************
*˵ <EFBFBD><EFBFBD>: <EFBFBD>شų<EFBFBD>ʸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㹫ʽ
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: J2000<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD>
*<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ش<EFBFBD>ʸ<EFBFBD><EFBFBD>
-----------------------------------------------------------------------------
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD><EFBFBD>ų<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> r <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵľ<EFBFBD><EFBFBD>루m<EFBFBD><EFBFBD>
Lon<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD>ȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> -pi<EFBFBD><EFBFBD>pi<EFBFBD><EFBFBD>
Lat<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD>ȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> -pi/2<EFBFBD><EFBFBD>pi/2<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>B={X Y Z} <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀռ<EFBFBD><EFBFBD>ų<EFBFBD>,<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>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));
}
// <20><><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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 ; //<2F><> nT
B[1]= B_east ; //<2F><> nT
B[2]= -B_earth ; //<2F><> nT
}
/****************************************************************
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ㾭γ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺cuR :<EFBFBD><EFBFBD>ǰλ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
******************************************************************/
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;}
/*<2A><><EFBFBD>㾭γ<E3BEAD><CEB3>*/
CalNormal(cuR, 3, &tmpNor);
clonlat[2] = tmpNor;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>*/
if ((0 == CalPFlag(cuR[0])) || (0 == CalPFlag(tmpNor)))
{return FALSE;}
//<2F>ྭ
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{;}
}
//<2F><>γ
clonlat[1] = POSE_ASINF(tmpUse);
return result;
}
/********************************************************************
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>ǰλ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ע<EFBFBD>⣺
*********************************************************************/
//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
{
;
}
//γ<><CEB3>
tempSl = POSE_SINF( tempSla );
tempCl = POSE_COSF( tempSla );
//<2F><><EFBFBD><EFBFBD>
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;
}
/****************************************************************
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>㻷<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD>ӿڣ<EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
******************************************************************/
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}}; //<2F>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TYPE_CAL AieT[3][3] ={{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}}; //<2F>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
TYPE_CAL Aoi[3][3] ={{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F><>ʼ<EFBFBD><CABC>
ZKEnvCycInit();
GetTime(SatTime); //<2F><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ʱ
//MJD_CAL_ATT(SatTime[0], &MJD); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//MJC_CAL_ATT(MJD, &MJC); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//CalGst(SatTime, &(pEnv->fGST)); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ν<EFBFBD>
#ifdef MINMODULE_TYPE
TYPE_CAL MJD, MJC;
UINT8 MJCLv;
sAttOrbitGetPara_t *pOrbit = NULL;
pOrbit = &tmpAtt->sOrbitPara;
MJD_CAL_ATT(SatTime[0], &MJD); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MJC_CAL_ATT(MJD, &MJC, &MJCLv); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pOrbit->MJCLv = MJCLv;
ObtSunVecI(MJC, pEnv->SunVecI, pEnv->SunPosVecI);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>λ<EFBFBD><CEBB>
#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);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>λ<EFBFBD><CEBB>
ShadowFlgCal(); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>־
BetaShadowCal(); //Beta<74><61>
if(0 == pObtIn->OrbitVld)
{
return;
}
Cal_OrbAttRate_Wo();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
if(!EarthLonLatCal(pObtIn->JPos, pEnv->fLonLat))
{
return;
}
//<2F>ӹ<EFBFBD><D3B9><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>ϵ
TransMatrixIE( pEnv->fLonLat, Aie[0]);
//* <20>ų<EFBFBD><C5B3><EFBFBD>ʽ */
MagMath( pEnv->fLonLat, pObtIn->GAST, pEnv->fvBe );
//* <20><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ʸ<EFBFBD><CAB8> */
MatrixTransposeHL(Aie[0], AieT[0],3, 3);
MatrixPlusVectorN( AieT[0], pEnv->fvBe, pEnv->fvBi, 3, 3 );
//<2F>ӹ<EFBFBD><D3B9><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
CalTransMatrix_IO(pObtIn->JPos, pObtIn->JVel, Aoi[0]);
/* <20><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ʸ<EFBFBD><CAB8> */
MatrixPlusVectorN( Aoi[0], pEnv->fvBi, pEnv->fvBo, 3, 3 );
if(POSE_NO == pDeter->Qi_Vld)
{
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>Ԫ<EFBFBD><D4AA>Qbo<62>õ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Rbo
Q2Att(pDeter->Qo, tmpRbo);
//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ʸ<EFBFBD><CAB8>
MatrixPlusVectorN( tmpRbo, pEnv->fvBo, pEnv->fvBb, 3, 3 );
pEnv->fvIn_Valid = POSE_OK;
#endif
}
Reference in New Issue Copy Permalink