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

/************************************************
 说明：定姿算法初始化，在最开始调用一次，进行常量的初始化动作
 输入：
 返回:
************************************************/
void 	ZKPGInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttDeterPara_t *pDeter =NULL;
//	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	
	pDeter=&tmpAtt->sDeterPara;
	pDeter->AttDeter_Type =0;
#ifdef  MINMODULE_TYPE
	//对日太阳角
	memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
	pDeter->AngToSun_ValidFlg=0x55;  //星敏本体系太阳矢量可用标志
	pDeter->CalAngToSun_Source=0;

	//闭环姿态角
	memset(pDeter->CLPAttAng,0.0,3*sizeof(TYPE_CAL));
	pDeter->CLPAttValid = POSE_OK;
#else
    //对日太阳角
	memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
	pDeter->AngToSun_ValidFlg=0x55;  //星敏本体系太阳矢量可用标志
	pDeter->CalAngToSun_Source=0;
	
    //轨道系姿态
	memset(pDeter->Qo,0.0,4*sizeof(TYPE_CAL));
	memset(pDeter->AttRateOrb,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AttAngTar,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AttAngOrb,0,3*sizeof(TYPE_CAL));
	memset(pDeter->SunVeco,0.0,3*sizeof(TYPE_CAL));
	pDeter->AttOrb_Vld = POSE_OK;						//卫星轨道系姿态有效标志

	//目标参考系姿态
	memset(pDeter->AttAngTarRe,0,3*sizeof(TYPE_CAL));
	//memset(pDeter->AttRateTarRe,0.00873,3*sizeof(TYPE_CAL));
	pDeter->AttRateTarRe[0] = 0.00873f;
	pDeter->AttRateTarRe[1] = 0.00873f;
	pDeter->AttRateTarRe[2] =  - 0.00873f;
	pDeter->AttTarRe_Vld = POSE_OK;

	//轨控参考系姿态
	memset(pDeter->AttAngOrbCtrl,0,3*sizeof(TYPE_CAL));
	memset(pDeter->AttRateOrbCtrl,0,3*sizeof(TYPE_CAL));
	pDeter->AttOrbCtrl_Vld = POSE_OK;

	//闭环姿态角
	memset(pDeter->CLPAttAng,0,3*sizeof(TYPE_CAL));
	pDeter->CLPAttValid = POSE_OK;
#endif
}

/***********************************************
说明：陀螺选择
***********************************************/
void Gyro_AttChoice(void)
{
	UINT8 i =0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//AttCtrlConst_t *tmpConst;
	GyroPara_t *PGyro[GYRO_NUM];
	sAttDeterPara_t *pDeter =NULL;
				
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
	PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
	PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
	pDeter=&tmpAtt->sDeterPara;	

	//if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0x55)//单陀螺可用
	if(	((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
		((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
		((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))//单陀螺可用
	{
		for(i=0;i<3;i++)
		{
			if(POSE_OK == PGyro[i]->Gyro_AvailableFlg)
			{
				memcpy(pDeter->Wi, PGyro[i]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->Wi_Vld=POSE_OK;
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | (0x01<<i) ;
				break;
			}
		}
	}
	//else if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0xAA)//两陀螺可用
	else if(((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
			((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))||
			((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))
	{
		pDeter->Wi_Vld=POSE_OK;
		if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg))
		{
			if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
			{
				memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
			}
			else
			{
				memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
			}
		}
		else if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
		{
			if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
			{
				memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
			}
			else
			{
				memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
			}
		}
		else
		{
			if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
			{
				memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
			}
			else
			{
				memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
				pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
			}
		}
	}
	else if((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))//三陀螺可用
	{
		pDeter->Wi_Vld=POSE_OK;
		if ((PGyro[0]->GyroPrior == PGyro[1]->GyroPrior)&&(PGyro[0]->GyroPrior == PGyro[2]->GyroPrior))
		{
			memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
			pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
		}
		else
		{
			if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
			{
				if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
				{
					memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
					pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
				}
				else
				{
					memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
					pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
				}
			}
			else
			{
				if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
				{
					memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
					pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
				}
				else
				{
					memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
					pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
				}
			}
		}
	}
	else{;}
	for(i=0;i<3;i++)
	{
		tmpAtt->sPerPara.Gyro_ATTWi[i] = pDeter->Wi[i];		
	}	
}

/***********************************************
说明：只用陀螺定姿
***********************************************/
void Gyro_AttDeter(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	GyroPara_t *PGyro[GYRO_NUM];	
	sAttDeterPara_t *pDeter =NULL;	
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}		
		
	PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
	PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
	PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
	pDeter=&tmpAtt->sDeterPara;

	//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
	if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
	{
		Gyro_AttChoice();
	}
	else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
	{
		memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//输出角速度
		pDeter->Wi_Vld=POSE_OK;
		pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
	}else{;}

	if(POSE_OK == pDeter->QiPre_Vld)//前次姿态有效
	{
		AttLongKutaCal(pDeter->WiPre, pDeter->QiPre, pDeter->Qi);//积分算四元数
		pDeter->Qi_Vld=POSE_OK;
		pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
		if (pDeter->GyroINT_Time<60000)
		{
			pDeter->GyroINT_Time++;
		}

		if (pDeter->GyroINT_Time > (tmpConst->GyroINT_TLimt*2) )
		{
			pDeter->Qi_Vld=POSE_NO;
			pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
		}
	}
	else//前次姿态无效
	{
		pDeter->Qi_Vld=POSE_NO;
		pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
		pDeter->GyroINT_Time= 0;
	}
}

/***********************************************
说明：星敏差分计算角速度
输入：星敏四元数，上周期四元数
输出：角速度
***********************************************/
void QuatDiffCal(TYPE_CAL * pQ_now, TYPE_CAL * pQ_pre, TYPE_CAL * pW)
{
	TYPE_CAL tmpDeltQ[4] ={0.0};
	TYPE_CAL tmpMatQuat[12] ={0.0};
	TYPE_CAL tmpMatDeltQ[4] ={0.0};
	UINT8 i =0;
	
	if(CalPFlag(pQ_now[3])==CalPFlag(pQ_pre[3]))
	{
		for(i=0;i<4;i++)
		{tmpDeltQ[i] = pQ_now[i] - pQ_pre[i];}
	}
	else//应该不会出现这种情况吧
	{
		for(i=0;i<4;i++)
		{tmpDeltQ[i] = -pQ_now[i] - pQ_pre[i];}
	}
	
	tmpMatQuat[0] = -pQ_pre[0];
	tmpMatQuat[1] = pQ_pre[3];
	tmpMatQuat[2] = pQ_pre[2];
	tmpMatQuat[3] = -pQ_pre[1];

	tmpMatQuat[4] = -pQ_pre[1];
	tmpMatQuat[5] = -pQ_pre[2];
	tmpMatQuat[6] = pQ_pre[3];
	tmpMatQuat[7] = pQ_pre[0];

	tmpMatQuat[8] = -pQ_pre[2];
	tmpMatQuat[9] = pQ_pre[1];
	tmpMatQuat[10] = -pQ_pre[0];
	tmpMatQuat[11] = pQ_pre[3];
	
	tmpMatDeltQ[0] = tmpDeltQ[3] / ATTPERIOD_S;
	tmpMatDeltQ[1] = tmpDeltQ[0] / ATTPERIOD_S;
	tmpMatDeltQ[2] = tmpDeltQ[1] / ATTPERIOD_S;
	tmpMatDeltQ[3] = tmpDeltQ[2] / ATTPERIOD_S;
	
	MatrixPlusVectorN(tmpMatQuat, tmpMatDeltQ, pW, 3, 4);
	for (i = 0; i < 3; ++i)
	{
		pW[i] *= 2.0f;
	}
}

/***********************************************
说明：使用陀螺确定角度，陀螺不可用，使用星敏差分的方式计算角度
***********************************************/
void SsGyro_AngDeter(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	GyroPara_t *PGyro[GYRO_NUM];	
	sAttDeterPara_t *pDeter =NULL;	
	
//	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}

	PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
	PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
	PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
	pDeter=&tmpAtt->sDeterPara;
	
	//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
	if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
	{
		Gyro_AttChoice();
	}
	else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
	{
		memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//输出角速度
		pDeter->Wi_Vld=POSE_OK;
		pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
	}
	else if(POSE_OK == pDeter->QiPre_Vld)//前次姿态有效
	{
		QuatDiffCal(pDeter->Qi, pDeter->QiPre, pDeter->Wi);//差分计算角速度
		pDeter->Wi_Vld = POSE_OK;
		pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
	}
	else
	{
		//pDeter->Wi_Vld = POSE_NO;
		//pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;	
		;	
	}
		
}


/***********************************************
说明：星敏陀螺定姿
输入：星敏,陀螺单元数据
输出：惯性系四元数/角速度，惯性系四元数/角速度有效标志
***********************************************/
void SsGyro_AttDeter(void)
{
	UINT8 i =0;
	TYPE_CAL	tmpRbi[9] ={0.0};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	GyroPara_t *PGyro[GYRO_NUM];	
	SsPara_t *PStar[STAR_NUM];	
	sAttDeterPara_t *pDeter =NULL;	
	AttIMPTPara_t *pImpt =NULL;	
	sAttModPara_t *pWMod =NULL;
	PPUPara_t *PPPU =NULL;
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}			
	
	PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
	PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
	PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
	
	PStar[0] = &tmpAtt->sPerPara.SsPara[0];
	PStar[1] = &tmpAtt->sPerPara.SsPara[1];
	PStar[2] = &tmpAtt->sPerPara.SsPara[2];

	pDeter=&tmpAtt->sDeterPara;	

	pImpt=&tmpAtt->sIMPTPara;
	
	pWMod =&tmpAtt->sModePara;
	
	PPPU = &tmpAtt->sPerPara.PPUPara;		
	//记录前次结果
	pDeter->QiPre_Vld=pDeter->Qi_Vld;
	memcpy(pDeter->QiPre,pDeter->Qi,4*sizeof(TYPE_CAL));
	memcpy(pDeter->WiPre,pDeter->Wi,3*sizeof(TYPE_CAL));
	
	//初始化
	pDeter->Qi_Vld=0;
	pDeter->Wi_Vld=0;
	memset(pDeter->Qi, 0, 4*sizeof(TYPE_CAL));
	memset(pDeter->Wi, 0, 3*sizeof(TYPE_CAL));
	pDeter->AttDeter_Type=0;

	//获取星敏优先级
	PStar[0]->ssPrior=tmpConst->SSUsePrior[0];
	PStar[1]->ssPrior=tmpConst->SSUsePrior[1];
	PStar[2]->ssPrior=tmpConst->SSUsePrior[2];
#ifdef  MINMODULE_TYPE
	//判断星敏ABC是否可用
	for(i=0;i<STAR_NUM;i++)
	{
		PStar[i]->ssAvailableFlg=POSE_NO;
		if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) )
		{PStar[i]->ssAvailableFlg=POSE_OK;}
	}
#else
	//判断星敏ABC是否可用
	for(i=0;i<STAR_NUM;i++)
	{
		PStar[i]->ssAvailableFlg=POSE_NO;
		if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) && (PStar[i]->ssCrCheckFlg!=CC_ERROR))
		{PStar[i]->ssAvailableFlg=POSE_OK;}
	}
#endif	
	//获取陀螺优先级
	PGyro[0]->GyroPrior=tmpConst->GyroUsePrior[0];
	PGyro[1]->GyroPrior=tmpConst->GyroUsePrior[1];
	PGyro[2]->GyroPrior=tmpConst->GyroUsePrior[2];
#ifdef  MINMODULE_TYPE
	//判断陀螺是否可用
	for(i=0;i<GYRO_NUM;i++)
	{
		PGyro[i]->Gyro_AvailableFlg=POSE_NO;
		if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK))
		{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}
	}
#else	
	//判断陀螺是否可用
	for(i=0;i<GYRO_NUM;i++)
	{
		PGyro[i]->Gyro_AvailableFlg=POSE_NO;
		if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK)&&(PGyro[i]->GyroCrCheckFlg!=CC_ERROR))
		{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}		
	}	
#endif		
	//if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0)//无星敏可用
	if((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))//无星敏可用
	{
		//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55 ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //陀螺可用
		if (((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg) || (POSE_OK == PGyro[2]->Gyro_AvailableFlg)) ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //陀螺可用
		{
			//只用陀螺定姿
			Gyro_AttDeter();
		}
		else
		{
			//星敏陀螺均不可用
			pDeter->Qi_Vld=0;
			pDeter->Wi_Vld=0;
			pDeter->AttDeter_Type=0;
		}
	}
	//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0x55)//单星敏可用
	else if(((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
			((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
			((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//单星敏可用
	{
		pDeter->GyroINT_Time = 0;
		//输出姿态四元数
		for(i=0;i<3;i++)
		{
			if(POSE_OK == PStar[i]->ssAvailableFlg)
			{
				memcpy(pDeter->Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->Qi_Vld=0x55;
				pDeter->Wi_Vld=0x55;
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |(0x20<<i) ;
				break;
			}
		}
		SsGyro_AngDeter();//确定角速度
	}
	//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xAA)//两星敏可用
	else if (((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
			((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))||
			((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//两星敏可用
	{
		pDeter->GyroINT_Time = 0;
		pDeter->Qi_Vld=0x55;
		pDeter->Wi_Vld=0x55;
		if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg))
		{
			if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
			{
				memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));	
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;			
			}
			else
			{
				memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));	
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
			}
		}
		else if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))
		{
			if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
			{
				memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
			}
			else
			{
				memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
			}
		}
		else
		{
			if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
			{
				memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
			}
			else
			{
				memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
				memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
				pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
			}
		}
		
		SsGyro_AngDeter();//确定角速度
	}
	//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xFF)//三星敏可用
	else if((POSE_OK == PStar[0]->ssAvailableFlg)&& (POSE_OK == PStar[1]->ssAvailableFlg)&& (POSE_OK == PStar[2]->ssAvailableFlg))//三星敏可用
	{
		pDeter->GyroINT_Time = 0;
		pDeter->Qi_Vld=0x55;
		pDeter->Wi_Vld=0x55;
		if ((PStar[0]->ssPrior == PStar[1]->ssPrior)&&(PStar[0]->ssPrior == PStar[2]->ssPrior))
		{
			memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
			memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
			memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
			memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
			pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
		} 
		else
		{
			if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
			{
				if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
				{
					memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
				} 
				else
				{
					memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
				}
			}
			else
			{
				if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
				{
					memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
				} 
				else
				{
					memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
					memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
					pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
				}
			}
		}
		SsGyro_AngDeter();//确定角速度
	}else{;}

//#ifdef  MINMODULE_TYPE
//;
//#else
#ifndef  MINMODULE_TYPE
	GetTime(pDeter->SatTime);
	//计数轨控推力器加速度
	if((POSE_OK == pDeter->Qi_Vld)&& (ATTMOD_ORBITCTL  == pWMod->WorkMode ) &&(0x01 == PPPU->PPUIn_FIREOK))
	{
		Q2Att(pDeter->Qi, tmpRbi);
		MatrixProductHL(tmpRbi, tmpConst->AccThru_Up, pDeter->Ai, 3, 3, 1);
		pDeter->PPUVld = POSE_OK;
	}
	else
	{
		pDeter->Ai[0] = 0;
		pDeter->Ai[1] = 0;
		pDeter->Ai[2] = 0;
		pDeter->PPUVld = POSE_NO;
	}
#endif
}


/***********************************************
说明：利用星敏+模拟太敏，计算卫星+Xb\+Yb\-Zb轴对日太阳角
***********************************************/
void Cal_AngToSun(void)
{
	TYPE_CAL tmpRbi[9] ={0.0};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//AttCtrlConst_t *tmpConst;
	sAttDeterPara_t *pDeter= NULL;	
	sAttModPara_t *pWMod= NULL;
	sAttDataPrePara_t *pDataPre= NULL;	
	sAttOrbitGetPara_t *pOrbit = NULL;		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(tmpAtt==NULL)
	{return;}
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	
	pDeter=&tmpAtt->sDeterPara;
	pWMod=&tmpAtt->sModePara;
	pDataPre=&tmpAtt->sPerPara ;
	pOrbit = &tmpAtt->sOrbitPara;
	//初始化
	if((POSE_OK == pDeter->Qi_Vld)&&(POSE_OK == pOrbit->MJCLv)) //使用星敏且星敏可用
	{
		Q2Att(pDeter->Qi, tmpRbi);
		MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVecb, 3, 3, 1);
		pDeter->CalAngToSun_Source=0x55;
		pDeter->AngToSun_ValidFlg=0x55;
	}
	else if ((POSE_OK == pDataPre->AssPara.Ass_SunVecVld)&&(CC_ERROR!= pDataPre->AssPara.AssCrCheckFlg))//太敏可用
	{
		memcpy(pDeter->SunVecb, pDataPre->AssPara.Ass_SunVecb, 3*sizeof(TYPE_CAL));
		pDeter->CalAngToSun_Source=0xAA;
		pDeter->AngToSun_ValidFlg=0x55;
	}
	else
	{
		pDeter->CalAngToSun_Source=0x00;
		pDeter->AngToSun_ValidFlg=0;
	}
	
	if (ATTMOD_MagTOSUN == pWMod->WorkMode)
	{
		if ((POSE_OK !=pWMod->WModSadaOpen))
		{
			//计算-Z轴对日太阳角
			if (POSE_OK == pDeter->AngToSun_ValidFlg)
			{
				pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
				pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
				pDeter->AngToSun[2]=0.0f;
			}
			else
			{
				pDeter->AngToSun[0]=0.0f;
				pDeter->AngToSun[1]=0.0f;
				pDeter->AngToSun[2]=0.0f;
			}
		} 
		else
		{
			//计算+Y轴对日太阳角
			if (POSE_OK == pDeter->AngToSun_ValidFlg)
			{
				pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[1]);
				pDeter->AngToSun[1]=0.0f;
				pDeter->AngToSun[2]= POSE_ATAN2F(pDeter->SunVecb[0],pDeter->SunVecb[1]);
			}
			else
			{
				pDeter->AngToSun[0]=0.0f;
				pDeter->AngToSun[1]=0.0f;
				pDeter->AngToSun[2]=0.0f;
			}
		}
	}
	else if (ATTMOD_WhlTOSUN == pWMod->WorkMode)
	{
		if ((POSE_OK!= pWMod->WModSadaOpen))
		{
			//计算-Z轴对日太阳角
			if (POSE_OK ==pDeter->AngToSun_ValidFlg)
			{
				pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
				pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
				pDeter->AngToSun[2]=0.0f;
			}
			else
			{
				pDeter->AngToSun[0]=0.0f;
				pDeter->AngToSun[1]=0.0f;
				pDeter->AngToSun[2]=0.0f;
			}
		}
		else
		{
			//计算X轴对日太阳角
			if (POSE_OK == pDeter->AngToSun_ValidFlg)
			{
				pDeter->AngToSun[0]= 0.0f;
				pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[0]);
				pDeter->AngToSun[2]=- POSE_ATAN2F(pDeter->SunVecb[1],pDeter->SunVecb[0]);
			}
			else
			{
				pDeter->AngToSun[0]=0.0f;
				pDeter->AngToSun[1]=0.0f;
				pDeter->AngToSun[2]=0.0f;
			}
		}
	}
	else if (ATTMOD_UNCTL == pWMod->WorkMode)
	{
		//计算-Z轴对日太阳角
		if (POSE_OK == pDeter->AngToSun_ValidFlg)
		{
			pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
			pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
			pDeter->AngToSun[2]=0.0f;
		}
		else
		{
			pDeter->AngToSun[0]=0.0f;
			pDeter->AngToSun[1]=0.0f;
			pDeter->AngToSun[2]=0.0f;
		}
	}else{;}	

}


/***********************************************
说明：本体系磁场强度计算
***********************************************/
//磁场计算系磁场强度
void DertCalcBb(void)
{
	UINT8 i =0;
	UINT32 tmpCNT = 0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//AttCtrlConst_t *tmpConst;
	MagPara_t *pMag =NULL;
	MtPara_t	*pMt =NULL;	
	sAttEnvPara_t *pEnv =NULL;
	sAttModPara_t *pWMod =NULL;
	AttIMPTPara_t *pImpt =NULL;
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(tmpAtt==NULL)
	{return;}
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
		
	pMag=&tmpAtt->sPerPara.MagPara;
	pMt=&tmpAtt->sPerPara.MtPara;
	pEnv = &tmpAtt->sEnvPara;
	pWMod=&tmpAtt->sModePara;
	pImpt=&tmpAtt->sIMPTPara;

	//判断磁强计是否可用
	for(i=0;i<MAG_NUM;i++)
	{
		pMag->MagAvailableFlg[i]=POSE_NO;
		if((pImpt->MagUseState[i]==POSE_OK) && (pMag->MagIn_Valid[i] ==POSE_OK) && (pMag->MagCrCheckFlg[i]!=CC_ERROR))
		{pMag->MagAvailableFlg[i]=POSE_OK;}
	}

	//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
	tmpCNT = pMt-> MTCtrlTime % 10;
	if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
	{	
		memcpy(pMag->Mag_BbPre, pMag->Mag_Bb, 3*sizeof(TYPE_CAL));
		pMag->Mag_UsePre = pMag->Mag_Use;	
	}

#ifdef  MINMODULE_TYPE
	if ( POSE_OK == pMag->MagAvailableFlg[0])
	{
		if (1 == tmpCNT)
		{
			memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
		}
		pMag->Mag_Use   =POSE_OK;
		pMag->MagUseSwich = 0x11;
	}
	else if (POSE_OK == pMag->MagAvailableFlg[1])
	{
		if (1 == tmpCNT)
		{
			memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
		}
		pMag->Mag_Use   =POSE_OK;
		pMag->MagUseSwich = 0x22;
	}
	else if (POSE_OK == pMag->MagIn_Valid[2])
	{
		if (1 == tmpCNT)
		{
			memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
		}
		pMag->Mag_Use   =POSE_OK;
		pMag->MagUseSwich = 0x44;
	}
	else
	{
		memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
		pMag->Mag_Use =POSE_NO;
		pMag->MagUseSwich =POSE_NO;
	}
#else
	
	if ((ATTMOD_RATEDMP == pWMod->WorkMode)|| (ATTMOD_MagTOSUN == pWMod->WorkMode))
	{

		if ( POSE_OK == pMag->MagAvailableFlg[0])
		{
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
			}
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x11;
		}
		else if (POSE_OK == pMag->MagAvailableFlg[1])
		{
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
			}
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x22;
		}
		else if (POSE_OK==pMag->MagIn_Valid[2])
		{
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
			}			
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x44;
		}
		else if (POSE_OK == pEnv->fvIn_Valid)
		{
			memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x88;
		}
		else
		{
			memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
			pMag->Mag_Use =POSE_NO;	
			pMag->MagUseSwich =POSE_NO;			
		}
	}
	else
	{
		if (POSE_OK == pEnv->fvIn_Valid)
		{
			memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x88;
		}
		else if (POSE_OK == pMag->MagAvailableFlg[0])
		{	
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
			}
			
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x11;
		}
		else if (POSE_OK == pMag->MagAvailableFlg[1])
		{
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
			}
			
			pMag->Mag_Use   =POSE_OK;
			pMag->MagUseSwich = 0x22;
		}
		else if (POSE_OK==pMag->MagIn_Valid[2])
		{
			if (1 == tmpCNT)
			{
				memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
			}
			pMag->Mag_Use = POSE_OK;
			pMag->MagUseSwich = 0x44;
		}
		else
		{
			memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
			pMag->Mag_Use =POSE_NO;	
			pMag->MagUseSwich =POSE_NO;			
		}
	}	

	if (POSE_OK == pMag->Mag_Use)
	{
		for(i=0; i<3; i++)
		{pMag->Mag_UBb[i] = pMag->Mag_Bb[i] ;}
		unitary( &pMag->Mag_UBb[0], 3 ); // 归一化
	}
#endif
}

/***********************************************
说明：计算轨道系下姿态、角速度、太阳矢量
***********************************************/
void AttDeterOrb(void)
{
	TYPE_CAL tmpRbi[9] ={0.0}, tmpRoi[9] ={0.0}, tmpRio[9] ={0.0}, tmpRbo[9] ={0.0};
	TYPE_CAL tmpWoi[3] ={0.0};
	TYPE_CAL tmpWbo[3] ={0.0};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttDeterPara_t *pDeter =NULL;	
	sAttOrbitGetPara_t *pOrbitIn =NULL;	
	
//	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(tmpAtt==NULL)
	{return;}

	pDeter=&tmpAtt->sDeterPara;
	pOrbitIn=&tmpAtt->sOrbitPara;
	
	if((0 == pDeter->Qi_Vld)||(0 == pOrbitIn->OrbitVld))
	{
		pDeter->AttOrb_Vld=0;
		return;
	}
	pDeter->AttOrb_Vld=0x55;
	//计算轨道系姿态角
	Q2Att(pDeter->Qi, tmpRbi);
	CalTransMatrix_IO(pOrbitIn->JPos, pOrbitIn->JVel, tmpRoi);
	MatrixTransposeHL(tmpRoi, tmpRio, 3, 3);
	MatrixProductHL(tmpRbi, tmpRio, tmpRbo, 3, 3, 3);
	Matrix2Eul312A(tmpRbo, pDeter->AttAngOrb);
	
	//计算卫星轨道系四元素
	Eul312A2Quat(pDeter->AttAngOrb, pDeter->Qo);

	//计算轨道系太阳矢量
	MatrixProductHL(tmpRoi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVeco, 3, 3, 1);

	if(0 == pDeter->Wi_Vld)
	{
		return;
	}
	
	//计算姿态角速度
	AglSpdOofICal(pOrbitIn->JPos, pOrbitIn->JVel, tmpWoi);
	//pDeter->OrbAttRate_Wo = tmpWoi[1];
	MatrixProductHL(tmpRbo, tmpWoi, tmpWbo, 3, 3, 1);
	tmpWbo[0]=pDeter->Wi[0]-tmpWbo[0];
	tmpWbo[1]=pDeter->Wi[1]-tmpWbo[1];
	tmpWbo[2]=pDeter->Wi[2]-tmpWbo[2];
	memcpy(pDeter->AttRateOrb,tmpWbo,3*sizeof(TYPE_CAL));

}

/***********************************************
说明：计算轨控参考系姿态
***********************************************/
void AttDeterOrbCtrl(void)
{
	TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//轨道系到轨道系欧拉角,旋转矩阵
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttOrbitCtlInfo_t *pOrb =NULL;
	sAttDeterPara_t *pDeter =NULL;		
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{return;}
	pOrb=&tmpAtt->sOrbitInp;
	pDeter=&tmpAtt->sDeterPara;

	pDeter->AttOrbCtrl_Vld=0;
	memset(pDeter->AttAngOrbCtrl,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AttRateOrbCtrl,0.0,3*sizeof(TYPE_CAL));
	//如果当前工作模式不为轨控调姿（0X66）或轨控（0X77）或轨控后（0X8），退出
	//如卫星轨道系姿态有效标志为无效	
	if((ATTMOD_ATTAJUSTPRE!= tmpAtt->sModePara.WorkMode)&&(ATTMOD_ORBITCTL!= tmpAtt->sModePara.WorkMode))
	{return;}
	
	if (0x55 == pDeter->AttOrb_Vld)
	{
		pDeter->AttOrbCtrl_Vld=0x55;
	}
	else
	{
		pDeter->AttOrbCtrl_Vld = 0x00;
		return;
	}

	//获取卫星目标调姿姿态
	memcpy(pDeter->AttAngTar,pOrb->OrbCtrlPackCur.OrbCtrAng,3*sizeof(TYPE_CAL));
	
	//由卫星轨道系四元数Qbo得到转换矩阵Rbo
	Q2Att(pDeter->Qo, tmpRbo);
	//轨道系到轨控调整角转移矩阵RCo
	Att2M(pDeter->AttAngTar, tmpRCo);
	
    MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
	
	//轨控参考系转换矩阵
	MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
		
	//轨控参考系姿态角
	Matrix2Eul312A(tmpRbc, pDeter->AttAngOrbCtrl);
	//卫星轨控参考系角速度即为卫星轨道系角速度
	memcpy(pDeter->AttRateOrbCtrl,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
}

/***********************************************
说明：计算目标参考系姿态
***********************************************/
void AttDeterTarRe(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttDeterPara_t *pDeter =NULL;	
	sAttModPara_t *pWMod =NULL;
	sAttTARCCtlInfo_t  *pTAR =NULL;	
	//sAttOrbitCtlInfo_t  *pOrb;		
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(tmpAtt==NULL)
	{return;}
	
	pDeter=&tmpAtt->sDeterPara;
	pWMod=&tmpAtt->sModePara;
	pTAR=&tmpAtt->sTARCInp;
	//pOrb=&tmpAtt->sTARCInp;

	TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//轨道系到目标系欧拉角,旋转矩阵

	memset(pDeter->AttAngTarRe,0.0,3*sizeof(TYPE_CAL));
	memset(pDeter->AttRateTarRe,0.0,3*sizeof(TYPE_CAL));

	pDeter->AttTarRe_Vld = 0x00;
	//如果当前工作模式为目标定向调姿（0X99）或目标定向（0XAA），进行以下计算；否则，退出。
	if((pWMod->WorkMode!=ATTMOD_ATTAJUST)&&(pWMod->WorkMode!=ATTMOD_ONLTOTAR))
	{return;}
	//如卫星轨道系姿态有效标志为无效，置卫星轨控参考系姿态有效标志为无效，并退出；否则，置标志为有效
	if (POSE_OK == pDeter->AttOrb_Vld)
	{
		pDeter->AttTarRe_Vld = 0x55;
	}
	else
	{
		pDeter->AttTarRe_Vld = 0x00;
		return;
	}

	//获取目标调姿姿态
	memcpy(pDeter->AttAngTar,pTAR->TARCtrlPackCur.TARCtrAng,3*sizeof(TYPE_CAL));

	//由卫星轨道系四元数Qbo得到转换矩阵Rbo
	Q2Att(pDeter->Qo, tmpRbo);
	//轨道系目标角转移矩阵RCo
	Att2M(pDeter->AttAngTar, tmpRCo);
	
	MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
	
	//目标参考系转换矩阵
	MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
	//目标参考系姿态角
	Matrix2Eul312A(tmpRbc, pDeter->AttAngTarRe);
	//卫星目标参考系角速度即为卫星轨道系角速度
	memcpy(pDeter->AttRateTarRe,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
}

/***********************************************
说明：闭环姿态确定
输入：模式
输出：闭环姿态，闭环姿态角
***********************************************/
void LoopAttDeter(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttDeterPara_t *pDeter =NULL;	
	sAttModPara_t *pWMod =NULL;	
	MagPara_t *pMag =NULL;	
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL ==tmpAtt)
	{return;}
	pDeter=&tmpAtt->sDeterPara;
	pWMod=&tmpAtt->sModePara;
	pMag=&tmpAtt->sPerPara.MagPara;
		
	pDeter->CLPAttValid=0;

	memset(pDeter->CLPAttAngRatPre,0.0,3*sizeof(TYPE_CAL));
	memcpy(pDeter->CLPAttAngRatPre, pDeter->CLPAttAngRat, 3*sizeof(TYPE_CAL));		
	memset(pDeter->CLPAttAng, 0.0, 3*sizeof(TYPE_CAL));
	memset(pDeter->CLPAttAngRat, 0.0, 3*sizeof(TYPE_CAL));
#ifdef  MINMODULE_TYPE
	switch (pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP:
		case ATTMOD_MagTOSUN:
		if (POSE_OK == pMag->Mag_Use )
		{
			pDeter->CLPAttValid= 0x55;
			memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
		}
		break;
		default:
		break;
	}
#else				
	switch (pWMod->WorkMode)
	{
		case ATTMOD_UNCTL:
		case ATTMOD_NOCOLL:
		if (0x55 == pDeter->Wi_Vld )
		{
			pDeter->CLPAttValid= 0x55;
			memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
		}
		break;
			
		case ATTMOD_RATEDMP:      //zmx20231007
		case ATTMOD_MagTOSUN:
		if (POSE_OK == pMag->Mag_Use)
		{
			pDeter->CLPAttValid= 0x55;
			memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
		}
		break;

		case ATTMOD_WhlTOSUN:
		if(0x55 == pDeter->Wi_Vld)
		{
			pDeter->CLPAttValid=0x55;      //zmx20231009
			memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
			if (pDeter->AngToSun_ValidFlg==0x55)
			{
				//pDeter->CLPAttValid=0x55;
				pDeter->CLPAttAng[0]=pDeter->AngToSun[0];
				pDeter->CLPAttAng[1]=pDeter->AngToSun[1];
				pDeter->CLPAttAng[2]=pDeter->AngToSun[2];
			}
		}
		break;		
			
		case ATTMOD_ONLTOEARTH:
		case ATTMOD_WHEELTOEARTH:
		if(0x55 == pDeter->AttOrb_Vld)
		{
			pDeter->CLPAttValid=0x55;
			memcpy(pDeter->CLPAttAng, pDeter->AttAngOrb, 3*sizeof(TYPE_CAL));
			memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrb, 3*sizeof(TYPE_CAL));		
		}
		break;
		case ATTMOD_ATTAJUST:
		case ATTMOD_ONLTOTAR:
		if(0x55 == pDeter->AttTarRe_Vld)
		{
			pDeter->CLPAttValid=0x55;
			memcpy(pDeter->CLPAttAng, pDeter->AttAngTarRe, 3*sizeof(TYPE_CAL));
			memcpy(pDeter->CLPAttAngRat, pDeter->AttRateTarRe, 3*sizeof(TYPE_CAL));
		}
		break;
		case ATTMOD_ATTAJUSTPRE:
		case ATTMOD_ORBITCTL:
		if(0x55 == pDeter->AttOrbCtrl_Vld)
		{
			pDeter->CLPAttValid=0x55;
			memcpy(pDeter->CLPAttAng, pDeter->AttAngOrbCtrl, 3*sizeof(TYPE_CAL));
			memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrbCtrl, 3*sizeof(TYPE_CAL));
		}
		break;
		default:
		break;
	}
#endif
}

/************************************************
 说明：定姿算法接口，每周期调用一次定姿处理,包括数据预处理以及定姿计算
 输入：wbi:陀螺惯性系姿态角速度 （rad/s）
        startFlag:定姿算法启动标志 
 返回 :定姿模式1算法惯性系角速度 （rad/s）
************************************************/
void   ZKPGAct(void)
{
#ifdef  MINMODULE_TYPE
	//姿态变量初始化
	ZKPGInit();
	//星敏陀螺定姿
	SsGyro_AttDeter();
	//磁场计算系磁场强度
	DertCalcBb();
	//计算卫星-Zb轴对日太阳角
	Cal_AngToSun();
	//闭环姿态确定
	LoopAttDeter();
#else
	//姿态变量初始化
	ZKPGInit();	
	//星敏陀螺定姿
	SsGyro_AttDeter();
	//磁场计算系磁场强度
	DertCalcBb();
	//计算卫星-Zb轴对日太阳角
	Cal_AngToSun();
	//printf("ZL0\n");
	//计算轨道系姿态
	AttDeterOrb();
	//printf("ZL1\n");
	//目标参考系姿态
	AttDeterTarRe();
	//printf("ZL2\n");
	//计算轨控惯性系姿态
	AttDeterOrbCtrl();
	//printf("ZL3\n");
	//闭环姿态确定
	LoopAttDeter();
#endif
}