/*
 * 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;
/************************************************
 说明：初始化参数,进程开始调用一次
 输入：pCtlPara:控制参数结构
 返回 :
************************************************/
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;
	
	//磁控相关参数初始化
	
	//PD控制算法参数初始化
	//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控制算法参数初始化
	//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;
    
	//飞轮控制参数
	//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));		
}

/************************************************
 说明：初始化参数,每周期调用一次
 输入：pCtlPara:控制参数结构
************************************************/
void    ZKPosCtlOutInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttCtlPara_t *pWCtrl =NULL;
	if(NULL == tmpAtt)
	{return;}

	pWCtrl=&tmpAtt->sCtlPara;	
	//控制指令
	memset(pWCtrl->Mag_DeltBb, 0.0, 3*sizeof(TYPE_CAL));
	memset(pWCtrl->WheelCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
	
}
/***********************************************
说明：磁速率计算
***********************************************/
//Bdot=当前拍-上一拍磁矢量
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;
		}
	}
}

/***********************************************
//磁控速率阻尼算法
***********************************************/
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)
	{
		/* 死区处理*/
		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;
		}
	}
}


//选择计算Bdot的方式\计算磁控阻尼控制力矩
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))
		{
			/* 使用前后拍差来计算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");
}


//磁控对日控制算法
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-------------------------*/
	//计算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] );
	
	//计算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))
	{
		//计算期望力矩与卫星本体系磁场强度夹角，
		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 ))
		{
			//计算卫星-Zb轴与太阳矢量夹角β
			Posedot( &pDeter->SunVecb[0], &tempXb[0], 3, &tempB );
			tempBeta = POSE_ACOSF(tempB);
			//计算太阳指向控制磁矩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] ;   //弧度
			tempW0[1] = tmpConst->Mag_FBZKWTOSUN;
		}
		else
		{
			//tempW0[2] = pDeter->AttRateTarRe[2] ;   //弧度
			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))
		{
			/* 使用前后拍差来计算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);	
}


/***********************************************
说明：飞轮PD控制算法
***********************************************/
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;			
	}

	//计算惯性系下整星三轴角动量
	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];
	}
	//计算本体系飞轮角动量	
	MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
	//计算惯性系下整星三轴角动量
	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);
	
	//计算偏差角和偏差角速度
	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];
	}
	
	//限幅
	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);
			}
		}
	}

	//一阶惯性
	for(i=0;i<3;i++)
	{
		pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
		(1.0f/(1.0f+tmpTs))*tmpTc[i];
	}
	//记录前次数据
	memcpy(pCtrl->WheelCtrlResultPre, pCtrl->WheelCtrlResult, 3*sizeof(TYPE_CAL));
}


/***********************************************
说明：飞轮PID控制算法
***********************************************/
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));

	//计算惯性系下整星三轴角动量
	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];
	}
	//计算本体系飞轮角动量
	MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
	//计算惯性系下整星三轴角动量
	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);

	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;
		}	
	}

	//计算偏差角和偏差角速度
	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];//角度偏差积分
	}
	memcpy(pCtrl->SumAerrPre, tmpAerr, 3*sizeof(TYPE_CAL));
	
	//计算三轴控制指令
	for(i=0;i<3;i++)
	{
		tmpTc[i]=tmpP[i]*tmpDeltaA[i]+tmpD[i]*tmpDeltaW[i]+tmpI[i]*pCtrl->SumAerrPre[i] + tmpHXW[i];
	}

	//限幅
	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{;}
		}
	}
	//一阶惯性
	for(i=0;i<3;i++)
	{
		pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
		(1.0f/(1.0f+tmpTs))*tmpTc[i];
	}
	//记录前次数据
	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]);
}


/********************飞轮角动量计算 *****************/
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;
	//计算本体系合角动量(复用)
	memcpy(tmpJ, tmpConst->Whl_J, 4*sizeof(TYPE_CAL));
	memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));

	//计算四个轮子的角动量
	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;
		}
	}
	////计算卫星本体下飞轮三轴角动量
	//MatrixProductHL(tmpConst->AttCmdFlashPara.M_Wheel, PWhl->Whl_Momentum, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//计算本体系三轴角动量

	for(i=0;i<4;i++)
	{
		tmpHj[i]= PWhl->Whl_Momentum[i] - tmpConst->Whl_HCent[i];  //tempHCent可更改
	}
	MatrixProductHL(tmpCw, tmpHj, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//计算本体系飞轮角动量

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


/***********************************************
说明：磁力矩器卸载算法，卸载飞轮+星体
***********************************************/
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];
		}
		//计算三轴推力器卸载指令
		CalNormal(&pData->MagPara.Mag_Bb[0], 3, &tmpModB);
		CalNormal(&tmpHwb[0], 3, &tmpModH);
	
		//计算卫星本体系下卸载标志
		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;}

		//计算磁卸载角
		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);  //结果为弧度
			tmpang = POSE_ACOSF(tmpLamd);
		}
		else
		{
			tmpang = 0;
		}
	
		//计算磁力矩器控制器控制指令
		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;
	}
}


/************************************************
 说明：姿态控制算法，根据使用状态进行控制模式选择
 输入：  控制模式标志
 输出：
************************************************/
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
	//初始化
	memset(pCtrl->Mag_DeltBb, 0.0f, 3*sizeof(TYPE_CAL));
	//计算飞轮角动量和三轴角动量
	Whl_Momentum_Count();
	switch (tmpAtt->sModePara.WorkMode)
	{
		case	ATTMOD_RATEDMP:			    /*速率阻尼*/
		MagDmpMomentCal();
		break;
		case    ATTMOD_MagTOSUN:            //磁控对日
		MagSunMomentCal();
		break;
		default:
		break;
	}
#else
	//初始化
	ZKPosCtlOutInit();
	//计算飞轮角动量和三轴角动量
	Whl_Momentum_Count();
	//pAttCtrl->AttCtrl_TypeTEST=0;
	switch (tmpAtt->sModePara.WorkMode)
	{
		case    ATTMOD_BREAKPRE:			/*星箭分离前*/
		//pAttCtrl->AttCtrl_Type=0;
		break;
		case    ATTMOD_NOCOLL:			    /*碰撞规避模式*/
		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:			    /*速率阻尼*/
		MagDmpMomentCal();
		//pAttCtrl->AttCtrl_Type=2;
		break;
		//case	ATTMOD_TOSUN:			    /*对日定向*/
		case    ATTMOD_MagTOSUN:            //磁控对日
		MagSunMomentCal();
		break;
		case    ATTMOD_WhlTOSUN:            //轮控对日
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//pAttCtrl->AttCtrl_Type=1;
		break;
		case	ATTMOD_ONLTOEARTH:		    /*对地定向*/
		WheelCalc_PD();
		memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));		
		//pAttCtrl->AttCtrl_Type=1;
		break;
		case	ATTMOD_WHEELTOEARTH:		/*稳态对地*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case	ATTMOD_ATTAJUSTPRE:		    /*轨控前调姿*/
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
		break;
		case	ATTMOD_ORBITCTL:		    /*轨道控制*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case	ATTMOD_ATTAJUST:		    /*定向前调姿*/
		WheelCalc_PD();
		CtrlWheelReleaseProc();
		//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
		break;
		case	ATTMOD_ONLTOTAR:		    /*对目标定向*/
		CtrlModPD2PID();
		CtrlWheelReleaseProc();
		break;
		case    ATTMOD_UNCTL:			    /*无控模式*/
		memset(pCtrl->WheelCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
		memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));	
		break;
		default:
		break;
	}
#endif	
}