/*
 * Created: 2022/11/4 11:02:40
 *  Author:  wangzk zhengmengxing
 */ 

#include	"..\PrjCommon\CommonDef.h"
#include	"..\PrjCommon\DevDefine.h"
#include 	"AttMath.h"
#include 	"AttCtrlMain.h"
#include    "..\PrjTelCtrlMng\TelCtrlMng.h"

/***********************************************
说明：数据处理模块初始化函数,进程开始调用一次	
***********************************************/
void 	ZKDPInit(void)
{
	UINT32 i = 0;
	sAttPriData *tmpAtt = NULL;
	SsPara_t *pStar= NULL;
	GyroPara_t *pGyro= NULL;
	AssPara_t *pAss= NULL;
	MagPara_t *pMag= NULL;
	WhlPara_t *PWhl= NULL;
	PPUPara_t *PPPU= NULL;

	tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;	
	if(NULL==tmpAtt)
	{
		return;		
	}
		
	pAss = &tmpAtt->sPerPara.AssPara;
	pMag = &tmpAtt->sPerPara.MagPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	PPPU = &tmpAtt->sPerPara.PPUPara;				
	
	//星敏
	for (i = 0; i < STAR_NUM; i++)
	{
		pStar = &tmpAtt->sPerPara.SsPara[i];
		pStar->ssIn_AttVld = POSE_NO;
		pStar->ssIn_DataSta  = 0;
		pStar->ssCrCheckFlg = NO_CC;
		memset(pStar->ssIn_W, 0, 3*sizeof(TYPE_CAL));
		pStar->ssExpose_Qbi[0] = 0;
		pStar->ssExpose_Qbi[1] = 0;
		pStar->ssExpose_Qbi[2] = 0;
		pStar->ssExpose_Qbi[3] = 1;
		memset(pStar->ssExpose_Wbi, 0, 3*sizeof(TYPE_CAL));
	}

	tmpAtt->sPerPara.Ss_Qi[0] = 0;
	tmpAtt->sPerPara.Ss_Qi[1] = 0;
	tmpAtt->sPerPara.Ss_Qi[2] = 0;
	tmpAtt->sPerPara.Ss_Qi[3] = 1;
	memset(tmpAtt->sPerPara.Ss_Wi, 0.0, 3*sizeof(TYPE_CAL));

	//陀螺
	for (i = 0; i < GYRO_NUM; i++)
	{
		pGyro = &tmpAtt->sPerPara.GyroPara[i];
		pGyro->GyroIn_AttVld = POSE_NO;
		pGyro->Gyro_ComTHUse[0] = POSE_OK;
		pGyro->Gyro_ComTHUse[1] = POSE_OK;
		pGyro->Gyro_ComTHUse[2] = POSE_OK;
		pGyro->GyroIn_DataSta  = 0;
		pGyro->GyroCrCheckFlg =NO_CC;
		memset(pGyro->GyroIn_Tmper, 0, 3*sizeof(TYPE_CAL));
		memset(pGyro->Gyro_Wi, 0, 3*sizeof(TYPE_CAL));
	}
	memset(tmpAtt->sPerPara.GyroIn_CombWi, 0, 3*sizeof(TYPE_CAL));
	memset(tmpAtt->sPerPara.Gyro_ATTWi, 0, 3*sizeof(TYPE_CAL));
	tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;	
	
	//模拟太敏
	for (i = 0; i < ASS_NUM; i++)
	{
		pAss->AssIn_AttVld[i]= POSE_NO;  
		memset(pAss->AssIn_sensor[i], 0, 4*sizeof(TYPE_CAL));
		pAss->AssCrCheckFlg =NO_CC;			
	}
	pAss->Ass_SunVecb[0]= 0;						//模拟太敏本体系太阳矢量XYZ
	pAss->Ass_SunVecb[1]= 0;						//模拟太敏本体系太阳矢量XYZ
	pAss->Ass_SunVecb[2]= 0;						//模拟太敏本体系太阳矢量XYZ
	pAss->Ass_SunVecVld= POSE_NO;					//模拟太敏太阳矢量有效标志
	pAss->Ass_SunVecbF[0]= 0;						//模拟太敏帆板系太阳矢量XYZ
	pAss->Ass_SunVecbF[1]= 0;						//模拟太敏帆板系太阳矢量XYZ
	pAss->Ass_SunVecbF[2]= 0;						//模拟太敏帆板系太阳矢量XYZ
	pAss->Ass_SunATT[0] = 0;
	pAss->Ass_SunATT[1] = 0;
	pAss->Ass_SunOK= POSE_NO;
	pAss->ADIn_Date= POSE_OK;
			
	//磁强计	
	for (i = 0; i < MAG_NUM; i++)
	{								
		pMag->MagIn_Valid[i]= POSE_NO;              //模拟磁强计A/B有效标志
		//输出量
		memset(pMag->Mag_Bc_MeaAB[i],0,3*sizeof(TYPE_CAL));
		memset(pMag->Mag_Bb_MeaAB[i],0,3*sizeof(TYPE_CAL));	
	}
	pMag->MagIn_Valid[2]= POSE_NO;								//模拟磁强计A/B有效标志	
	pMag->MagIn_DataSta= 0;		
	pMag->MagUseSwich= 0;	
	pMag->Mag_Bb_MeaZH[0] =0;
	pMag->Mag_Bb_MeaZH[1] =0;
	pMag->Mag_Bb_MeaZH[2] =0;
	for (i = 0; i < MAG_NUM; i++)
	{								
		pMag->MagCrCheckFlg[i]=	NO_CC;
		pMag->MagAvailableFlg[i] =	POSE_OK;
	}		
	memset(pMag->Mag_UBb,0,3*sizeof(TYPE_CAL));

	//飞轮
	for (i = 0; i < WHEEL_NUM; i++)
	{
		PWhl->WhlIn_DataSta[i] = 0;
		PWhl->WhlIn_Valid[i] = POSE_NO;	
		PWhl->Whl_Current[i]= 0.0f;
		PWhl->Whl_Tmper[i] = 0.0f;
		PWhl->Whl_Momentum[i]= 0;			//四个飞轮的角动量	
	}
	for(i=0;i<3;i++)
	{
		PWhl->Whl_Momentum_XYZ[i]= 0.0; 	//飞轮三轴的角动量
	}
	PWhl->Whl_Momentum_Sum = 0;

#ifndef  MINMODULE_TYPE	
	//电推
	PPPU->PPUIn_Valid = POSE_NO;			//PPU数据有效标志
	PPPU->PPUIn_DataSta = 0;				//PPU 数据无效原因

	PPPU->PPUIn_ReCnt= 0;					//PPU遥测请求计数
	PPPU->PPUIn_AVoltage = 0;				//推进阳极电源电压
	PPPU->PPUIn_ACurrent = 0;				//推进阳极电源电流

	PPPU->PPUT_BOTTLE_Tmper1 = 0.0f;
	PPPU->PPUT_BOTTLE_Tmper2 = 0.0f;
	PPPU->PPUT_PIPE_Tmper1 = 0.0f;
	PPPU->PPUT_PIPE_Tmper2 = 0.0f;

#endif
}

/***********************************************
说明：获取星敏数据，注意星敏A\B原包数据均为小端
***********************************************/
void Ss_DataGet(void )   //ST-APS2-2B(X)星敏感器
{
	UINT8	sum = 0;
	UINT8	i = 0;
	UINT8	j = 0;
	UINT16	tmpHEAD[3] = {0,0,0};
	UINT16	tmpINFIN[3] ={0,0,0};
	INT32	tempQdata[4] = {0,0,0,1};
	INT16	tempWdata = 0;
	INT32	tempQdataC = 0;
	INT16	tempWdataC = 0;
	sAttPriData *tmpAtt = NULL;
	sDevInfo * tmpDevInfo[3]= {NULL};
	SsPara_t *PStar[STAR_NUM]= {NULL};	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL==tmpAtt)
	{
		return;		
	}

	//获取设备数据地址
	tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART15);  //星敏A（803）
	tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART16);  //星敏B（803）
	tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART4);	//星敏C（天银）
		
	PStar[0] = &tmpAtt->sPerPara.SsPara[0];
	PStar[1] = &tmpAtt->sPerPara.SsPara[1];
	PStar[2] = &tmpAtt->sPerPara.SsPara[2];
	
	//记录前次原包数据
	for (i = 0; i < STAR_NUM; i++)
	{
		memcpy(PStar[i]->ssIn_Qpre, PStar[i]->ssIn_Q, 4*sizeof(TYPE_CAL));
		//初始化
		PStar[i]->ssIn_Q[0] = 0;
		PStar[i]->ssIn_Q[1] = 0;
		PStar[i]->ssIn_Q[2] = 0;
		PStar[i]->ssIn_Q[3] = 1;		
	}
	
	//获取星敏单机包头
	for (i = 0; i < STAR_NUM; i++)
	{
		tmpHEAD[i] = (tmpDevInfo[i]->cDevYCBuffer[0 + 20]<<8)|tmpDevInfo[i]->cDevYCBuffer[0 + 20+1];
	}
	//获取星敏单机信息字
	tmpINFIN[0] = (tmpDevInfo[0]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + 20+3];
	tmpINFIN[1] = (tmpDevInfo[1]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + 20+3];
	tmpINFIN[2] = tmpDevInfo[2]->cDevYCBuffer[0 + 20+2];

	//星敏AB数据获取
	for (i = 0; i < (STAR_NUM -1); i++)
	{
		if  (INFO_SIG_CHANGE == tmpDevInfo[i] ->cDevYCBuffer[0])
		{	
			if ((STARAB_HEAD != tmpHEAD[i]) || (STARAB_ID != tmpINFIN[i]))
			{
				PStar[i]->ssIn_AttVld	= POSE_NO;
				PStar[i]->ssIn_DataSta  = ZERO_TO_ONE(PStar[i]->ssIn_DataSta , 1, 1 ); /* errNo=0,星敏包头|ID号错误   */
			}
			else
			{
				sum = 0;
				/*判断校验和*/
				for(j = 0; j <  (STARAB_LEN - 1); j++)
				{
					sum += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];
				}
				if ((0xFF & sum) != tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+STARAB_LEN-1])
				{
					PStar[i]->ssIn_AttVld	= POSE_NO;
					PStar[i]->ssIn_DataSta  = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 2, 1 ); /* errNo=0,星敏数据校验和不对  */
				}
				else
				{
					//星敏数据包中状态字1判断
					if (0x20 == (tmpDevInfo[i]->cDevYCBuffer[20+20] & 0x30))
					{
						PStar[i]->ssIn_AttVld	= POSE_NO;
						PStar[i]->ssIn_DataSta  = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 3, 1 ); /* 星敏感器状态字1中数据无效*/
					}
					else
					{
						PStar[i]->ssIn_AttVld	= POSE_OK;			
					}
				}
			}
		}
		else
		{
			PStar[i]->ssIn_AttVld	= POSE_NO;
			PStar[i]->ssIn_DataSta  = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 0, 1 ); /* errNo=0,星敏数据不更新  */
		}

		//解析星敏AB的数据
		if (POSE_OK == PStar[i]->ssIn_AttVld)
		{
			//四元数
			for(j = 0; j < 4; j++)
			{
				tempQdata[j] = DoGetInt32From8(&(tmpDevInfo[i]->cDevYCBuffer[20+4*j+4]));
			}
			PStar[i]->ssIn_Q[3] = tempQdata[0]/2147483647.5f;
			PStar[i]->ssIn_Q[0] = tempQdata[1]/2147483647.5f;
			PStar[i]->ssIn_Q[1] = tempQdata[2]/2147483647.5f;
			PStar[i]->ssIn_Q[2] = tempQdata[3]/2147483647.5f;
				
			//角速度
			for(j = 0; j < 3; j++)
			{
				tempWdata = DoGetInt16From8(&tmpDevInfo[i]->cDevYCBuffer[20+2*j+45]);
				PStar[i]->ssIn_W[j] = ((float)tempWdata)*ANG2RADIAN*POSE_2T13;
			}
			PStar[i]->ssIn_W[2] = - PStar[i]->ssIn_W[2];
		}
	}

	//星敏C数据获取
	if  (INFO_SIG_CHANGE == tmpDevInfo[2] ->cDevYCBuffer[0])
	{
		if ((STARC_HEAD != tmpHEAD[2]) || (STARC_ID != tmpINFIN[2]))
		{
			PStar[2]->ssIn_AttVld	= POSE_NO;
			PStar[2]->ssIn_DataSta  = ZERO_TO_ONE(PStar[2]->ssIn_DataSta , 1, 1 ); /* errNo=0,星敏包头|ID号错误   */
		}
		else
		{
			sum = 0;
			/*判断校验和*/
			for(j = 2; j < (STARC_LEN - 1); j++)
			{sum += tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+j];}
			if ((0xFF & sum) != tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+STARC_LEN - 1])
			{
				PStar[2]->ssIn_AttVld	= POSE_NO;
				PStar[2]->ssIn_DataSta  = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 2, 1 ); /* errNo=0,星敏数据校验和不对  */
			}
			else
			{
				//星敏数据包中状态字1判断
				if ((tmpDevInfo[2]->cDevYCBuffer[20+33] & 0x01) != 0x01)
				{
					PStar[2]->ssIn_AttVld	= POSE_NO;
					PStar[2]->ssIn_DataSta  = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 3, 1 ); /* 星敏感器状态字1中数据无效*/
				}
				else
				{
					PStar[2]->ssIn_AttVld	= POSE_OK;			
				}
			}
		}
	}
	else
	{
		PStar[2]->ssIn_AttVld	= POSE_NO;
		PStar[2]->ssIn_DataSta  = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 0, 1 ); /* errNo=0,星敏数据不更新  */
	}

	//解析星敏C的数据
	if (POSE_OK == PStar[2]->ssIn_AttVld)
	{
		//四元数
		for(j = 0; j < 4; j++)
		{
			tempQdataC = DoGetInt32From8(&tmpDevInfo[2]->cDevYCBuffer[20+4*j+3]);
			PStar[2]->ssIn_Q[j] = ((float)tempQdataC)/2147483647.0f;
		}
		//角速度
		for(j = 0; j < 3; j++)
		{
			tempWdataC = DoGetInt16From8(&tmpDevInfo[2]->cDevYCBuffer[20+2*j+42]);
			PStar[2]->ssIn_W[j] = ((float)tempWdataC)*ANG2RADIAN*POSE_2T11;
		}
	}
}

/***********************************************
说明：星敏数据处理
***********************************************/
void Ss_DataPre(void)
{
	UINT8 i	= 0;
	UINT8 j	= 0;
	TYPE_CAL	tmpRbs[9] = {0.0};
	TYPE_CAL	tmpRsi[9] = {0.0};
	TYPE_CAL	tmpRbi[9] = {0.0};
	TYPE_CAL 	tmp_SS[3][3][3] = {0.0};			//星敏安装矩阵
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	SsPara_t *PStar[STAR_NUM] = {NULL};	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	if((NULL==tmpAtt)||(NULL==tmpConst))
	return;
		
	PStar[0] = &tmpAtt->sPerPara.SsPara[0];
	PStar[1] = &tmpAtt->sPerPara.SsPara[1];
	PStar[2] = &tmpAtt->sPerPara.SsPara[2];
	
	memcpy(&(tmp_SS[0][0][0]), &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9*sizeof(TYPE_CAL));
	memcpy(&(tmp_SS[1][0][0]), &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9*sizeof(TYPE_CAL));
	memcpy(&(tmp_SS[2][0][0]), &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9*sizeof(TYPE_CAL));

	for (i = 0; i < STAR_NUM; i++)
	{
		if ((POSE_OK ==PStar[i]->ssIn_AttVld))
		{
			if (POSE_ABSF(PStar[i]->ssIn_Qpre[0]-PStar[i]->ssIn_Q[0])< SENSOR_ZERO
			&&POSE_ABSF(PStar[i]->ssIn_Qpre[1]-PStar[i]->ssIn_Q[1])< SENSOR_ZERO
			&&POSE_ABSF(PStar[i]->ssIn_Qpre[2]-PStar[i]->ssIn_Q[2])< SENSOR_ZERO
			&&POSE_ABSF(PStar[i]->ssIn_Qpre[3]-PStar[i]->ssIn_Q[3])< SENSOR_ZERO)
			{
				if(PStar[i]->ssExpose_cnt<255)
				{
					PStar[i]->ssExpose_cnt++;
				}
			}
			else
			{
				PStar[i]->ssExpose_cnt =0;
			}
			
			if (PStar[i]->ssExpose_cnt >= 5)
			{
				PStar[i]->ssIn_AttVld	= POSE_NO;
				PStar[i]->ssIn_DataSta  = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 4, 1 );
			}
			else
			{
				if(PStar[i]->ssIn_Q[3]<0)
				{
					for (j = 0; j < 4; j++)
					{
						PStar[i]->ssIn_Q[j] = - PStar[i]->ssIn_Q[j];
					}
				}
				
				Q2Att(PStar[i]->ssIn_Q, tmpRsi);
				memcpy(tmpRbs, &(tmp_SS[i][0][0]), 9 * sizeof(TYPE_CAL));
				MatrixProductHL(tmpRbs, tmpRsi, tmpRbi, 3, 3, 3);
				Att2Q(tmpRbi, PStar[i]->ssExpose_Qbi);
				MatrixProductHL(tmpRbs, PStar[i]->ssIn_W, PStar[i]->ssExpose_Wbi, 3, 3, 1);
				PStar[i]->ssIn_AttVld=POSE_OK;
			}
		}
	}
}


/***********************************************
说明：获取陀螺数据
***********************************************/
void 	Gyro_DataGet(void)
{
	UINT8 i = 0;
	UINT8 j = 0;
	UINT8 k = 0;
	INT32 tmpDtInt = 0;
	INT16 tmpDtTmper = 0;
	UINT8 sum[GYRO_NUM] = {0,0,0};
	UINT16 tmpHEAD[GYRO_NUM] = {0,0,0};
	UINT8  tmpLONG[GYRO_NUM] = {0,0,0};
	UINT16 tmpDATAHEAD[GYRO_NUM] = {0,0,0};
	TYPE_CAL	tmpGyroIn_Speed[3]= {0.0,0.0,0.0};
	TYPE_CAL	tmpkGyroIn_Speed[3]= {0.0,0.0,0.0};
	UINT8 tmpcnt[GYRO_NUM][3] = {{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK}};
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	sDevInfo * tmpDevInfo[GYRO_NUM] = {NULL};	
	GyroPara_t *PGyro[GYRO_NUM] = {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];

    //获取陀螺地址
	tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART5) ;
	tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART6) ;
	tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART7) ;
	//获取数据包头
	tmpHEAD[0]=(tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
	tmpHEAD[1]=(tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
	tmpHEAD[2]=(tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
	tmpDATAHEAD[0] = GYRO_DATA_HEADA;
	tmpDATAHEAD[1] = GYRO_DATA_HEADA;
	tmpDATAHEAD[2] = GYRO_DATA_HEADC;
	//获取数据长
	tmpLONG[0] = GYRO_DATA_LENA;
	tmpLONG[1] = GYRO_DATA_LENA;
	tmpLONG[2] = GYRO_DATA_LENC;

	//记录前次原包数据
	for (i = 0; i < GYRO_NUM; i++)
	{
		memcpy(PGyro[i]->GyroIn_SpeedPre, PGyro[i]->GyroIn_Speed, 3*sizeof(TYPE_CAL));
		//初始化
		memset(PGyro[i]->GyroIn_Speed, 0, 3*sizeof(TYPE_CAL));
	}
	
	for (i = 0; i < GYRO_NUM ; ++i)
	{
		if (INFO_SIG_CHANGE == tmpDevInfo[i]->cDevYCBuffer[0])
		{
			if (tmpHEAD[i] == tmpDATAHEAD[i])
			{   
                /*判断校验和*/
                for(j = 0; j < tmpLONG[i] - 1; j++)
                {
	                sum[i] += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];//陀螺的包长
                }
				
                if ((0xFF & sum[i]) == tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+tmpLONG[i] - 1])
                {
					PGyro[i]->GyroIn_AttVld = POSE_OK;
				}
				else
				{
					PGyro[i]->GyroIn_AttVld	 = POSE_NO;
					PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 2, 1 ); /* errNo=0,陀螺A数据校验和错误  */
				}				
			} 
			else
			{
				PGyro[i]->GyroIn_AttVld	= POSE_NO;
				PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE(  PGyro[i]->GyroIn_DataSta, 1, 1 ); /* errNo=0,陀螺A包头|包长错误 */
			}
		}
		else
		{
            PGyro[i]->GyroIn_AttVld	= POSE_NO;
            PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 0, 1 ); /* errNo=0,陀螺A数据不更新  */
		}
	}

	//陀螺AB数据解析
	for (j = 0; j < 2 ; ++j)
	{
		if (POSE_OK ==PGyro[j]->GyroIn_AttVld)
		{
			for (i = 0; i < 3 ; ++i)
			{
				//XYZ轴角速度,单位：°/s
				tmpDtInt = DoGetInt32From8(&(tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+4*i+9]));
				tmpGyroIn_Speed[i] = (float)tmpDtInt  * (1.0E-5)*ANG2RADIAN  ;
				//XYZ轴角温度，单位：℃
				tmpDtTmper= tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+21]<<8|tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+22];
				PGyro[j]->GyroIn_Tmper[i]  = (float)(tmpDtTmper)*(1.0E-2);
			}
			MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[j][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);
			for (i = 0; i < 3 ; ++i)
			{
				//XYZ轴角速度,单位：°/s
				PGyro[j]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[j][i] ;
			}			
		}		
	}

	//陀螺C数据解析
	if (POSE_OK == PGyro[2]->GyroIn_AttVld)
	{
		for (i = 0; i < 3 ; ++i)
		{
			//XYZ轴角速度,单位：°/s
			tmpGyroIn_Speed[i] = ((PoseGetInt32From24(&(tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+3*i+9]))))*POSE_POW(2,-18)*ANG2RADIAN ;
			//XYZ轴角温度，单位：℃
			tmpDtTmper= tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+19]<<8|tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+20];
			PGyro[2]->GyroIn_Tmper[i]  = (float)(tmpDtTmper)*POSE_POW(2,-8);	
		}
		MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[2][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);		
		for (i = 0; i < 3 ; ++i)
		{
			//XYZ轴角速度,单位：°/s
			PGyro[2]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[2][i] ;
		}
	}	

    //陀螺数据判断
    for (i = 0; i < GYRO_NUM ; ++i)
    {
	    if (POSE_OK == PGyro[i]->GyroIn_AttVld)
	    {   for (k = 0; k < 3 ; ++k)
		    {
				if (POSE_ABSF(PGyro[i]->GyroIn_SpeedPre[k]  - PGyro[i]->GyroIn_Speed[k]) < POSE_ZERO)
			    {
					if (PGyro[i]->Gyro_ComCnt[k] < 60000)
					{
						PGyro[i]->Gyro_ComCnt[k]++;
					}
			    }
			    else
			    {
				    PGyro[i]->Gyro_ComCnt[k] =0;
			    }
				if (POSE_ABSF(PGyro[i]->GyroIn_Speed[k]) > 0.436)
			    {
				    tmpcnt[i][k] = POSE_NO;
			    }
			    else
			    {
				    tmpcnt[i][k] = POSE_OK;
			    }
			    
				if ((PGyro[i]->Gyro_ComCnt[k] < 5)&& (tmpcnt[i][k] == POSE_OK))
			    {
				    PGyro[i]->Gyro_ComTHUse[k] = POSE_OK;
			    }
			    else
			    {
				    PGyro[i]->Gyro_ComTHUse[k] = POSE_NO;
					PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 3+k, 1 ); 
			    }
		    }		
	    }
		else
		{
			PGyro[i]->Gyro_ComTHUse[0] = POSE_NO;
			PGyro[i]->Gyro_ComTHUse[1] = POSE_NO;
			PGyro[i]->Gyro_ComTHUse[2] = POSE_NO;
		}
		
	    if ((POSE_NO == PGyro[i]->Gyro_ComTHUse[0])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[1])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[2]))
	    {
		    PGyro[i]->GyroIn_AttVld= POSE_NO;
	    }
    }		
}

/***********************************************
说明：陀螺数据处理
***********************************************/
void Gyro_DataPre(void)
{
	TYPE_CAL tmpRbs[9]= {0.0};
	TYPE_CAL tmpRbi[3]= {0.0,0.0,0.0};//陀螺输出角速度XYZ
	UINT8 i = 0;
	UINT8 k = 0;
	UINT8 tmpGyro = POSE_NO;
	UINT8 tmpComTHUse[3] = {0,0,0};
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	GyroPara_t *PGyro[GYRO_NUM] = {NULL};	
	AttIMPTPara_t 	*pIMU = NULL;

	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt)||(NULL==tmpConst))
	{
		return;		
	}

	pIMU=&tmpAtt->sIMPTPara;
	
	PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
	PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
	PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];

	if ((0x55 == PGyro[0]->GyroIn_AttVld) || (0x55 == PGyro[1]->GyroIn_AttVld) || (0x55 == PGyro[2]->GyroIn_AttVld))
	{
		for (i = 0; i < GYRO_NUM ; ++i)
		{
			tmpGyro = POSE_NO;
			if (POSE_OK == PGyro[i]->GyroIn_AttVld)
			{
				for (k = 0; k < 3 ; ++k)
				{
					tmpRbi[k] = PGyro[i]->GyroIn_Speed[k];
				}
				tmpGyro = POSE_OK;
			}
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_GYRO[i][0][0]), 9 * sizeof(TYPE_CAL));

			if (tmpGyro == POSE_OK)
			{
				MatrixProductHL(tmpRbs, tmpRbi, PGyro[i]->Gyro_Wi, 3, 3, 1);
			}
		}			
	} 
	
	if((POSE_NO == pIMU->GyroUseState[0]) && (POSE_NO == pIMU->GyroUseState[1]) && (POSE_NO == pIMU->GyroUseState[2])) 
	{
		if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
		{
			tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][0];
			tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][0];
			tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][0];
			tmpRbi[0] = PGyro[0]->GyroIn_Speed[0];
			tmpComTHUse[0] 	= 0x55;
		}
		else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
		{
			tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][0];
			tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][0];
			tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][0];
			tmpRbi[0] = PGyro[1]->GyroIn_Speed[0];
			tmpComTHUse[0] 	= 0x55;			
		}
		else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
		{
			tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][0];
			tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][0];
			tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][0];
			tmpRbi[0] = PGyro[2]->GyroIn_Speed[0];
			tmpComTHUse[0] 	= 0x55;			
		}

		if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
		{
			tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][1];
			tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][1];
			tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][1];
			tmpRbi[1] = PGyro[0]->GyroIn_Speed[1];
			tmpComTHUse[1] 	= 0x55;
		}
		else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
		{
			tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][1];
			tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][1];
			tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][1];
			tmpRbi[1] = PGyro[1]->GyroIn_Speed[1];
			tmpComTHUse[1] 	= 0x55;
		}
		else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
		{
			tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][1];
			tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][1];
			tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][1];
			tmpRbi[1] = PGyro[2]->GyroIn_Speed[1];
			tmpComTHUse[1] 	= 0x55;
		}
				
		if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
		{
			tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][2];
			tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][2];
			tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][2];
			tmpRbi[2] = PGyro[0]->GyroIn_Speed[2];
			tmpComTHUse[2] 	= 0x55;
		}
		else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
		{
			tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][2];
			tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][2];
			tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][2];
			tmpRbi[2] = PGyro[1]->GyroIn_Speed[2];
			tmpComTHUse[2] 	= 0x55;
		}
		else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
		{
			tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][2];
			tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][2];
			tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][2];
			tmpRbi[2] = PGyro[2]->GyroIn_Speed[2];
			tmpComTHUse[2] 	= 0x55;
		}
		
		if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]) )
		{
			tmpAtt->sPerPara.GyroIn_Comb = POSE_OK;
			MatrixProductHL(tmpRbs, tmpRbi, tmpAtt->sPerPara.GyroIn_CombWi, 3, 3, 1);	
		} 
		else
		{
			tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;
		}
	}
}


/***********************************************
说明：获取模拟太敏数据
***********************************************/
void Ass_DataGet(void )
{
	UINT32 i = 0;
	UINT32 j = 0;
	INT16 tmpDtInt = 0;
	INT16 *pAss16 = NULL;
	sDevInfo * tmpDevInfo = NULL;
	sAttPriData *tmpAtt = NULL;
	AssPara_t *pAss = NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL==tmpAtt)
	{
		return;		
	}

	pAss = &tmpAtt->sPerPara.AssPara;

	//UINT32 ID[6] = {97,113,101,117,121,125};	
	UINT32 ID[6] = {SA1_TM_XP1-1, SA9_TM_YN1-1,SA5_TM_XN1-1,SA13_TM_YP1-1,SA17_TM_ZN1-1,SA21_TM_P1-1};	
	
	//获取AD采集数据
	tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
	pAss16 =  (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
	//模拟太敏有效性判断
	if (INFO_SIG_CHANGE ==tmpDevInfo->cDevYCBuffer[0])
	{
		for (i= 0; i < ASS_NUM; i++)
		{
			for (j= 0; j < 4; j++)
			{
				//获取模拟太敏6*4电池片电压
				tmpDtInt = *(pAss16 + ID[i] +j);				
				pAss->AssIn_sensor[i][j] = (((TYPE_CAL)(tmpDtInt))*10/4095.0-5.0)/966.0*1000.0 ;
			}		
		}	
		pAss->ADIn_Date = POSE_OK;
	}
	else
	{
		pAss->ADIn_Date = POSE_NO;		
	}	
}


/***********************************************
说明：太敏数据处理
***********************************************/
void Ass_DataPre(void)
{
	UINT32		i = 0;
	UINT32		j = 0;
	UINT32		k = 0;
	UINT32		iMax = 0;
	UINT8		tempAssUseState[ASS_NUM]= {0,0,0,0,0,0};;             //单个太敏可用标志
	TYPE_CAL 	tmpSumMax = 0.0;
	TYPE_CAL    tmpAss_data_SUM[ASS_NUM] = {0.0,0.0,0.0,0.0,0.0,0.0};
	TYPE_CAL 	tmpU[4] = {0.0,0.0,0.0,0.0};
	TYPE_CAL	tmpRbs[9]= {0.0};
	TYPE_CAL 	tmpSm[3]= {0.0};
	TYPE_CAL 	tmpXs = 0.0;
	TYPE_CAL 	tmpYs = 0.0;
	TYPE_CAL    tmpX = 1.0;
	TYPE_CAL    tmpY = 1.0;
	TYPE_CAL	tmpa=5.0;//电池片宽度
	TYPE_CAL	tmpH=1.95;//光栅距离电池面距离
	TYPE_CAL	tmpTheta = 0.0;
	TYPE_CAL	tmpGamma = 0.0;

	sAttPriData *tmpAtt = NULL; 
	AttCtrlConst_t *tmpConst = NULL; 
	AssPara_t *pAss = NULL; 
	AttIMPTPara_t *pImpt = NULL; 
	sAttModPara_t *pWMod = NULL; 
				
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
	{
		return;		
	}

	pAss = &tmpAtt->sPerPara.AssPara;
	pImpt=&tmpAtt->sIMPTPara;
	pWMod=&tmpAtt->sModePara;	
	
	if (POSE_OK == pAss->ADIn_Date)
	{
		for (i= 0; i < 4; i++)
		{
			for (j= 0; j < 4; j++)
			{
				tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
			}
			if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[0] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[0]))
			{
				pAss->AssIn_AttVld[i] = POSE_OK;
			}
			else
			{
				pAss->AssIn_AttVld[i] = POSE_NO;	
			}
		}
		for (i= 4; i < ASS_NUM; i++)
		{
			for (j= 0; j < 4; j++)
			{
				tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
			}
			if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[1] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[1]))
			{
				pAss->AssIn_AttVld[i] = POSE_OK;
			}
			else
			{
				pAss->AssIn_AttVld[i] = POSE_NO;
			}
		}
	}
	
	if ((0x55 == pWMod->WModSadaOpen)&& (0x55 != pWMod->WModSadaOpenPre))
	{
		pImpt->AssUseState[4] = POSE_NO;	
	} 
	
	//选择当前使用的模拟太敏，如果均不可用，则没有太敏可用于计算太阳矢量，输出模拟太敏矢量无效
	for (k= 0; k < ASS_NUM -1; k++)
	{
		if ((POSE_OK == pAss->AssIn_AttVld[k])&& (POSE_OK == pImpt->AssUseState[k]))
		{
			if(tmpAss_data_SUM[k]>tmpSumMax)
			{
				tmpSumMax=tmpAss_data_SUM[k];
				iMax=k;
			}
			tempAssUseState[k] = 0x55;
		}
	}

	if((0 == tempAssUseState[0])&&(0 == tempAssUseState[1])&&(0 == tempAssUseState[2])&&(0 == tempAssUseState[3])&&(0 == tempAssUseState[4]))//太敏均无效
	{
		pAss->Ass_SunVecVld = POSE_NO;
	}
	else
	{

		if(0 == iMax)
		{
			memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSA[0][0]), 9 * sizeof(TYPE_CAL));
		}
		else if(1 == iMax)
		{
			memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSB[0][0]), 9 * sizeof(TYPE_CAL));
		}
		else if(2 == iMax)
		{
			memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSC[0][0]), 9 * sizeof(TYPE_CAL));
		}
		else if(3 == iMax)
		{
			memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSD[0][0]), 9 * sizeof(TYPE_CAL));
		}
		else if(4 == iMax)
		{
			memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
			memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSE[0][0]), 9 * sizeof(TYPE_CAL));
		}

		if ((iMax>=0)&&(iMax<4))
		{
			tmpH = 1.95;
		}
		else 
		{
			tmpH = 4.0;
		}

		tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
		if(tmpXs!=0)
		{
			tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
		}
	
		tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
		if(tmpYs!=0)
		{
			tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
		}

		//根据坐标值求出太阳矢量方位角θ、虚余仰角r
		tmpTheta = POSE_ATAN2F(tmpY, tmpX);
		tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);
		tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
		tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
		tmpSm[2] = POSE_COSF(tmpGamma);
		MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecb, 3, 3, 1);
		pAss->Ass_SunVecVld= POSE_OK;
		
	}


	//计算帆板模拟太敏方位角θ、虚余仰角r
	if ((POSE_OK == pAss->AssIn_AttVld[5])&& (POSE_OK == pImpt->AssUseState[5]))
	{
		tmpH = 4.0;
		memcpy(tmpU, &(pAss->AssIn_sensor[5][0]), 4*sizeof(TYPE_CAL));
		memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSF[0][0]), 9 * sizeof(TYPE_CAL));
		tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
		if(tmpXs!=0)
		{
			tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
		}
	
		tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
		if(tmpYs!=0)
		{
			tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
		}
		//根据坐标值求出太阳矢量方位角θ、虚余仰角r
		tmpTheta = POSE_ATAN2F(tmpY, tmpX);
		tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);	
		pAss->Ass_SunATT[0] = tmpTheta;
		pAss->Ass_SunATT[1] = tmpGamma;
		tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
		tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
		tmpSm[2] = POSE_COSF(tmpGamma);
		MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecbF, 3, 3, 1);
		pAss->Ass_SunOK = POSE_OK;		
	}
	else
	{
		pAss->Ass_SunOK = POSE_NO;	
	}
}

/***********************************************
说明：获取磁强计数据
***********************************************/
void Mag_DataGet(void)
{
	TYPE_CAL	tmpMag_Bb_Limit = 55000.0;                          //sjd20231123

	INT16 tmpDtInt = 0;
	INT16 *pMag16 = NULL;
	UINT32 i = 0;
	UINT32 j = 0;
	TYPE_CAL tmpVm[MAG_NUM][3] = {{0,0,0},{0,0,0}};
	UINT32 ID = 2;
	ID = SV3_CQJA_X -1 ;
		
	sDevInfo * tmpDevInfo = NULL;
	sAttPriData *tmpAtt= NULL;
	AttCtrlConst_t *tmpConst= NULL;
	MagPara_t *pMag= NULL;	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
	return;
	pMag = &tmpAtt->sPerPara.MagPara;

	//获取AD采集数据
	tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
	pMag16 = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
	
	//模拟磁强计的补偿矩阵计算
	if (INFO_SIG_CHANGE == tmpDevInfo -> cDevYCBuffer[0])
	{
		for (i = 0; i < MAG_NUM; ++i)
		{
			//计算Vm
			for (j = 0; j< 3; j++)
			{
				tmpDtInt = *(pMag16 + ID +j + 3*i);
				tmpVm[i][j] = tmpConst->AttCmdFlashPara.MAG_LK[i][j] * (((float)(tmpDtInt))*10/4095 - 5 ) + tmpConst->AttCmdFlashPara.MAG_LB[i][j];    //MAG_LK   24000
			}
		}
		if ((POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit))
		{	
			if (POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)
			{
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 1, 1 );  //磁强计A测量值超阈值
			}
			if (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)
			{
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 2, 1 );  //磁强计A测量值超阈值
			}
			if (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit)
			{
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 3, 1 );  //磁强计A测量值超阈值
			}
			pMag->MagIn_Valid[0] = POSE_NO;
		}
		else
		{
			memcpy(pMag->Mag_Bc_MeaAB[0], tmpVm[0], 3*sizeof(TYPE_CAL));
			pMag->MagIn_Valid[0] = POSE_OK;
		}
		
		if ((POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit) || (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)||(POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit))
		{
			if (POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit)
			{
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 4, 1 );  //磁强计A测量值超阈值
			}
			if (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)
			{			
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 5, 1 );  //磁强计A测量值超阈值
			}
			if (POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit)
			{			
				pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 6, 1 );  //磁强计A测量值超阈值
			}

			pMag->MagIn_Valid[1] = POSE_NO;
		}
		else
		{
			memcpy(pMag->Mag_Bc_MeaAB[1], tmpVm[1], 3*sizeof(TYPE_CAL));
			pMag->MagIn_Valid[1] = POSE_OK;
		}
	}
	else
	{
		pMag->MagIn_DataSta  = ZERO_TO_ONE( pMag->MagIn_DataSta, 0, 1 );   //数据不更新
		pMag->MagIn_Valid[0] = POSE_NO;
		pMag->MagIn_Valid[1] = POSE_NO;
	}		
}


/***********************************************
说明：磁强计数据处理
***********************************************/
void Mag_DataPre(void)
{
	UINT8 i = 0;
	TYPE_CAL	tmKm[3][3] = {{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0}};
	TYPE_CAL	tmpMag_Bc_MeaAB[3]= {0.0,0.0,0.0};
	UINT8 tmpComTHUse[3] = {0,0,0};
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	MagPara_t *pMag = NULL;
	AttIMPTPara_t 	*pIMU = NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{
		return;		
	}

	pMag = &tmpAtt->sPerPara.MagPara;
	pIMU=&tmpAtt->sIMPTPara;
		
	//测量系磁强计A磁矢量
	if (POSE_OK == pMag->MagIn_Valid[0])
	{
		memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[0], 9 * sizeof(TYPE_CAL));
		MatrixPlusVectorN( tmKm[0],  pMag->Mag_Bc_MeaAB[0], pMag->Mag_Bb_MeaAB[0], 3, 3 );
	}
	if (POSE_OK ==pMag->MagIn_Valid[1])
	{
		memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[1], 9 * sizeof(TYPE_CAL));
		MatrixPlusVectorN( tmKm[0],  pMag->Mag_Bc_MeaAB[1], pMag->Mag_Bb_MeaAB[1], 3, 3 );
	}

	if ((POSE_NO == pIMU->MagUseState[0])&&(POSE_NO == pIMU->MagUseState[1]))
	{
		for (i = 0; i < 3; ++i)
		{
			if (0x11 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
			{
				tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[0][0][i];
				tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[0][1][i];
				tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[0][2][i];
				tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[0][i];
				tmpComTHUse[i] = 0x55;
			}
			else if (0x22 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
			{
				tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[1][0][i];
				tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[1][1][i];
				tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[1][2][i];
				tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[1][i];
				tmpComTHUse[i] = 0x55;		
			}

		}
		if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]))
		{
			MatrixPlusVectorN( tmKm[0],  tmpMag_Bc_MeaAB, pMag->Mag_Bb_MeaZH, 3, 3 );
			pMag->MagIn_Valid[2] = POSE_OK;				
		}
		else
		{
			pMag->MagIn_Valid[2] = POSE_NO;		
		}	
	}
}

/***********************************************
说明：获取飞轮数据
***********************************************/
void Whl_DataGet(void)
{

	UINT8 sum= 0;
	UINT32 i = 0;
	UINT32 j = 0;
	UINT16 tmpU16=0;
	INT16  tmpI16=0;
	UINT8 tmpWhlHEAD = 0;
	UINT8 tmpWhlHEADP = 0;
	UINT8 tmpWhlHEADID[4]={0};
	UINT16 tmpWhlEnd = 0;
	sDevInfo * tmpDevInfo = NULL;
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	WhlPara_t *PWhl = NULL;			
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{
		return;			
	}

	PWhl = &tmpAtt->sPerPara.WhlPara;	
	
    tmpWhlHEADID[0] = tmpConst->Whl_ID[0];
	tmpWhlHEADID[1] = tmpConst->Whl_ID[1];
	tmpWhlHEADID[2] = tmpConst->Whl_ID[2];
	tmpWhlHEADID[3] = tmpConst->Whl_ID[3];
	
	//记录数据
	memcpy(PWhl->Whl_RatePre,PWhl->Whl_Rate,4*sizeof(TYPE_CAL));
	memcpy(PWhl->Whl_TimePRE,PWhl->Whl_Time,4*sizeof(UINT32));

	for (i = 0; i < (WHEEL_NUM); ++i)
	{
		tmpDevInfo = GETDEVINFO(i+ DEV_NO_UART8) ;
		tmpWhlHEAD = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF];
		tmpWhlHEADP = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
		tmpWhlEnd = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+32]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+33];

		/*飞轮数据更新*/
		if  (INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
		{
			sum = 0;
			for(j = 2; j < WHL_DATA_LEN -1; j++)
			{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF  +j];}
			sum = 0xFF - sum;
				
			if((WHL_DATA_END != tmpWhlEnd)||(WHL_CMD_CODE_REQ!=tmpWhlHEADP)||(tmpWhlHEADID[i]!=tmpWhlHEAD))
			{
					PWhl->WhlIn_Valid[i] = POSE_NO;
					PWhl->WhlIn_DataSta[i]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 1, 1 ); /* errNo=0,飞轮包头|包长错误   */	
			}
			else
			{
				
				if (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+ WHL_DATA_LEN  -1]!=sum)
				{
					PWhl->WhlIn_Valid[i] = POSE_NO;
					PWhl->WhlIn_DataSta[i]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 2, 1 ); /* errNo=0,飞轮包头|包长错误   */	
				} 
				else
				{
					PWhl->Whl_Time[i]=DoGetInt32From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +2]));
					tmpI16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+22]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+23];
					tmpU16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+25];
					PWhl->Whl_Rate[i] = DoGetFloatFrom8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+10]))*RADS2RPM;
					PWhl->Whl_Current[i]  = tmpI16;
					PWhl->Whl_Tmper[i]  = 0.4863 * (TYPE_CAL)(tmpU16) - 273.15	; 	
					PWhl->WhlIn_Valid[i] = POSE_OK;
				}				
			}					
		}
		else
		{
			PWhl->WhlIn_Valid[i]	= POSE_NO;
			PWhl->WhlIn_DataSta[i]  = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 0, 1 ); /* errNo=0,飞轮转速不更新  */
		}
	}	
}


/***********************************************
说明：飞轮数据处理
***********************************************/
void  Whl_DataPre(void)
{
	UINT8 i = 0;
	sAttPriData *tmpAtt = NULL;
	WhlPara_t *PWhl = NULL;

	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	{
		return;		
	}

	PWhl = &tmpAtt->sPerPara.WhlPara;

	for (i = 0; i < WHEEL_NUM; ++i)
	{
		if (POSE_OK == PWhl->WhlIn_Valid[i])
		{	
			if (POSE_ABSF( PWhl->Whl_Rate[i]- PWhl->Whl_RatePre[i] ) > 100*RADS2RPM)  //200t230
			{
				PWhl->WhlIn_DataSta[i]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 4, 1 ); 
				PWhl->WhlIn_Valid[i] = POSE_NO;
			}
			else
			{
				PWhl->WhlIn_Valid[i] = POSE_OK;
			}
		}
	}
}


/***********************************************
说明：霍尔电推数据处理
***********************************************/
void  PPU_DataPre(void)
{
	UINT16	tmpPPUHEAD = 0;
	UINT8	tmpPPSUM = 0;
	UINT8	sum = 0;
	UINT8	i = 0;
	UINT32 TempSatTime[2] ={0,0};
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;	
	sDevInfo * tmpDevInfo = NULL;
	PPUPara_t *PPPU = NULL;	
	sAttOrbitCtlInfo_t *pOrb = NULL;
	sAttModPara_t *pWMod= NULL;	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{
		return;		
	}

	pWMod=&tmpAtt->sModePara;	
	PPPU = &tmpAtt->sPerPara.PPUPara;
	pOrb=&tmpAtt->sOrbitInp;	
	
	tmpDevInfo = GETDEVINFO(DEV_NO_CAN0) ;
	tmpPPUHEAD = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
	tmpPPSUM = 	tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF + PPU_CMD_LEN +1];
	
	/*获取星上时*/
	GetTime(TempSatTime);
	
	/*PPU单机数据更新判断*/
	if(INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
	{
		for(i = 0; i < PPU_CMD_LEN+1 ; i++)
		{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +i];}
		/*PPU数据有效判断*/
		if((PPU_CMD_LEN == tmpPPUHEAD )&&(sum == tmpPPSUM))
		{
			/*PPU数据有效更新上一拍、当前拍的请求计数（单机包中返回请求计数）*/
			PPPU->PPUIn_ReCntPre = PPPU->PPUIn_ReCnt;
			PPPU->PPUIn_ReCnt= tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+3];
			
			/*PPU数据上一拍、当前拍数据 不更新，则PPU数据无效 ？？？？*/
			if (PPPU->PPUIn_ReCntPre == PPPU->PPUIn_ReCnt)
			{
				PPPU->PPUIn_Valid	= POSE_NO;
				PPPU->PPUIn_DataSta  = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 2, 1 );
			} 
			else
			{
				/*PPU数据上一拍、当前拍数据更新，则PPU数据有效*/
				PPPU->PPUIn_AVoltage=tmpConst->AttCmdFlashPara.PPUPVK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+15])) + tmpConst->AttCmdFlashPara.PPUPVB;				//推进阳极电源电压
				PPPU->PPUIn_ACurrent=tmpConst->AttCmdFlashPara.PPUPAK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+17])) + tmpConst->AttCmdFlashPara.PPUPAB;				//推进阳极电源电流
				
				/*跟新上一拍、当前拍的点火标志（单机数据包中的点火标志）*/
				PPPU->PPUIn_FIREOKPre = PPPU->PPUIn_FIREOK;
				PPPU->PPUIn_FIREOK = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24];	//点火成功标志
				PPPU->PPUIn_Valid	= POSE_OK;
			}
		}
		else
		{
			/*PPU数据校验不通过，设置PPU数据无效*/
			PPPU->PPUIn_Valid	= POSE_NO;
			PPPU->PPUIn_DataSta  = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 1, 1 );
			///*PPU数据校验不通过，设置上一拍、当前拍点火成功标志 为 未点火*/
			//PPPU->PPUIn_FIREOKPre = 0x00;
			//
			///*因PPU单机数据校验不通过，如果上一拍的点火成功，当前拍点火标志清零认为点火结束？？？，记录点火结束时间*/
			//if(0x01 == PPPU->PPUIn_FIREOK)
				//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
			//
			//PPPU->PPUIn_FIREOK = 0x00;
			//
			//return;
		}
	}
	else
	{
		/*PPU单机数据不更新，设置PPU数据无效*/
		PPPU->PPUIn_Valid	= POSE_NO;
		PPPU->PPUIn_DataSta  = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 0, 1 );
		///*PPU数据校验不通过，设置上一拍、当前拍点火成功标志 为 未点火*/
		//PPPU->PPUIn_FIREOKPre = 0x00;
		//
		///*因PPU单机数据未更新，如果上一拍的点火成功，当前拍点火标志清零认为点火结束？？？，记录点火结束时间*/
		//if(0x01 == PPPU->PPUIn_FIREOK)
			//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
		//
		//PPPU->PPUIn_FIREOK = 0x00;
//
		//return;
	}
	
	/*PPU点火成功 总次数 不清除 一直记录*/
	if(0x01 == PPPU->PPUIn_FIREOK)
	{
		if (PPPU->PPUT_FIR_Sum< 0xFFFFFFFE)
		{
			PPPU->PPUT_FIR_Sum++ ;
		}
		else
		{
			PPPU->PPUT_FIR_Sum= 0;
		}
	}

	/*PPU从 点火 到 未点火，数据记录*/
	if ((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
	{
		if(PPPU->PPUT_FIR_Cnt>0)
		PPPU->PPUT_FIR_Cnt--;		
	}

	/*PPU从未点火 到 点火，数据记录*/
	if ((0x01 != PPPU->PPUIn_FIREOKPre)&&(0x01 == PPPU->PPUIn_FIREOK))
	{
		//if(PPPU->PPUT_FIR_Cnt>0)
			//PPPU->PPUT_FIR_Cnt--;
			
		if(PPPU->PPUT_FIR_SumCnt < 60000)
		{
			PPPU->PPUT_FIR_SumCnt++;
		}
		else
		{
			PPPU->PPUT_FIR_SumCnt = 0;
		}
		
		if((pOrb->CurOrbCtrlPcak_ID >= 0) &&(pOrb->CurOrbCtrlPcak_ID< 50))
		{
			PPPU->PPUT_FIR_Start[pOrb->CurOrbCtrlPcak_ID] = TempSatTime[0];
		}
	}/*PPU从点火 到 未点火，数据记录*/
	else if((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
	{
		tmpConst->AttCmdFlashPara.PPUT_FIR_EndLast = TempSatTime[0] ;
		pWMod->ATTFlashSwitch = POSE_OK;
		
		if(((pOrb->CurOrbCtrlPcak_ID  >= 1) &&(pOrb->CurOrbCtrlPcak_ID <= 50)))
		{
			PPPU->PPUT_FIR_End[pOrb->CurOrbCtrlPcak_ID - 1] = TempSatTime[0];
		}
		else if(0==pOrb->CurOrbCtrlPcak_ID)
		{
			if((PPPU->PPUT_FIR_CntS >= 1) &&(PPPU->PPUT_FIR_CntS <= 50))
			{
				PPPU->PPUT_FIR_End[PPPU->PPUT_FIR_CntS - 1] = TempSatTime[0];
				PPPU->PPUT_FIR_CntS =0;
			}
		}
	}
	else
	{;} /*不做处理*/
}

/***********************************************
说明：星敏互校验
***********************************************/
void SSABC_CC(void)
{
	UINT8 		i=0;
	UINT8 		tmpCheckCnt=0;
	TYPE_CAL	tmpLb[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
	TYPE_CAL	tmpLi[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
	TYPE_CAL	tmpRbs[3][9]={
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
							};
	TYPE_CAL	tmpAab_b = 0.0;
	TYPE_CAL	tmpAbc_b = 0.0;
	TYPE_CAL	tmpAac_b = 0.0;
	TYPE_CAL	tmpAab_i = 0.0;
	TYPE_CAL	tmpAbc_i= 0.0;
	TYPE_CAL	tmpAac_i= 0.0;
	TYPE_CAL	tmpRsi[3][9]={
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
							};
	TYPE_CAL	tmpRis[3][9]={
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
								{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
							};
	UINT8		Valid[3]={0};
	UINT8		ssA_CCFlg=0;
	UINT8		ssB_CCFlg=0;
	UINT8		ssC_CCFlg=0;
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	SsPara_t *ssA = NULL;
	SsPara_t *ssB = NULL;
	SsPara_t *ssC = NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) ||( NULL==tmpConst))
	{
		return;		
	}
	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];
		
	Valid[0]=ssA->ssIn_AttVld;
	Valid[1]=ssB->ssIn_AttVld;
	Valid[2]=ssC->ssIn_AttVld;
	
	memcpy(tmpRbs[0], &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9 * sizeof(TYPE_CAL));
	memcpy(tmpRbs[1], &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9 * sizeof(TYPE_CAL));
	memcpy(tmpRbs[2], &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9 * sizeof(TYPE_CAL));
	
	//计算星敏本体系光轴矢量
	for(i=0;i<3;i++)
	{
		tmpLb[i][0]=tmpRbs[i][2];
		tmpLb[i][1]=tmpRbs[i][5];
		tmpLb[i][2]=tmpRbs[i][8];
	}
	//计算星敏本体系光轴矢量夹角
	CalVecArc(tmpLb[0], tmpLb[1], &tmpAab_b);
	CalVecArc(tmpLb[0], tmpLb[2], &tmpAac_b);
	CalVecArc(tmpLb[1], tmpLb[2], &tmpAbc_b);
	
	//计算惯性系光轴矢量
	Q2Att(ssA->ssIn_Q, tmpRsi[0]);
	Q2Att(ssB->ssIn_Q, tmpRsi[1]);
	Q2Att(ssC->ssIn_Q, tmpRsi[2]);
	for(i=0;i<3;i++)
	{
		MatrixTransposeHL(tmpRsi[i], tmpRis[i],3, 3);
		tmpLi[i][0]=tmpRis[i][2];
		tmpLi[i][1]=tmpRis[i][5];
		tmpLi[i][2]=tmpRis[i][8];
	}
	//计算惯性系光轴矢量夹角
	CalVecArc(tmpLi[0], tmpLi[1], &tmpAab_i);
	CalVecArc(tmpLi[0], tmpLi[2], &tmpAac_i);
	CalVecArc(tmpLi[1], tmpLi[2], &tmpAbc_i);
	
	if (0x55 == Valid[0])
	{
		ssA_CCFlg=CC_UNCERTAIN;
	}
	else
	{
		ssA_CCFlg=NO_CC;		
	}	

	if (0x55 == Valid[1])
	{
		ssB_CCFlg=CC_UNCERTAIN;
	}
	else
	{
		ssB_CCFlg=NO_CC;
	}	
	if (0x55 == Valid[2])
	{
		ssC_CCFlg=CC_UNCERTAIN;
	}
	else
	{
		ssC_CCFlg=NO_CC;
	}
		
	if((0x55 == Valid[0])&&(0x55 == Valid[1]))
	{
		if(POSE_ABSF(tmpAab_i-tmpAab_b)<(tmpConst->SS_SS_CThr))
		{
			ssA_CCFlg=CC_CORRECT;
			ssB_CCFlg=CC_CORRECT;
			tmpCheckCnt++;
		}
	}
	
	if((0x55 == Valid[0])&&(0x55 == Valid[2]))
	{
		if(POSE_ABSF(tmpAac_i-tmpAac_b)<(tmpConst->SS_SS_CThr))
		{
			ssA_CCFlg=CC_CORRECT;
			ssC_CCFlg=CC_CORRECT;
			tmpCheckCnt++;
		}
	}
	
	if((0x55 == Valid[1])&&(0x55 == Valid[2]))
	{
		if(POSE_ABSF(tmpAbc_i-tmpAbc_b)<(tmpConst->SS_SS_CThr))
		{
			ssB_CCFlg=CC_CORRECT;
			ssC_CCFlg=CC_CORRECT;
			tmpCheckCnt++;
		}
	}

	if(tmpCheckCnt>0)//存在校验正确的星敏
	{
		if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
		if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
		if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
	}
	ssA->ssCrCheckFlg=ssA_CCFlg;
	ssB->ssCrCheckFlg=ssB_CCFlg;
	ssC->ssCrCheckFlg=ssC_CCFlg;
}


/***********************************************
说明：星敏+太敏互校验
***********************************************/
void StarSun_CC(void)
{
	UINT8 i,tmpCheckCnt=0;
	UINT8		ssA_CCFlg, ssB_CCFlg, ssC_CCFlg, Sun_CCFlg;
	TYPE_CAL	tmpRbi[3][9];
	TYPE_CAL	tmpSb[3][3];
	TYPE_CAL	tmpAam,tmpAbm,tmpAcm;
	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;
	SsPara_t *ssA, *ssB, *ssC;
	AssPara_t *pASS;
	sAttOrbitGetPara_t *pOrbit = NULL;
			
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
		return;

	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];
	pASS=&tmpAtt->sPerPara.AssPara;
	pOrbit = &tmpAtt->sOrbitPara;	
	
	//校验条件判断（这里应该不判断也可）
	if((CC_CORRECT == ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT == ssC->ssCrCheckFlg))
		return;
	if(0x00 == pASS->Ass_SunVecVld)
		return;
	if((0x00 == ssA->ssIn_AttVld)&&(0x00 == ssB->ssIn_AttVld)&&(0x00 == ssC->ssIn_AttVld))
		return;
	if(POSE_NO == pOrbit->MJCLv)
	{
		return;
	}
	
	ssA_CCFlg=CC_UNCERTAIN;
	ssB_CCFlg=CC_UNCERTAIN;
	ssC_CCFlg=CC_UNCERTAIN;
	Sun_CCFlg=CC_UNCERTAIN;
	
	//计算本体系太阳矢量
	Q2Att(ssA->ssExpose_Qbi, tmpRbi[0]);
	Q2Att(ssB->ssExpose_Qbi, tmpRbi[1]);
	Q2Att(ssC->ssExpose_Qbi, tmpRbi[2]);
	for(i=0;i<3;i++)
		MatrixProductHL(tmpRbi[i], tmpAtt->sEnvPara.SunVecI, tmpSb[i], 3, 3, 1);
	
	//计算本体系下星敏太阳矢量和太敏太阳矢量夹角
	CalVecArc(tmpSb[0], pASS->Ass_SunVecb, &tmpAam);
	CalVecArc(tmpSb[1], pASS->Ass_SunVecb, &tmpAbm);
	CalVecArc(tmpSb[2], pASS->Ass_SunVecb, &tmpAcm);
	
	if((0x55 == ssA->ssIn_AttVld)&&(tmpAam<tmpConst->SS_ASS_CCThr))
	{
		ssA_CCFlg=CC_CORRECT;
		Sun_CCFlg=CC_CORRECT;
		tmpCheckCnt++;
	}
	
	if((0x55 == ssB->ssIn_AttVld)&&(tmpAbm<tmpConst->SS_ASS_CCThr))
	{
		ssB_CCFlg=CC_CORRECT;
		Sun_CCFlg=CC_CORRECT;
		tmpCheckCnt++;
	}
	
	if((0x55 == ssC->ssIn_AttVld)&&(tmpAcm<tmpConst->SS_ASS_CCThr))
	{
		ssC_CCFlg=CC_CORRECT;
		Sun_CCFlg=CC_CORRECT;
		tmpCheckCnt++;
	}
	
	if(tmpCheckCnt>0)
	{
		if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
		if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
		if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
	}
	else
		Sun_CCFlg=CC_UNCERTAIN;
	
	ssA->ssCrCheckFlg=ssA_CCFlg;
	ssB->ssCrCheckFlg=ssB_CCFlg;
	ssC->ssCrCheckFlg=ssC_CCFlg;
	pASS->AssCrCheckFlg=Sun_CCFlg;
}

/***********************************************
说明：星敏校太敏
***********************************************/
void StarCheckSun(void)
{
	TYPE_CAL	tmpQbi[4];
	TYPE_CAL	tmpRbi[9];
	TYPE_CAL	tmpSb[3];
	TYPE_CAL	tmpAsm;
	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;	
	SsPara_t *ssA, *ssB, *ssC;
	AssPara_t *pASS;	

	sAttOrbitGetPara_t *pOrbit = NULL;

	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	
	if(NULL==tmpAtt || NULL==tmpConst)
		return;
		
	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];
	pASS=&tmpAtt->sPerPara.AssPara;
	pOrbit = &tmpAtt->sOrbitPara;

	if((0x00 == pASS->Ass_SunVecVld)||(POSE_NO == pOrbit->MJCLv))
	{
		pASS->AssCrCheckFlg=NO_CC;
		return;
	}

	//利用星敏计算本体系太阳矢量
	if(CC_CORRECT == ssA->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else if(CC_CORRECT == ssB->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else if(CC_CORRECT == ssC->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else
	{
		return;
	}
		
	Q2Att(tmpQbi, tmpRbi);
	MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, tmpSb, 3, 3, 1);
	//判断夹角
	CalVecArc(tmpSb, pASS->Ass_SunVecb, &tmpAsm);
	if(tmpAsm<tmpConst->SS_ASS_CThr)
		pASS->AssCrCheckFlg=CC_CORRECT;
	else
		pASS->AssCrCheckFlg=CC_ERROR;
}


/***********************************************
说明：星敏校陀螺
因为进入的条件是必有正常星敏，那么该星敏一定校验为1且角速度有效，该子函数中不再进行判断
***********************************************/
void StarCheckGyro(void)
{
	UINT8 i, j;
	TYPE_CAL	deltaA[GYRO_NUM][3];//三轴角速度偏差
	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;
	SsPara_t *ssA, *ssB, *ssC;
	GyroPara_t *pGyro[3];
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;				
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
	return;
	
	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];

	pGyro[0]=&tmpAtt->sPerPara.GyroPara[0];
	pGyro[1]=&tmpAtt->sPerPara.GyroPara[1];
	pGyro[2]=&tmpAtt->sPerPara.GyroPara[2];
	
	//校陀螺
	if(CC_CORRECT ==ssA->ssCrCheckFlg)
	{
		for(i=0;i<GYRO_NUM;i++)
		{
			for (j=0;j<3;j++)
			{
				deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssA->ssExpose_Wbi[j]);
			}
		}
	}
	else if(CC_CORRECT ==ssB->ssCrCheckFlg)
	{
		for(i=0;i<GYRO_NUM;i++)
		{
			for (j=0;j<3;j++)
			{
				deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssB->ssExpose_Wbi[j]);
			}
		}		
	}
	else if(CC_CORRECT ==ssC->ssCrCheckFlg)
	{
		for(i=0;i<GYRO_NUM;i++)
		{
			for (j=0;j<3;j++)
			{
				deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssC->ssExpose_Wbi[j]);
			}
		}
	}
	else
	{
		return;
	}

	for(i=0;i<GYRO_NUM; i++)
	{
		if (POSE_NO == pGyro[i]->GyroIn_AttVld)
			pGyro[i]->GyroCrCheckFlg=NO_CC;
		else if((deltaA[i][0]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][1]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][2]<tmpConst->SS_GYRO_CThr))
			pGyro[i]->GyroCrCheckFlg=CC_CORRECT;
		else
			pGyro[i]->GyroCrCheckFlg=CC_ERROR;
	}
}

/***********************************************
说明：星敏校磁强计
***********************************************/
void StarCheckMag(void)
{
	UINT8 i;
	TYPE_CAL	deltaA[MAG_NUM];
	TYPE_CAL	tmpQbi[4];
	TYPE_CAL	tmpRbi[9];
	TYPE_CAL	tmpSb[3];

	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;
	SsPara_t *ssA, *ssB, *ssC;
	MagPara_t *pMag;
	sAttEnvPara_t *pEnv;
	sAttOrbitGetPara_t *pObtIn;
					
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
		return;
	
	pObtIn = &tmpAtt->sOrbitPara;
	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];
	pMag=&tmpAtt->sPerPara.MagPara;
	pEnv=&tmpAtt->sEnvPara;	

	if(0x00 == pObtIn->OrbitVld)
	{
		pMag->MagCrCheckFlg[0]=NO_CC;
		pMag->MagCrCheckFlg[1]=NO_CC;
		return;
	}

	//利用星敏计算本体系太阳矢量
	if(CC_CORRECT == ssA->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else if(CC_CORRECT == ssB->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else if(CC_CORRECT == ssC->ssCrCheckFlg)
	{
		memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
	}
	else
	{
		return;
	}
		
	Q2Att(tmpQbi, tmpRbi);
	MatrixProductHL(tmpRbi, pEnv->fvBi, tmpSb, 3, 3, 1);

	//校磁强计
	for(i=0;i<MAG_NUM;i++)
	{
		if(POSE_OK == pMag->MagIn_Valid[i])
		{
			CalVecArc(tmpSb, pMag->Mag_Bb_MeaAB[i], &deltaA[i]);

			if(deltaA[i]<tmpConst->AttCmdFlashPara.SS_MAG_CThr)
			{
				pMag->MagCrCheckFlg[i]=CC_CORRECT;				
			}
			else
			{
				pMag->MagCrCheckFlg[i]=CC_ERROR;					
			}
		}
		else
		{
			pMag->MagCrCheckFlg[i]=NO_CC;
		}
	}
}

/***********************************************
说明：陀螺互校验
***********************************************/
void Gyro_CC(void)
{
	UINT8 	i=0;
	UINT8	Valid[3];
	UINT8   tmpCnt = 0;
	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;
	GyroPara_t *pGyro[3];
	
	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];
	
	for(i=0;i<GYRO_NUM;i++)
	{
		pGyro[i]->GyroCrCheckFlg=NO_CC;
		
		Valid[i]=pGyro[i]->GyroIn_AttVld;
		if (0x55 == Valid[i])
		{
			pGyro[i]->GyroCrCheckFlg=CC_UNCERTAIN;
		}
	}
	
	if((0x55 == Valid[0])&&(0x55 ==Valid[1]))
	{
		if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[1]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[1]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[1]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
		)
		{
			pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
			pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
			tmpCnt++;
		}
		else
		{
			;/*不做处理*/
		}
	}
	
	if((0x55 == Valid[0])&&(0x55 == Valid[2]))
	{
		if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
		)
		{
			pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
			pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
			tmpCnt++;
		}
		else
		{
			;/*不做处理*/
		}
	}
	
	if((0x55 == Valid[1])&&(0x55 == Valid[2]))
	{
		if((POSE_ABSF(pGyro[1]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[1]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
		&&(POSE_ABSF(pGyro[1]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
		)
		{
			pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
			pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
			tmpCnt++;
		}
		else
		{
			;/*不做处理*/
		}
	}
	
	for(i=0; i<3; i++)
	{
		if((NO_CC == pGyro[i]->GyroCrCheckFlg)
		||(CC_CORRECT == pGyro[i]->GyroCrCheckFlg)
		)
		continue;
		
		if(tmpCnt > 0)
		pGyro[i]->GyroCrCheckFlg = CC_ERROR;
	}
}


/***********************************************
说明：磁强计互校验
***********************************************/
void Mag_CC(void)
{
	UINT8 i;
	TYPE_CAL	deltaA[3];
	sAttPriData *tmpAtt;
	AttCtrlConst_t *tmpConst;
	MagPara_t *pMag;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL==tmpAtt || NULL==tmpConst)
	return;
	pMag=&tmpAtt->sPerPara.MagPara;

	if((POSE_OK == pMag->MagIn_Valid[0])&&(POSE_OK ==pMag->MagIn_Valid[1]))
	{
		for(i=0;i<3;i++)
		{
			deltaA[i]=POSE_ABSF(pMag->Mag_Bb_MeaAB[0][i]-pMag->Mag_Bb_MeaAB[1][i]);
		}
		if((deltaA[0]<tmpConst->MAG_MAG_CThr)&&(deltaA[1]<tmpConst->MAG_MAG_CThr)&&(deltaA[2]<tmpConst->MAG_MAG_CThr))
		{
			pMag->MagCrCheckFlg[0]=CC_CORRECT;
			pMag->MagCrCheckFlg[1]=CC_CORRECT;
		}
		else
		{
			pMag->MagCrCheckFlg[0]=CC_UNCERTAIN;
			pMag->MagCrCheckFlg[1]=CC_UNCERTAIN;
		}
	}
	else
	{
		pMag->MagCrCheckFlg[0]=NO_CC;
		pMag->MagCrCheckFlg[1]=NO_CC;
	}
}


/***********************************************
说明：单机互校验
***********************************************/
void Att_CrossCheck(void)
{
	UINT8	tmpValidSum;
	sAttPriData *tmpAtt;
	SsPara_t *ssA, *ssB, *ssC;
	AssPara_t *pASS;
		
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL ==tmpAtt)
	return;
	
	ssA=&tmpAtt->sPerPara.SsPara[0];
	ssB=&tmpAtt->sPerPara.SsPara[1];
	ssC=&tmpAtt->sPerPara.SsPara[2];

	pASS=&tmpAtt->sPerPara.AssPara;
		
	tmpValidSum=ssA->ssIn_AttVld+ssB->ssIn_AttVld+ssC->ssIn_AttVld;
	
	if((0xAA ==tmpValidSum)||(0xFF ==tmpValidSum))//2或3个星敏有效
	{
		SSABC_CC();//2、3个星敏间互校验
		if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
		{
			StarCheckSun();//正常星敏校太敏
			StarCheckGyro();//正常星敏校陀螺
			StarCheckMag();//正常星敏校磁强计
		}
		else
		{
			if(0x55 == pASS->Ass_SunVecVld)//太敏有效
			{
				StarSun_CC();//星敏太敏互校验
				if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
				{
					StarCheckGyro();//正常星敏校陀螺
					StarCheckMag();//正常星敏校磁强计					
				}
				else
				{
					Gyro_CC();
					Mag_CC();	
				}
			}
			else
			{
				Gyro_CC();
				Mag_CC();
			}
		}
	}
	else if(0x55 ==tmpValidSum)//单星敏有效
	{
		if(0x55 ==pASS->Ass_SunVecVld)//太敏有效
		{
			StarSun_CC();//星敏太敏互校验
			if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
			{
				StarCheckGyro();//正常星敏校陀螺
				StarCheckMag();//正常星敏校磁强计
			}
			else
			{
				Gyro_CC();
				Mag_CC();
			}
		}
		else
		{
			Gyro_CC();
			Mag_CC();
		}
	}
	else if(0 ==tmpValidSum)//没有有效星敏
	{
		Gyro_CC();
		Mag_CC();
	}	
}

/************************************************
 说明：数据预处理模块每周期调用操作
 输入：		
 输出：
************************************************/
void 	ZKDPAction(void)
{

	AttCtrlConst_t *tmpConst;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL == tmpConst)
	return;
	//初始化
	ZKDPInit();

	//星敏
	Ss_DataGet();
	Ss_DataPre();
	
	//陀螺
	Gyro_DataGet();
	Gyro_DataPre();
	
	//模拟太敏
	Ass_DataGet();
	Ass_DataPre();
	
	//磁强计
	Mag_DataGet();
	Mag_DataPre();

	//飞轮
	Whl_DataGet();
	Whl_DataPre();

#ifndef  MINMODULE_TYPE	
	//电推
	PPU_DataPre();	
	//互校验
	if (tmpConst->Allow_Sensor_CThr == 0x55 )
	{
		Att_CrossCheck();	
	}	
#endif
}