rag_agent/RAG-TEST-TOOLS/PrjAttCtrlMng/AttPosCtrl.c

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

#include    "AttMath.h"
#include    "AttCtrlMain.h"

//extern sAttPriData	* 	m_pZKPriInfo;
/************************************************
 ˵<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>룺pCtlPara:<EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ
 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> :
************************************************/
void    ZKPosCtlInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	//sAttOrbitCtlInfo_t *pOrbCtrl; 	
	//sAttCtlPara_t *pWCtrl;
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	
	//pOrbCtrl = &tmpAtt->sOrbitInp;
	//pWCtrl=&tmpAtt->sCtlPara;

	//pOrbCtrl->CurOrbCtrlPcak_ReceiveFlg = 0x00;
	
	//<2F>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD>ز<EFBFBD><D8B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
	
	//PD<50><44><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
	//tmpConst->AttCmdFlashPara.kp_ToSun_PD[0] = 2.6832f;
	//tmpConst->AttCmdFlashPara.kp_ToSun_PD[1] = 3.1585f;
	//tmpConst->AttCmdFlashPara.kp_ToSun_PD[2] = 1.3033f;
	//tmpConst->AttCmdFlashPara.kd_ToSun_PD[0] = 58.694f ;
	//tmpConst->AttCmdFlashPara.kd_ToSun_PD[1] = 69.0921f ;
	//tmpConst->AttCmdFlashPara.kd_ToSun_PD[2] = 22.8084f ;
	//tmpConst->AttCmdFlashPara.w0max_PD = 0.1f * ANG2RADIAN;

	tmpConst->AttCmdFlashPara.kp_ToSun_PD[0] = 0.98064f;
	tmpConst->AttCmdFlashPara.kp_ToSun_PD[1] = 1.1544f;
	tmpConst->AttCmdFlashPara.kp_ToSun_PD[2] = 0.773f;
	tmpConst->AttCmdFlashPara.kd_ToSun_PD[0] = 27.0464f ;
	tmpConst->AttCmdFlashPara.kd_ToSun_PD[1] = 31.8378f ;
	tmpConst->AttCmdFlashPara.kd_ToSun_PD[2] = 13.388f ;
	tmpConst->AttCmdFlashPara.w0max_PD = 0.1f * ANG2RADIAN;

	tmpConst->kp_NoToSun_PD[0] = 0.6949f;
	tmpConst->kp_NoToSun_PD[1] = 2.6546f;
	tmpConst->kp_NoToSun_PD[2] = 3.2654f;
	tmpConst->kd_NoToSun_PD[0] = 7.6008f ;
	tmpConst->kd_NoToSun_PD[1] = 29.0342f ;
	tmpConst->kd_NoToSun_PD[2] = 35.7145f ;
	tmpConst->w0max_NoPD = 0.2f * ANG2RADIAN;	
	tmpConst->PDNo_Ts = 0.2f;
	tmpConst->AttCmdFlashPara.PD_Ts = 3.0f;
	//PID<49><44><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
	//tmpConst->AttCmdFlashPara.kp_PID[0] = 2.6832f;
	//tmpConst->AttCmdFlashPara.kp_PID[1] = 3.1585f;
	//tmpConst->AttCmdFlashPara.kp_PID[2] = 1.3033f;
	//tmpConst->AttCmdFlashPara.kd_PID[0] = 58.694f;
	//tmpConst->AttCmdFlashPara.kd_PID[1] = 69.0921f;
	//tmpConst->AttCmdFlashPara.kd_PID[2] = 22.8084f;
	//tmpConst->AttCmdFlashPara.ki_PID[0] = 0.0008f;
	//tmpConst->AttCmdFlashPara.ki_PID[1] = 0.0009f;
	//tmpConst->AttCmdFlashPara.ki_PID[2] = 0.0002f;
	//tmpConst->AttCmdFlashPara.w0max_PID = 0.1f * ANG2RADIAN;
	tmpConst->AttCmdFlashPara.kp_PID[0] = 0.98064f;
	tmpConst->AttCmdFlashPara.kp_PID[1] = 1.1544f;
	tmpConst->AttCmdFlashPara.kp_PID[2] = 0.773f;
	tmpConst->AttCmdFlashPara.kd_PID[0] = 27.0464f;
	tmpConst->AttCmdFlashPara.kd_PID[1] = 31.8378f;
	tmpConst->AttCmdFlashPara.kd_PID[2] = 13.388f;
	tmpConst->AttCmdFlashPara.ki_PID[0] = 0.0004f;
	tmpConst->AttCmdFlashPara.ki_PID[1] = 0.0004f;
	tmpConst->AttCmdFlashPara.ki_PID[2] = 0.0002f;
	tmpConst->AttCmdFlashPara.w0max_PID = 0.1f * ANG2RADIAN;
	//tmpConst->kp_NoPID[0] = 0.5;
	//tmpConst->kp_NoPID[1] = 2.2;
	//tmpConst->kp_NoPID[2] = 3.6;
	//tmpConst->kd_NoPID[0] = 5.0;
	//tmpConst->kd_NoPID[1] = 25.9;
	//tmpConst->kd_NoPID[2] = 42.2;
	//tmpConst->ki_NoPID[0] = 0.0008;
	//tmpConst->ki_NoPID[1] = 0.0009;
	//tmpConst->ki_NoPID[2] = 0.0002;

	tmpConst->kp_NoPID[0] = 0.6949f;
	tmpConst->kp_NoPID[1] = 2.6546f;
	tmpConst->kp_NoPID[2] = 3.2654f;
	tmpConst->kd_NoPID[0] = 7.6008f;
	tmpConst->kd_NoPID[1] = 29.0342f;
	tmpConst->kd_NoPID[2] = 35.7145f;
	tmpConst->ki_NoPID[0] = 0.0002f;
	tmpConst->ki_NoPID[1] = 0.0008f;
	tmpConst->ki_NoPID[2] = 0.0009f;
	tmpConst->w0max_NoPID = 0.20f * ANG2RADIAN;
	tmpConst->PIDNo_Ts = 0.2f;
	tmpConst->AttCmdFlashPara.PID_Ts = 4.0f;
    
	//<2F><><EFBFBD>ֿ<EFBFBD><D6BF>Ʋ<EFBFBD><C6B2><EFBFBD>
	//tmpConst->Whl_J[0] = 0.0064;
	//tmpConst->Whl_J[1] = 0.0064;
	//tmpConst->Whl_J[2] = 0.0064;
	//tmpConst->Whl_J[3] = 0.0064;
   
	//memset(pWCtrl->WheelCtrlResultPre, 0, 3*sizeof(TYPE_CAL));		
}

/************************************************
 ˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ÿ<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
 <EFBFBD><EFBFBD><EFBFBD>룺pCtlPara:<EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ
************************************************/
void    ZKPosCtlOutInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttCtlPara_t *pWCtrl =NULL;
	if(NULL == tmpAtt)
	{return;}

	pWCtrl=&tmpAtt->sCtlPara;	
	//<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
	memset(pWCtrl->Mag_DeltBb, 0.0, 3*sizeof(TYPE_CAL));
	memset(pWCtrl->WheelCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
	
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
//Bdot=<3D><>ǰ<EFBFBD><C7B0>-<2D><>һ<EFBFBD>Ĵ<EFBFBD>ʸ<EFBFBD><CAB8>
void MagDataDeltaProc(TYPE_CAL *cMagBb, TYPE_CAL *cMagBbPre, TYPE_CAL *cMagDelta, TYPE_CAL cDeltaT)
{
	UINT32 i = 0;
	if(POSE_ABSF(cDeltaT) >POSE_ZERO)
	{	
		for (i = 0; i < 3; ++i)
		{
			cMagDelta[i] = (cMagBb[i] - cMagBbPre[i]) / cDeltaT;
		}
	}
}

/***********************************************
//<2F>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
***********************************************/
void CtrlMagBdotDamp(TYPE_CAL *cDeltBb, TYPE_CAL *cMagT)
{
	UINT8 i =0;
	TYPE_CAL tmpBdot[3] = { 0.0 };
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;	}
	memcpy( tmpBdot, cDeltBb, 12 );

	for (i = 0; i < 3; ++i)
	{
		/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
		if ((cDeltBb[i] > -POSE_ABSF(tmpConst->Mag_DampBdotMin)) && (cDeltBb[i] < POSE_ABSF(tmpConst->Mag_DampBdotMin)))
		{
			tmpBdot[i] = 0;
		}
		else
		{
			;
		}
		//cMagT[i] = -tmpConst->Mag_Bb_CtrlMax * ((float)CalPFlag(tmpBdot[i]));
		if (tmpBdot[i]>POSE_ZERO)
		{
			cMagT[i] = -tmpConst->Mag_Bb_CtrlMax;
		} 
		else if (tmpBdot[i]<POSE_ZERO)
		{
			cMagT[i] = tmpConst->Mag_Bb_CtrlMax;
		}
		else
		{
			cMagT[i] = 0;
		}
	}
}


//ѡ<><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Bdot<6F>ķ<EFBFBD>ʽ\<5C><><EFBFBD><EFBFBD><EFBFBD>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void MagDmpMomentCal(void)
{
	UINT32 tmpCNT = 0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//AttCtrlConst_t *tmpConst;	
	MagPara_t *PMag =NULL;
	sAttCtlPara_t *pWCtrl =NULL;	
	sAttDeterPara_t*pDeter =NULL;	
	MagPara_t *pMag =NULL;	
	//sAttEnvPara_t *pEnv;		
	MtPara_t	*pMt =NULL;
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	

	PMag = &tmpAtt->sPerPara.MagPara;
	pWCtrl=&tmpAtt->sCtlPara;
	pDeter=&tmpAtt->sDeterPara;
	pMag=&tmpAtt->sPerPara.MagPara;
	//pEnv = &tmpAtt->sEnvPara;
	pMt=&tmpAtt->sPerPara.MtPara;

	//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
	tmpCNT = pMt-> MTCtrlTime % 10;
	
	if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != pDeter->Wi_Vld))
	{
		if ((1 == tmpCNT)||( 0x88 == pMag->MagUseSwich))
		{
			cross( &PMag->Mag_Bb[0], &pDeter->Wi[0], &pWCtrl->Mag_DeltBb[0] );
			CtrlMagBdotDamp( pWCtrl->Mag_DeltBb,  pWCtrl->MagCtrlResult);				
		}
	}
	else if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != PMag->Mag_UsePre))
	{
		if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
		{
			/* ʹ<><CAB9>ǰ<EFBFBD><C7B0><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Bdot */
			//MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),tmpConst->Mag_BdotltaT );
			MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),5.0f );
			CtrlMagBdotDamp( pWCtrl->Mag_DeltBb,  pWCtrl->MagCtrlResult);				
		}
	}
	else
	{
		pWCtrl->MagCtrlResult[0] = 0.0f;
		pWCtrl->MagCtrlResult[1] = 0.0f;
		pWCtrl->MagCtrlResult[2] = 0.0f;
	}
	//printf("--FL1--\n");
}


//<2F>ſض<C5BF><D8B6>տ<EFBFBD><D5BF><EFBFBD><EFBFBD>㷨
void MagSunMomentCal(void)
{
	UINT32		tmpCNT = 0;
	UINT32		i = 0;
	UINT32		k = 0;
	UINT8		tempM1OK = POSE_OK; 
	TYPE_CAL    tempXb[3] = { 1.0f , 0.0f , 0.0f };
	TYPE_CAL    tempDt[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempN[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempA = 0.0f;
	TYPE_CAL    tempAlpha = 0.0f;
	TYPE_CAL    tempB = 0.0f;
	TYPE_CAL    tempBeta = 0.0f;
	TYPE_CAL    tempTXB[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempM1[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempWXB[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempW0[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL    tempM2[3] = { 0.0F , 0.0F , 0.0F };
	TYPE_CAL	tmpDtM=0.0f;
	//TYPE_CAL	tmpDtK1=0.05,tmpDtK2=1.0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	MagPara_t *PMag =NULL;	
	sAttCtlPara_t *pCtrl =NULL;	
	sAttDeterPara_t*pDeter =NULL;	
	sAttModPara_t *pWMod =NULL;
	sAttCtlPara_t *pWCtrl =NULL;	
	MagPara_t *pMag =NULL;	
	MtPara_t	*pMt =NULL;		
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	
	PMag = &tmpAtt->sPerPara.MagPara;
	pCtrl=&tmpAtt->sCtlPara;
	pDeter=&tmpAtt->sDeterPara;
	pWMod =&tmpAtt->sModePara;
	pWCtrl=&tmpAtt->sCtlPara;
	pMag=&tmpAtt->sPerPara.MagPara;
	pMt=&tmpAtt->sPerPara.MtPara;
	
	/*-----------------------------M1-------------------------*/
	//<2F><><EFBFBD><EFBFBD>Tm
	if (0x55 == pWMod->WModSadaOpen)
	{
		tempXb[0] = 0.0f ;
		tempXb[1] = 1.0f ;
		tempXb[2] = 0.0f ;
	}
	else
	{
		tempXb[0] = 0.0f ;
		tempXb[1] = 0.0f ;
		tempXb[2] = -1.0f ;
	}
	cross( &tempXb[0], pDeter->SunVecb , &tempDt[0] );
	
	//<2F><><EFBFBD><EFBFBD>Tn
	cross( &tempDt[0], &tempXb[0] , &tempN[0] );
	CalNormal(tempN, 3, &tmpDtM); 
	
	if(tmpDtM < POSE_ZERO)
	{
		tempM1OK = POSE_NO; 	
	}
	else
	{
		tempN[0] = tempN[0]/tmpDtM;
		tempN[1] = tempN[1]/tmpDtM;
		tempN[2] = tempN[2]/tmpDtM;			
	}
	tmpCNT = pMt-> MTCtrlTime % 10;
	if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
	{
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>ϵ<EFBFBD>ų<EFBFBD>ǿ<EFBFBD>ȼнǣ<D0BD>
		Posedot( &tempN[0], &PMag->Mag_UBb[0], 3, &tempA );
		tempAlpha = POSE_ACOSF(tempA);
		if ((tempAlpha > tmpConst->Mag_ATTTOSUN ) && ((tempAlpha < (180.0* ANG2RADIAN-tmpConst->Mag_ATTTOSUN))&&(tmpConst->Mag_ATTTOSUN< 90.0*ANG2RADIAN))&&(POSE_OK ==tempM1OK ))
		{
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-Zb<5A><62><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD>нǦ<D0BD>
			Posedot( &pDeter->SunVecb[0], &tempXb[0], 3, &tempB );
			tempBeta = POSE_ACOSF(tempB);
			//<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ƴž<C6B4>M1
			cross( &tempN[0], &PMag->Mag_Bb[0] , &tempTXB[0] );
			if (pWMod->WModSadaOpen== 0x55)
			{
				tempM1[0] = tmpConst->Mag_KpFBZK[0]*tempBeta*tempTXB[0];
				tempM1[1] = tmpConst->Mag_KpFBZK[1]*tempBeta*tempTXB[1];
				tempM1[2] = tmpConst->Mag_KpFBZK[2]*tempBeta*tempTXB[2];
			} 
			else
			{
				tempM1[0] = tmpConst->Mag_Kp[0]*tempBeta*tempTXB[0];
				tempM1[1] = tmpConst->Mag_Kp[1]*tempBeta*tempTXB[1];
				tempM1[2] = tmpConst->Mag_Kp[2]*tempBeta*tempTXB[2];
			}
			//printf("pCtrl12: %d \n", tmpCNT);
		}
		else
		{
			tempM1[0] =0.0f;
			tempM1[1] =0.0f;
			tempM1[2] =0.0f;
		}

		/*-----------------------------M2-------------------------*/
		if (0x55 == pWMod->WModSadaOpen)
		{
			//tempW0[0] = pDeter->AttRateTarRe[0] ;
			//tempW0[1] = pDeter->AttRateTarRe[1] ;   //<2F><><EFBFBD><EFBFBD>
			tempW0[1] = tmpConst->Mag_FBZKWTOSUN;
		}
		else
		{
			//tempW0[2] = pDeter->AttRateTarRe[2] ;   //<2F><><EFBFBD><EFBFBD>
			tempW0[2] = tmpConst->Mag_WTOSUN;
		}
		cross( &tempW0[0], &PMag->Mag_Bb[0] , &tempWXB[0] );

		if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != pDeter->Wi_Vld))
		{
			cross( &PMag->Mag_Bb[0], &pDeter->Wi[0], &pWCtrl->Mag_DeltBb[0] );
			for (i = 0; i < 3; ++i)
			{
				if (pWMod->WModSadaOpen== 0x55)
				{
					tempM2[i] = tmpConst->Mag_KdFBZK[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
				}
				else
				{
					tempM2[i] = tmpConst->Mag_Kd[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
				}			
			}
			//printf("pCtrl12: %d \n", tmpCNT);
		}
		else if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != PMag->Mag_UsePre))
		{
			/* ʹ<><CAB9>ǰ<EFBFBD><C7B0><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Bdot */
			//MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),tmpConst->Mag_BdotltaT );
			MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),5.0f );
			for (i = 0; i < 3; ++i)
			{
				if (pWMod->WModSadaOpen== 0x55)
				{
					tempM2[i] = tmpConst->Mag_KdFBZK[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
				}
				else
				{
					tempM2[i] = tmpConst->Mag_Kd[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
				}
			}
		}else{;}
		
		for (k = 0; k < 3; ++k)
		{
			pCtrl->MagCtrlResult[k] = - tempM1[k] - tempM2[k];
		}	
	}
	//printf("pCtrl12: %d \n", tmpCNT);	
}


/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PD<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
***********************************************/
void WheelCalc_PD(void)
{
	TYPE_CAL tmpDeltaA[3]= {0.0}, tmpDeltaW[3]= {0.0};
	UINT8 i=0;
	TYPE_CAL tmpTc[3]= {0.0};
	TYPE_CAL tmpP[3]= {0.0}, tmpD[3]= {0.0};
	TYPE_CAL tmpTs =0.0;
	TYPE_CAL tmpHwb[3]= {0.0}, tmpCw[12]= {0.0},tmpSJ[9]= {0.0},tmpHj[4]= {0.0}, tmpHXW[3]= {0.0},tmpHb[3]= {0.0}, tmpHIbi[3]= {0.0};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttDeterPara_t *pDeter =NULL;	
	sAttGuidLawPara_t *pGuide =NULL;
	sAttCtlPara_t *pCtrl =NULL;	
	sAttModPara_t *pWMod =NULL;
	WhlPara_t *PWhl =NULL;		
	TYPE_CAL tmpW0 =0.0;		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	

	pDeter=&tmpAtt->sDeterPara;
	pGuide=&tmpAtt->sGuidLawPara;
	pCtrl=&tmpAtt->sCtlPara;
	pWMod=&tmpAtt->sModePara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	

	if (POSE_OK ==  pWMod->WModSadaOpen)
	{	
		memcpy(tmpP, tmpConst->AttCmdFlashPara.kp_ToSun_PD, 3*sizeof(TYPE_CAL));
		memcpy(tmpD, tmpConst->AttCmdFlashPara.kd_ToSun_PD, 3*sizeof(TYPE_CAL));
		tmpW0 =	tmpConst->AttCmdFlashPara.w0max_PD;	
		tmpTs = tmpConst->AttCmdFlashPara.PD_Ts;	
	} 
	else
	{
		memcpy(tmpP, tmpConst->kp_NoToSun_PD, 3*sizeof(TYPE_CAL));
		memcpy(tmpD, tmpConst->kd_NoToSun_PD, 3*sizeof(TYPE_CAL));	
		tmpW0 =	tmpConst->w0max_NoPD;
		tmpTs = tmpConst->PDNo_Ts;			
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	memcpy(tmpSJ, tmpConst->SAT_J, 9*sizeof(TYPE_CAL));
	memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));
	for(i=0;i<4;i++)
	{
		tmpHj[i]= PWhl->Whl_Momentum[i];
	}
	//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD><CFB5><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD>	
	MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	MatrixProductHL(tmpSJ, pDeter->Wi, tmpHIbi, 3, 3, 1);

	for(i=0;i<3;i++)
	{
		tmpHb[i] = tmpHwb[i] + tmpHIbi[i];
	}
	cross(tmpHb, pDeter->Wi, tmpHXW);
	
	//<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD>Ǻ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
	for(i=0;i<3;i++)
	{
		tmpDeltaA[i]=pDeter->CLPAttAng[i]-pGuide->TargAng[i];
		DeviAngleLimit(&tmpDeltaA[i]);
		tmpDeltaW[i]=pDeter->CLPAttAngRat[i]-pGuide->TargAngRat[i];
	}
	
	for(i=0;i<3;i++)
	{
		tmpTc[i]=tmpP[i]*tmpDeltaA[i]+tmpD[i]*tmpDeltaW[i] + tmpHXW[i];
	}
	
	//<2F>޷<EFBFBD>
	for(i=0;i<3;i++)
	{
		if(POSE_ABSF(pDeter->CLPAttAngRat[i])>tmpW0)
		{
			if ((pDeter->CLPAttAngRat[i]>POSE_ZERO)&&(tmpTc[i]<POSE_ZERO))
			{
				tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]- tmpW0);
			}
			else if((pDeter->CLPAttAngRat[i]<POSE_ZERO)&&(tmpTc[i]>POSE_ZERO))
			{
				tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]+ tmpW0);
			}
		}
	}

	//һ<>׹<EFBFBD><D7B9><EFBFBD>
	for(i=0;i<3;i++)
	{
		pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
		(1.0f/(1.0f+tmpTs))*tmpTc[i];
	}
	//<2F><>¼ǰ<C2BC><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	memcpy(pCtrl->WheelCtrlResultPre, pCtrl->WheelCtrlResult, 3*sizeof(TYPE_CAL));
}


/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
***********************************************/
void WheelCalc_PID(void)
{
	TYPE_CAL tmpP[3] ={0.0f}, tmpD[3] ={0.0f}, tmpI[3] ={0.0f};
	TYPE_CAL tmpDeltaA[3] ={0.0f}, tmpDeltaW[3] ={0.0f};
	UINT8 i =0;
	UINT8 tmp_cnt = 0;
	TYPE_CAL tmpAerr[3] ={0.0f};
	TYPE_CAL tmpTc[3] ={0.0f};
	TYPE_CAL tmpTs =0.0f;
	TYPE_CAL tmpW0 =0.0f;
	TYPE_CAL tmpHwb[3] ={0.0f}, tmpCw[12] ={0.0f},tmpSJ[9] ={0.0f},tmpHj[4] ={0.0f}, tmpHXW[3] ={0.0f},tmpHb[3] ={0.0f}, tmpHIbi[3] ={0.0f};
	
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttDeterPara_t *pDeter =NULL;	
	sAttGuidLawPara_t *pGuide =NULL;
	sAttCtlPara_t *pCtrl =NULL;	
	sAttModPara_t *pWMod =NULL;
	WhlPara_t *PWhl =NULL;
	AttIMPTPara_t *pImpt =NULL;
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pDeter=&tmpAtt->sDeterPara;
	pGuide=&tmpAtt->sGuidLawPara;
	pCtrl=&tmpAtt->sCtlPara;
	//tmpTs=tmpConst->AttCmdFlashPara.PID_Ts;
	pWMod=&tmpAtt->sModePara;	
	PWhl = &tmpAtt->sPerPara.WhlPara;
	pImpt=&tmpAtt->sIMPTPara;
	
	if (POSE_OK ==  pWMod->WModSadaOpen)
	{
		memcpy(tmpP, tmpConst->AttCmdFlashPara.kp_PID, 3*sizeof(TYPE_CAL));
		memcpy(tmpD, tmpConst->AttCmdFlashPara.kd_PID, 3*sizeof(TYPE_CAL));
		memcpy(tmpI, tmpConst->AttCmdFlashPara.ki_PID, 3*sizeof(TYPE_CAL));
		tmpW0 = tmpConst->AttCmdFlashPara.w0max_PID;
		tmpTs=tmpConst->AttCmdFlashPara.PID_Ts;
	} 
	else
	{
		memcpy(tmpP, tmpConst->kp_NoPID, 3*sizeof(TYPE_CAL));
		memcpy(tmpD, tmpConst->kd_NoPID, 3*sizeof(TYPE_CAL));
		memcpy(tmpI, tmpConst->ki_NoPID, 3*sizeof(TYPE_CAL));
		tmpW0 = tmpConst->w0max_NoPID;
		tmpTs=tmpConst->PIDNo_Ts;
	}
		
	memcpy(tmpAerr, pCtrl->SumAerrPre, 3*sizeof(TYPE_CAL));

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	memcpy(tmpSJ, tmpConst->SAT_J, 9*sizeof(TYPE_CAL));
	memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));
	for(i=0;i<4;i++)
	{
		tmpHj[i]= PWhl->Whl_Momentum[i];
	}
	//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD><CFB5><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD>
	MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	MatrixProductHL(tmpSJ, pDeter->Wi, tmpHIbi, 3, 3, 1);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽǶ<C7BD><C7B6><EFBFBD>
	for(i=0;i<3;i++)
	{
		tmpHb[i] = tmpHwb[i] + tmpHIbi[i];
	}
	cross(tmpHb, pDeter->Wi, tmpHXW);

	for (i = 0; i < WHEEL_NUM; ++i)
	{
		if ((POSE_OK == pImpt->Whl_Use[i])&&(POSE_OK == PWhl->WhlIn_Valid[i]))
		{
			if(tmp_cnt<255)
			{tmp_cnt++;}
		}
	}

	if (tmp_cnt < 3)
	{
		for(i=0;i<3;i++)
		{
			tmpHXW[i] =0.0f;
		}	
	}

	//<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD>Ǻ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
	for(i=0;i<3;i++)
	{
		tmpDeltaA[i]=pDeter->CLPAttAng[i]-pGuide->TargAng[i];
		DeviAngleLimit(&tmpDeltaA[i]);
		tmpDeltaW[i]=pDeter->CLPAttAngRat[i]-pGuide->TargAngRat[i];	
		tmpAerr[i]+=tmpDeltaA[i];//<2F>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	}
	memcpy(pCtrl->SumAerrPre, tmpAerr, 3*sizeof(TYPE_CAL));
	
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
	for(i=0;i<3;i++)
	{
		tmpTc[i]=tmpP[i]*tmpDeltaA[i]+tmpD[i]*tmpDeltaW[i]+tmpI[i]*pCtrl->SumAerrPre[i] + tmpHXW[i];
	}

	//<2F>޷<EFBFBD>
	for(i=0;i<3;i++)
	{
		if(POSE_ABSF(pDeter->CLPAttAngRat[i])>tmpW0 )
		{
			//printf("C: %d \n", i);
			if ((pDeter->CLPAttAngRat[i]>POSE_ZERO)&&(tmpTc[i]<POSE_ZERO))
			{
				tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]- tmpW0 );
			}
			else if((pDeter->CLPAttAngRat[i]<POSE_ZERO)&&(tmpTc[i]>POSE_ZERO))
			{
				tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]+ tmpW0);
			}
			else{;}
		}
	}
	//һ<>׹<EFBFBD><D7B9><EFBFBD>
	for(i=0;i<3;i++)
	{
		pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
		(1.0f/(1.0f+tmpTs))*tmpTc[i];
	}
	//<2F><>¼ǰ<C2BC><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	memcpy(pCtrl->WheelCtrlResultPre, pCtrl->WheelCtrlResult, 3*sizeof(TYPE_CAL));
	//printf("pCtrl11: %f,  |%f,  |%f \n", pCtrl->WheelCtrlResult[0],pCtrl->WheelCtrlResult[1],pCtrl->WheelCtrlResult[2]);
	//printf("pCtrl12: %f,  |%f,  |%f \n", pCtrl->WheelCtrlResultPre[0],pCtrl->WheelCtrlResultPre[1],pCtrl->WheelCtrlResultPre[2]);
}


/********************<2A><><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> *****************/
void Whl_Momentum_Count(void)
{
	UINT32 i = 0;
	TYPE_CAL  tmpCw[12] ={0.0f}, tmpJ[4] ={0.0f};
	TYPE_CAL tmpHj[4] ={0.0f};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	//sAttErrCtlPara_t 	*pREcheck;	
	WhlPara_t *PWhl =NULL;	
	//AttIMPTPara_t *pImpt;			
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}		
	//pREcheck=&tmpAtt->sErrCtlPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	//pImpt=&tmpAtt->sIMPTPara;
	//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD>ϽǶ<CFBD><C7B6><EFBFBD>(<28><><EFBFBD><EFBFBD>)
	memcpy(tmpJ, tmpConst->Whl_J, 4*sizeof(TYPE_CAL));
	memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));

	//<2F><><EFBFBD><EFBFBD><EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD>ӵĽǶ<C4BD><C7B6><EFBFBD>
	for (i = 0; i < WHEEL_NUM; ++i)
	{
		if (POSE_OK == PWhl->WhlIn_Valid[i])
		{
			PWhl->Whl_Momentum[i] =  tmpJ[i] * PWhl->Whl_Rate[i]*RPM2RADS;
		}
		else
		{
			PWhl->Whl_Momentum[i] = 0;
		}
	}
	////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	//MatrixProductHL(tmpConst->AttCmdFlashPara.M_Wheel, PWhl->Whl_Momentum, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>

	for(i=0;i<4;i++)
	{
		tmpHj[i]= PWhl->Whl_Momentum[i] - tmpConst->Whl_HCent[i];  //tempHCent<6E>ɸ<EFBFBD><C9B8><EFBFBD>
	}
	MatrixProductHL(tmpCw, tmpHj, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD><CFB5><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD>

	CalNormal(PWhl->Whl_Momentum_XYZ, 3, &PWhl->Whl_Momentum_Sum);
}


/***********************************************
˵<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 CtrlWheelReleaseProc(void)
{
	UINT8 i = 0;

	//TYPE_CAL tmpH[4];
	TYPE_CAL  tmpHwb[3] ={0.0f};
	//TYPE_CAL tmpHb[3],tmpHIbi[3];
	TYPE_CAL tmpk1 =0.0f,tmpk2 =0.0f;
	TYPE_CAL tmpModB =0.0f;
	TYPE_CAL tmpModH =0.0f;
	//TYPE_CAL tmpModHDET;
	TYPE_CAL tmpLamd =0.0f;
	TYPE_CAL tmpang =0.0f;
	TYPE_CAL tmpJB[3] ={0.0f};
	TYPE_CAL tempBXH[3] = { 0.0F , 0.0F , 0.0F };
	UINT32	tmpCNT = 0;
	
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttDeterPara_t *pDeter =NULL;	
	sAttCtlPara_t *pCtrl =NULL;	
	sAttDataPrePara_t *pData =NULL;
	MagPara_t	*pMag =NULL;	
	WhlPara_t	*PWhl =NULL;	
	MtPara_t	*pMt =NULL;	
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	
	pDeter=&tmpAtt->sDeterPara;
	pCtrl=&tmpAtt->sCtlPara;
	pData=&tmpAtt->sPerPara;
	pMag=&tmpAtt->sPerPara.MagPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	pMt=&tmpAtt->sPerPara.MtPara;

	if(POSE_NO == pDeter->Wi_Vld)
	{
		memset(pCtrl->MagCtrlResult, 0, 3*sizeof(TYPE_CAL));
		return;
	}

	tmpCNT = pMt-> MTCtrlTime % 10;
	if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
	{
		for(i=0;i<3;i++)
		{
			 tmpHwb[i] = PWhl->Whl_Momentum_XYZ[i];
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>ָ<EFBFBD><D6B8>
		CalNormal(&pData->MagPara.Mag_Bb[0], 3, &tmpModB);
		CalNormal(&tmpHwb[0], 3, &tmpModH);
	
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>ϵ<EFBFBD><CFB5>ж<EFBFBD>ر<EFBFBD>־
		tmpk1=tmpConst->k1_dump;
		tmpk2=tmpConst->k2_dump;
		if(tmpModH>tmpk1)
		{pCtrl->IsDump=POSE_OK;}
		else if((tmpModH>tmpk2)&&(POSE_OK==pCtrl->IsDump))
		{pCtrl->IsDump=POSE_OK;}
		else
		{pCtrl->IsDump=POSE_NO;}

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ؽ<EFBFBD>
		if ((tmpModB * tmpModH) > POSE_ZERO)
		{
			for(i=0;i<3;i++)
			{
				tmpJB[i] = tmpHwb[i]/(tmpModB * tmpModH);
			}
			Posedot(pData->MagPara.Mag_Bb,tmpJB,3, &tmpLamd);  //<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
			tmpang = POSE_ACOSF(tmpLamd);
		}
		else
		{
			tmpang = 0;
		}
	
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
		if ((tmpang>tmpConst->MTAtt_dump)&&(tmpang< (180.0 *ANG2RADIAN - tmpConst->MTAtt_dump))&&(pCtrl->IsDump==POSE_OK))
		{
			cross( pData->MagPara.Mag_Bb, tmpHwb , tempBXH );
			for(i=0;i<3;i++)
			{
				if (tmpModB > POSE_ZERO)
				{
					pCtrl->MagCtrlResult[i] = - tmpConst->ku_dump * tempBXH[i] /tmpModB /tmpModB ;
				}
				else
				{
					pCtrl->MagCtrlResult[i]=0.0f;
				}
			}
		}
		else
		{
			memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
		}
	}			
}

void CtrlModPD2PID(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttCtlPara_t *pCtrl =NULL;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttDeterPara_t *pDeter =NULL;
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL == tmpAtt) || (NULL == tmpConst))
	{return;}
	TYPE_CAL tmpAng = 5.0f * ANG2RADIAN;
	TYPE_CAL tmpAngRat = 0.1f * ANG2RADIAN;
	pCtrl=&tmpAtt->sCtlPara;
	pDeter=&tmpAtt->sDeterPara;	

	if  ((0x55 == pDeter->CLPAttValid) &&
	(POSE_ABSF(pDeter->CLPAttAng[0])<tmpAng) &&
	(POSE_ABSF(pDeter->CLPAttAng[1])<tmpAng) &&
	(POSE_ABSF(pDeter->CLPAttAng[2])<tmpAng) &&
	(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpAngRat) &&
	(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpAngRat)&&
	(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpAngRat))
	{
		if(pCtrl->PD2PIDCnt<60000)
		{pCtrl->PD2PIDCnt++;}
		if (20 <= pCtrl->PD2PIDCnt)
		{
			pCtrl->CtrlPD2PID = POSE_OK;
		}
	}
	else
	{
		pCtrl->PD2PIDCnt = 0;
	}
	if (POSE_OK == pCtrl->CtrlPD2PID)
	{
		WheelCalc_PID();
		//pCtrl->AttCtrl_TypeTEST=2;
	}
	else
	{
		WheelCalc_PD();
		//pCtrl->AttCtrl_TypeTEST=1;
	}
}


/************************************************
 ˵<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	ZKPosCtlAct(sAttDeterPara_t *pAttDet, sAttGuidLawPara_t *pAttGuid, sAttCtlPara_t *pAttCtrl)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttCtlPara_t *pCtrl =NULL;
	sAttModPara_t *pWMod =NULL;
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	pCtrl=&tmpAtt->sCtlPara;
	pWMod=&tmpAtt->sModePara;
	
#ifdef  MINMODULE_TYPE
	//<2F><>ʼ<EFBFBD><CABC>
	memset(pCtrl->Mag_DeltBb, 0.0f, 3*sizeof(TYPE_CAL));
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	Whl_Momentum_Count();
	switch (tmpAtt->sModePara.WorkMode)
	{
		case	ATTMOD_RATEDMP:			    /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
		MagDmpMomentCal();
		break;
		case    ATTMOD_MagTOSUN:            //<2F>ſض<C5BF><D8B6><EFBFBD>
		MagSunMomentCal();
		break;
		default:
		break;
	}
#else
	//<2F><>ʼ<EFBFBD><CABC>
	ZKPosCtlOutInit();
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>
	Whl_Momentum_Count();
	//pAttCtrl->AttCtrl_TypeTEST=0;
	switch (tmpAtt->sModePara.WorkMode)
	{
		case    ATTMOD_BREAKPRE:			/*<2A>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>ǰ*/
		//pAttCtrl->AttCtrl_Type=0;
		break;
		case    ATTMOD_NOCOLL:			    /*<2A><>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>ģʽ*/
		if  (pWMod->WModCnt1 >= 40)
		{	WheelCalc_PD();}
		else 
		{
			memset(pCtrl->WheelCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
			memset(pCtrl->WheelCtrlResultPre, 0.0f, 3*sizeof(TYPE_CAL));
		}
		//pAttCtrl->AttCtrl_Type=1;
		break;
		case	ATTMOD_RATEDMP:			    /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
		MagDmpMomentCal();
		//pAttCtrl->AttCtrl_Type=2;
		break;
		//case	ATTMOD_TOSUN:			    /*<2A><><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>*/
		case    ATTMOD_MagTOSUN:            //<2F>ſض<C5BF><D8B6><EFBFBD>
		MagSunMomentCal();
		break;
		case    ATTMOD_WhlTOSUN:            //<2F>ֿض<D6BF><D8B6><EFBFBD>
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//pAttCtrl->AttCtrl_Type=1;
		break;
		case	ATTMOD_ONLTOEARTH:		    /*<2A>Եض<D4B5><D8B6><EFBFBD>*/
		WheelCalc_PD();
		memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));		
		//pAttCtrl->AttCtrl_Type=1;
		break;
		case	ATTMOD_WHEELTOEARTH:		/*<2A><>̬<EFBFBD>Ե<EFBFBD>*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case	ATTMOD_ATTAJUSTPRE:		    /*<2A><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
		break;
		case	ATTMOD_ORBITCTL:		    /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case	ATTMOD_ATTAJUST:		    /*<2A><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
		break;
		case	ATTMOD_ONLTOTAR:		    /*<2A><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case    ATTMOD_UNCTL:			    /*<2A>޿<EFBFBD>ģʽ*/
		memset(pCtrl->WheelCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
		memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));	
		break;
		default:
		break;
	}
#endif	
}