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

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

/***********************************************
说明：模块初始化函数,进程开始调用一次 	
注意:	
***********************************************/
void    ZKREInit(void)
{
	UINT8 i =0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	AttIMPTPara_t 	*pIMU =NULL;
	sAttErrCtlPara_t 	*pREcheck =NULL;
	//WhlPara_t *PWhl;
	
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pIMU=&tmpAtt->sIMPTPara;
	pREcheck=&tmpAtt->sErrCtlPara;
	//PWhl = &tmpAtt->sPerPara.WhlPara;		
	//陀螺
	for(i=0;i<GYRO_NUM;i++)
	{
        pREcheck->GyroIn_PowerOn[i]=POSE_OK;
		pREcheck->Gyro_PowerOnCnt[i]=0;
		//tmpConst->AllowGyroDiagFlg[i]=POSE_OK;
		//tmpConst->AllowGyroDiagFlg[i]=0;
		tmpConst->GyroDiagThr = 100;
		pREcheck->GyroErrCnt[i]	=0;	
		pREcheck->Gyro_Err_Cnt1[i] =0;
		pREcheck->Gyro_Err_Cnt2[i] =0;
		pREcheck->Gyro_Err_Cnt3[i] =0;
		pREcheck->Gyro_Diag[i] =POSE_OK;
		pREcheck->Gyro_err[i] =POSE_OK;
		tmpConst->AllowGyroONOFF[i]  = POSE_OK;//POSE_NO;	20240630	
		pIMU->GyroUseState[i] = POSE_OK;				//使用状态置0	
		//tmpConst->GyroUsePrior[i] = PriorHig;
		//tmpConst->Gyro_ComRESW[i]=0;				
	}

	tmpConst->GyroUsePrior[0] = PriorHig;	
	tmpConst->GyroUsePrior[1] = PriorMid;	
	tmpConst->GyroUsePrior[2] = PriorLow;	
	tmpConst->AttCmdFlashPara.Gyro_ComRESW[0]=0;
	tmpConst->AttCmdFlashPara.Gyro_ComRESW[1]=0;
	tmpConst->AttCmdFlashPara.Gyro_ComRESW[2]=0;
	tmpConst->Gyro_Delay_CntLimt =6;					//陀螺开机异常延迟判断计数
	tmpConst->Gyro_Err_CntLimt =8;						//陀螺异常判断计数
	tmpConst->Gyro_OK_CntLimt =8;						//陀螺正常判断计数
	tmpConst->Gyro_ReSet_CntLimt =5;					//陀螺重启总次数
	tmpConst->Gyro_ReSetGAP_CntLimt =10;				//陀螺重启间隔拍数

	//星敏
	for(i=0;i<STAR_NUM;i++)
	{
		pREcheck->ssIn_PowerOn[i]=POSE_OK;
		pREcheck->ss_PowerOnCnt[i]=0;
		//tmpConst->AllowssDiagFlg[i]=POSE_OK;
		tmpConst->AllowssONOFF[i]= POSE_OK;//POSE_NO;20240630
		tmpConst->ssDiagThr = 1200;
		pREcheck->ssErrCnt[i]	=0;
		pREcheck->ss_Err_Cnt1[i] =0;
		pREcheck->ss_Err_Cnt2[i] =0;
		pREcheck->ss_Err_Cnt3[i] =0;
		pREcheck->ss_Diag[i] =POSE_OK;
		pREcheck->SS_err[i] =POSE_OK;
		pIMU->SSUseState[i] = POSE_OK;					//使用状态置0
	}
	tmpConst->SSUsePrior[0] = PriorHig;	
	tmpConst->SSUsePrior[1] = PriorMid;	
	tmpConst->SSUsePrior[2] = PriorLow;	
	//tmpConst->Ss_DiagCnt_Limt=10;
	tmpConst->Ss_Delay_CntLimt =12;						//星敏开机异常延迟判断计数
	//tmpConst->Ss_Err_CntLimt =14;						//星敏异常判断计数
	tmpConst->Ss_Err_CntLimt =30;						//星敏异常判断计数
	tmpConst->Ss_OK_CntLimt =8;							//星敏正常判断计数
	tmpConst->Ss_ReSet_CntLimt =5;						//星敏重启总次数
	tmpConst->Ss_ReSetGAP_CntLimt =10;					//星敏重启间隔拍数	
	
	//磁强计
	for(i=0;i<MAG_NUM;i++)
	{
		pREcheck->MagIn_PowerOn[i]=POSE_OK;
		pREcheck->Mag_PowerOnCnt[i] =0;     			//模拟磁强计加电状态计数
		pREcheck->MagErrCnt[i]=0;						//模拟磁强计异常诊断计数,上限60000
		pREcheck->MagNoErrCnt[i]=0;
		pREcheck->Mag_Err_Cnt1[i]=0;					//模拟磁强计数据异常计数
		pREcheck->Mag_Err_Cnt2[i]=0;					//模拟磁强计故障重启计数
		pREcheck->Mag_Err_Cnt3[i]=0;					//模拟磁强计数据正常计数
		pREcheck->Mag_Diag[i]=POSE_OK;					//模拟磁强计故障标志
		pREcheck->Mag_err[i]=POSE_OK;
		//tmpConst->AllowMagDiagFlg[i]=POSE_OK;			//磁强计异常诊断允许标志。0x55允许0不允许，默认为0x55
		tmpConst->AllowMagONOFF[i]= POSE_OK;//POSE_NO;	20240630				//磁强计异常诊断允许标志。0x55允许0不允许，默认为0x55
		
	}
	tmpConst->MagDiagThr= 100;							//判断时间阈值,单位s,可上注
	tmpConst->Mag_Delay_CntLimt =12;						//磁强计开机异常延迟判断计数
	tmpConst->Mag_Err_CntLimt =14;						//磁强计异常判断计数
	tmpConst->Mag_OK_CntLimt =8;						//磁强计正常判断计数
	tmpConst->Mag_ReSet_CntLimt = 5;					//磁强计重启总次数
	tmpConst->Mag_ReSetGAP_CntLimt =10;					//磁强计重启间隔拍数
	
	//飞轮
	for(i=0;i<WHEEL_NUM;i++)
	{
		pREcheck->WhlIn_PowerOn[i]=POSE_OK;
		pREcheck->Whl_PowerOnCnt[i]=0;
		//tmpConst->AllowWhlDiagFlg[i] = POSE_OK;
		tmpConst->AllowWhlONOFF[i] = POSE_OK;//POSE_NO;
		//tmpConst->AllowWhlONOFF = 0;
		pREcheck->WhlOutErrCnt[i]= 0;
		pREcheck->WhlTemErrCnt[i]= 0;	
		pREcheck->Whl_Err_Cnt1[i]= 0;					//飞轮数据异常计数
		pREcheck->Whl_Err_Cnt2[i]= 0;					//飞轮故障重启计数
		pREcheck->Whl_Err_Cnt3[i]= 0;					//飞轮数据正常计数
		pREcheck->Whl_Err[i]= POSE_OK;
		pREcheck->Whl_Diag[i]=POSE_OK;
	}	
	tmpConst->WhlTemperDiagThr = 90;
	tmpConst->WhlDiag_TimeThr = 10;
	tmpConst->WhlDiag_ErrThr=15;
	
	tmpConst->Whl_Delay_CntLimt =6;						//飞轮开机异常延迟判断计数
	//tmpConst->Whl_Err_CntLimt= 8;						//飞轮异常判断计数
	tmpConst->Whl_Err_CntLimt= 16;						//飞轮异常判断计数
	//tmpConst->Whl_OK_CntLimt= 8;						//飞轮正常判断计数
	tmpConst->Whl_OK_CntLimt= 16;						//飞轮正常判断计数
	tmpConst->Whl_ReSet_CntLimt=5;						//飞轮重启总次数
	tmpConst->Whl_ReSetGAP_CntLimt=16;					//飞轮重启间隔拍数

	//PPU
	pREcheck->PPU_Err=0;	
	//pREcheck->PPUTXT_Err=POSE_OK;
	pREcheck->PPU_Diag=POSE_OK;							//PPU故障标志
	pREcheck->PPUOutErrCnt=0;							//PPU输出不正常计数
	pREcheck->PPUIn_ACurCnt=0;							//阳极电源电流计数
	pREcheck->PPUIn_VCurCnt=0;							//阳极电源电压计数
	pREcheck->PPUOUT_FIRECnt=0;							//开机点火计数
	pREcheck->PPUOUT_TemperECnt=0;						//气瓶管路四路测温点温度
	pREcheck->PPU_Err_Cnt1=0;							//PPU数据异常计数
	pREcheck->PPU_Err_Cnt2=0;							//PPU故障重启计数
	pREcheck->PPU_Err_Cnt3=0;							//PPU数据正常计数
	//PPU
	//tmpConst->AllowPPUDiagFlg= POSE_OK;				//PPU异常诊断允许标志。0x55允许0不允许，默认为0x55
	//tmpConst->AllowPPUDiagFlg= 0;						//PPU异常诊断允许标志。0x55允许0不允许，默认为0x55
	tmpConst->AllowPPUONOFF= POSE_OK;//POSE_NO;	20240630						//PPU重构允许标志。0x55允许0不允许，默认为0x00
	//tmpConst->SafePPUONOFF=POSE_OK;
	tmpConst->PPU_Delay_CntLimt =6;						//磁强计开机异常延迟判断计数
	tmpConst->PPU_Err_CntLimt =8;						//磁强计异常判断计数
	tmpConst->PPU_OK_CntLimt =8;						//磁强计正常判断计数
	tmpConst->PPU_ReSet_CntLimt =5;						//磁强计重启总次数
	tmpConst->PPU_ReSetGAP_CntLimt=10;					//磁强计重启间隔拍数

	tmpConst->PPUTXTimeLimit=15;						//电推通信故障时间阈值15s
	tmpConst->PPUTATimeLimit=30;						//电推过流故障时间阈值30s
	tmpConst->AttCmdFlashPara.PPUTALimit=22;							//电推过流故障阈值2.2
	tmpConst->PPUTVTimeLimit=30;						//电推过压故障时间阈值30s
	tmpConst->AttCmdFlashPara.PPUTVLimit=260;							//电推过压故障阈值260
	tmpConst->AttCmdFlashPara.PPUTFireLimit=420;						//电推点火故障阈值420S
	tmpConst->PPUTemperTimeLimit=5;						//电推温度故障时间阈值 5s
	tmpConst->AttCmdFlashPara.PPUHTemperLimit=30;						//电推温度故障上限阈值30
	//tmpConst->PPULTemperLimit=10;						//电推温度故障下限阈值10
	tmpConst->AttCmdFlashPara.PPULTemperLimit=5;						//电推温度故障下限阈值10

	////姿态基准丢失
	tmpConst->AllowAttLoseDiagFlg = 0x55;//0x00;20240630
	tmpConst->TimeThr_AttLoseDiag[0] =30;				//30s
	tmpConst->TimeThr_AttLoseDiag[1] = 60*60;			//60min
	tmpConst->TimeThr_AttLoseDiag[2] = 60*60;			//60min
	tmpConst->TimeThr_AttLoseDiag[3] = 30;				//30s
	tmpConst->TimeThr_AttLoseDiag[4] = 30;				//30s
	tmpConst->TimeThr_AttLoseDiag[5] = 30;				//30s
	tmpConst->TimeThr_AttLoseDiag[6] = 10;				//10s
	tmpConst->TimeThr_AttLoseDiag[7] = 30;				//30s
	tmpConst->TimeThr_AttLoseDiag[8] = 30;				//30s
	pREcheck->AttLoseCnt=0;

	//姿态超差判断
	tmpConst->AllowAttExDiagFlg= 0x55;//0x00;	20240630				//姿态超差判断允许标志,默认为0x55
	pREcheck->AttExFlg[0]=0x00;
	pREcheck->AttExFlg[1]=0x00;
	pREcheck->AttExFlg[2]=0x00;
	//tmpConst->TimeThr_AttEx[0]=30;						//30s
	tmpConst->TimeThr_AttEx[0]=900;						//900s
	tmpConst->TimeThr_AttEx[1]=30;						//30s
	tmpConst->TimeThr_AttEx[2]=500;						//500s		
	tmpConst->TimeThr_AttEx[3]=30;						//30s
	//tmpConst->TimeThr_AttEx[4]=30;						//30s
	tmpConst->TimeThr_AttEx[4]=2400;						//2400s
	tmpConst->TimeThr_AttEx[5]=30;						//30s	
	tmpConst->TimeThr_AttEx[6]=20;						//20s	
	tmpConst->TimeThr_AttEx[7]=30;						//30s	
	tmpConst->TimeThr_AttEx[8]=30;						//30s	
//	tmpConst->YawCntLimit=1200;							//对地偏航/对目标偏航下偏航姿态超差计数	    
	tmpConst->YawCntLimit=2400;							//对地偏航/对目标偏航下偏航姿态超差计数	 
	
	tmpConst-> AngThr_AttEx[0] =0;						//不判断
	tmpConst-> AngThr_AttEx[1] =0;						//不判断
	tmpConst-> AngThr_AttEx[2] = 0*ANG2RADIAN;          //30°
	tmpConst-> AngThr_AttEx[3] = 0*ANG2RADIAN;          //30°
	tmpConst-> AngThr_AttEx[4] = 10*ANG2RADIAN;         //10°
	tmpConst-> AngThr_AttEx[5] = 0*ANG2RADIAN;          //10°
	tmpConst-> AngThr_AttEx[6] = 10*ANG2RADIAN;         //10°
	tmpConst-> AngThr_AttEx[7] = 0*ANG2RADIAN;          //10°
	tmpConst-> AngThr_AttEx[8] = 10*ANG2RADIAN;         //10°
	
	tmpConst->RateThr_AttEx[0]=-0.002f*ANG2RADIAN;		//-0.002°/s
	tmpConst->RateThr_AttEx[1]=1*ANG2RADIAN;			//-0.002°/s
	tmpConst->RateThr_AttEx[2]=4*ANG2RADIAN;			//1°/s
	tmpConst->RateThr_AttEx[3]=1*ANG2RADIAN;			//1°/s
	tmpConst->RateThr_AttEx[4]=0.5f*ANG2RADIAN;			//0.5°/s
	tmpConst->RateThr_AttEx[5]=1*ANG2RADIAN;			//0.5°/s
	tmpConst-> RateThr_AttEx[6] = 0.5f*ANG2RADIAN;       //10°
	tmpConst-> RateThr_AttEx[7] = 1*ANG2RADIAN;         //10°
	tmpConst-> RateThr_AttEx[8] = 0.5f*ANG2RADIAN;       //10°

	memset(pREcheck->AngRateMod_RATEDMP,0,61*sizeof(TYPE_CAL));
	pREcheck->YawCnt=0;		
	pREcheck->AttExCnt=0;								//姿态超差计数

	////姿态不稳定判断
	tmpConst->AllowAttStaDiagFlg = 0x55;//0x00;	20240630
	pREcheck->AttUnSta[0] = 0x00;
	pREcheck->AttUnSta[1] = 0x00;
	pREcheck->AttUnSta[2] = 0x00;

	tmpConst->TimeThr_AttUnSta[0] = 10000;				//10000s
	//tmpConst->TimeThr_AttUnSta[1] = 1200;				//1200s
	tmpConst->TimeThr_AttUnSta[1] = 1800;				//1800s
	tmpConst->TimeThr_AttUnSta[2] = 15000;				//15000s
	tmpConst->TimeThr_AttUnSta[3] = 3600;				//3600s
	tmpConst->TimeThr_AttUnSta[4] = 10;					//10s
	tmpConst->TimeThr_AttUnSta[5] = 10;					//10s
	tmpConst->TimeMod_AttUnSta[0] = 0;					//0
	tmpConst->TimeMod_AttUnSta[1] = 0;					//0
	tmpConst->TimeMod_AttUnSta[2] = 0;					//0
	tmpConst->TimeMod_AttUnSta[3] = 0;					//0
	//tmpConst->TimeMod_AttUnSta[4] = 1500;				//1500s
	tmpConst->TimeMod_AttUnSta[4] = 1800;				//1800s
	tmpConst->TimeMod_AttUnSta[5] = 1500;				//1500s
	
	tmpConst->AngThr_AttUnSta[0] = 0*ANG2RADIAN;        //20°
	tmpConst->AngThr_AttUnSta[1] = 20.0*ANG2RADIAN;     //20°
	tmpConst->AngThr_AttUnSta[2] = 30.0*ANG2RADIAN;     //30°
	tmpConst->AngThr_AttUnSta[3] = 0*ANG2RADIAN;        //20°
	tmpConst->AngThr_AttUnSta[4] = 2.0*ANG2RADIAN;      //20°
	tmpConst->AngThr_AttUnSta[5] = 2.0*ANG2RADIAN;      //20°
	
	tmpConst->RateThr_AttUnSta[0] = 0*ANG2RADIAN;       //1°/s
	tmpConst->RateThr_AttUnSta[1] = 1.0*ANG2RADIAN;     //1°/s
	tmpConst->RateThr_AttUnSta[2] = 1.0*ANG2RADIAN;     //0.1°/s
	tmpConst->RateThr_AttUnSta[3] = 0*ANG2RADIAN;       //0.1°/s
	tmpConst->RateThr_AttUnSta[4] = 0.5*ANG2RADIAN;		//0.5°/s
	tmpConst->RateThr_AttUnSta[5] = 0.1*ANG2RADIAN;     //0.1°/s
	
	pREcheck->AttUnStaCnt=0;
	pREcheck->AttModCnt=0;
		               
}

/***********************************************
说明：单机加\断电处理
Chl :单机通道号
Cnt1:异常连续判断拍数
Cnt2:故障重启次数
Cnt3:正常连续判断拍数
err : 异常字
state :故障字		   
***********************************************/
void ImuPowerOnOff(	UINT8 ChlON,UINT8 ChlOFF,  UINT8 ONOFF, 
					UINT32 *Cnt1 , UINT32 *Cnt2, UINT32 *Cnt3, 
					UINT32 Cnt1Limt ,  UINT32 Cnt2Limt , UINT32 Cnt3Limt ,UINT32 Cnt4Limt , 
					UINT8 err, UINT8 *state)
{
	UINT8 PulT = 16;  //160MS
	UINT8 Type = 0;
	
	if (POSE_NO == *state)
	{
		//state=POSE_NO;
		*Cnt1 = 0;
		//*Cnt2 = 0;
		*Cnt3 = 0;
		return;
	}

	if (POSE_NO == err)
	{
		if(*Cnt1<60000)
		{(*Cnt1) ++;}
		*Cnt3 = 0;
	}
	else
	{
		if(*Cnt3<60000)
		{(*Cnt3)++;}
		*Cnt1=0;
		//Cnt2 = 0;
	}
	
	if ((*Cnt1) >= Cnt1Limt )
	{
		if (*Cnt2 >= Cnt2Limt)
		{
			*state = POSE_NO;
			*Cnt1 = 0;
			//*Cnt2 = 0;
			*Cnt3 = 0;

			return;
		}
		else
		{
			if ((*Cnt1 >= (Cnt1Limt + Cnt4Limt) ) && (*Cnt1 < (Cnt1Limt + Cnt4Limt +1) ))
			{
				*Cnt1 = 0;
				*Cnt3 = 0;
				if(*Cnt2<60000)
				{(*Cnt2) ++;}
				
				if (*Cnt2 >= Cnt2Limt)
				{
					if ( POWER_OFF == ONOFF)
					{
						//留在关单机
						OCOut(ChlOFF, Type, PulT);
					}
					else if (POWER_ON == ONOFF)
					{
						//开单机
						OCOut(ChlON, Type, PulT);
					}
				} 
				else
				{
					//开单机
					OCOut(ChlON, Type, PulT);
				}	
			}
		}
		if ((*Cnt1 < (Cnt1Limt +1) )&&(*Cnt1 >= Cnt1Limt ))
		{
			//关单机
			OCOut(ChlOFF, Type, PulT);
			*state = POSE_IN;
		}				
	}
	else
	{  
		if (*Cnt3 >= Cnt3Limt)
		{
			*state = POSE_OK;
			*Cnt2 = 0;      //zmx20231011
		} 
		else
		{
			*state = POSE_IN;
		}
	}
}

/***********************************************
说明：陀螺异常诊断

***********************************************/
void GyroImuErrJudge(void)
{
	UINT8 i =0;
	UINT8 ONOFF = POWER_ON;
	UINT8 ChlON[GYRO_NUM] = {OC66_GYROA_ON,OC69_GYROB_ON,OC71_GYROC_ON};
	UINT8 ChlOFF[GYRO_NUM] = {OC67_GYROA_OFF,OC70_GYROB_OFF,OC72_GYROC_OFF};
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	GyroPara_t *PGyro[GYRO_NUM];	
	AttIMPTPara_t 	*pIMU =NULL;	
	sAttErrCtlPara_t 	*pREcheck =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];
	pIMU=&tmpAtt->sIMPTPara;
	pREcheck=&tmpAtt->sErrCtlPara;
	
	for(i=0;i<GYRO_NUM;i++)
	{
		if (pREcheck->Gyro_PowerOnCnt[i] >= tmpConst->Gyro_Delay_CntLimt)
		{
			if((POSE_NO == PGyro[i]->GyroIn_AttVld)||(CC_ERROR == PGyro[i]->GyroCrCheckFlg))
			{
				if(pREcheck->GyroErrCnt[i]<60000)
				{pREcheck->GyroErrCnt[i]++;}
			}
			else
			{
				pREcheck->GyroErrCnt[i]=0;
			}
		
			if(pREcheck->GyroErrCnt[i] >= tmpConst->GyroDiagThr*2)
			{
				pREcheck->Gyro_err[i]  = POSE_NO;
			}
			else
			{
				pREcheck->Gyro_err[i]  = POSE_OK;
			}
		}

		if (POSE_OK == tmpConst->AllowGyroONOFF[i])
		{
			//陀螺单机加断电
			ImuPowerOnOff(	ChlON[i], ChlOFF[i],  ONOFF, 
			&pREcheck->Gyro_Err_Cnt1[i] ,  &pREcheck->Gyro_Err_Cnt2[i], &pREcheck->Gyro_Err_Cnt3[i],
			tmpConst->Gyro_Err_CntLimt ,   tmpConst->Gyro_ReSet_CntLimt ,  tmpConst->Gyro_OK_CntLimt , tmpConst->Gyro_ReSetGAP_CntLimt ,
			pREcheck->Gyro_err[i],  &pREcheck->Gyro_Diag[i] );
		
			if (POSE_NO == pREcheck->Gyro_Diag[i])
			{
				;//pIMU->GyroUseState[i] = POSE_NO;//使用状态置0
			}
		} 
		else
		{
			pREcheck->Gyro_Err_Cnt1[i] = 0;
			pREcheck->Gyro_Err_Cnt2[i] = 0;
			pREcheck->Gyro_Err_Cnt3[i] = 0;
			pREcheck->Gyro_Diag[i] =POSE_OK;
			pREcheck->Gyro_err[i]  = POSE_OK;
			pREcheck->GyroErrCnt[i]=0;
		}
	}
}


/***********************************************
说明：星敏异常诊断
***********************************************/
void SsImuErrJudge(void)
{
	UINT8 i =0;
	UINT8 ONOFF = POWER_ON;
	UINT8 ChlON = 0;
	UINT8 ChlOFF = 0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	SsPara_t *Pss[STAR_NUM];	
	AttIMPTPara_t 	*pIMU;	
	sAttErrCtlPara_t 	*pREcheck;	

	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	Pss[0] = &tmpAtt->sPerPara.SsPara[0];
	Pss[1] = &tmpAtt->sPerPara.SsPara[1];
	Pss[2] = &tmpAtt->sPerPara.SsPara[2];
	pIMU=&tmpAtt->sIMPTPara;
	pREcheck=&tmpAtt->sErrCtlPara;
		
	for(i=0;i<STAR_NUM;i++)
	{

		if (pREcheck->ss_PowerOnCnt[i] >= tmpConst->Ss_Delay_CntLimt)
		{
			if((POSE_NO == Pss[i]->ssIn_AttVld)||(CC_ERROR == Pss[i]->ssCrCheckFlg))
			{
				if(pREcheck->ssErrCnt[i]<60000)
				{pREcheck->ssErrCnt[i]++;}
			}
			else
			{
				pREcheck->ssErrCnt[i]=0;
			}
		
			if(pREcheck->ssErrCnt[i] >= tmpConst->ssDiagThr*2)
			{
				pREcheck->SS_err[i]  = POSE_NO;
			}
			else
			{
				pREcheck->SS_err[i]  = POSE_OK;
			}
		} 

		if (POSE_OK == tmpConst->AllowssONOFF[i])
		{
			ChlON = OC74_STA_ON + i*2 ;
			ChlOFF = OC74_STA_ON + i*2 + 1;
			if (i == 2)
			{
				ChlOFF = OC83_STC_OFF;
			}
		
			//星敏单机加断电
			ImuPowerOnOff(	ChlON, ChlOFF,  ONOFF, 
			&pREcheck->ss_Err_Cnt1[i] ,  &pREcheck->ss_Err_Cnt2[i], &pREcheck->ss_Err_Cnt3[i],
			tmpConst->Ss_Err_CntLimt ,   tmpConst->Ss_ReSet_CntLimt ,  tmpConst->Ss_OK_CntLimt , tmpConst->Ss_ReSetGAP_CntLimt ,
			pREcheck->SS_err[i],  &pREcheck->ss_Diag[i] );
		
			if ((POSE_NO == pREcheck->ss_Diag[i]) && (POSE_OK == tmpConst->AllowssONOFF[i]))
			{
				;//pIMU->SSUseState[i] = POSE_NO;//使用状态置0
			}
		} 
		else
		{
			pREcheck->ss_Err_Cnt1[i] = 0;
			pREcheck->ss_Err_Cnt2[i] = 0;
			pREcheck->ss_Err_Cnt3[i] = 0;
			pREcheck->ss_Diag[i] =POSE_OK;
			pREcheck->ssErrCnt[i]=0;
			pREcheck->SS_err[i]  = POSE_OK;
		}		
	}
}

/***********************************************
说明：磁强计异常诊断
***********************************************/
void MagImuErrJudge(void)
{
	UINT8 i =0;
	UINT8 ONOFF = POWER_ON;	
	UINT8 ChlON = 0;
	UINT8 ChlOFF = 0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	MagPara_t *PMag =NULL;	
	AttIMPTPara_t 	*pIMU =NULL;	
	sAttErrCtlPara_t 	*pREcheck =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;
	pREcheck=&tmpAtt->sErrCtlPara;
	
	for(i=0;i<MAG_NUM;i++)
	{	
		if (pREcheck->Mag_PowerOnCnt[i] >= tmpConst->Mag_Delay_CntLimt)
		{
			if((POSE_NO == PMag->MagIn_Valid[i])||(CC_ERROR == PMag->MagCrCheckFlg[i]))
			{
				if(pREcheck->MagErrCnt[i]<60000)
				{pREcheck->MagErrCnt[i]++;}
				pREcheck->MagNoErrCnt[i]=0;
			}
			else
			{
				if(pREcheck->MagNoErrCnt[i]<60000)
				{pREcheck->MagNoErrCnt[i]++;}				
				pREcheck->MagErrCnt[i]=0;
			}
		
			if(pREcheck->MagErrCnt[i] >= tmpConst->MagDiagThr*2)
			{
				pREcheck->Mag_err[i]  = POSE_NO;
			}
			else if (pREcheck->MagNoErrCnt[i] > 2)
			{
				pREcheck->Mag_err[i]  = POSE_OK;
			}
		} 

		if (POSE_OK == tmpConst->AllowMagONOFF[i])
		{
			ChlON = OC94_CQJA_ON + i*2 ;
			ChlOFF = OC94_CQJA_ON + i*2 + 1;
		
			//磁强计单机加断电
			ImuPowerOnOff(	ChlON, ChlOFF,  ONOFF, 
			&pREcheck->Mag_Err_Cnt1[i] ,  &pREcheck->Mag_Err_Cnt2[i], &pREcheck->Mag_Err_Cnt3[i],
			tmpConst->Mag_Err_CntLimt ,   tmpConst->Mag_ReSet_CntLimt ,  tmpConst->Mag_OK_CntLimt , tmpConst->Mag_ReSetGAP_CntLimt ,
			pREcheck->Mag_err[i],  &pREcheck->Mag_Diag[i] );
		
			if ((POSE_NO == pREcheck->Mag_Diag[i]) && (POSE_OK == tmpConst->AllowMagONOFF[i]))
			{
				;//pIMU->MagUseState[i]  = POSE_NO;
			}
		}
		else
		{
			pREcheck->Mag_Err_Cnt2[i] = 0;
			pREcheck->Mag_Diag[i] =POSE_OK;
			pREcheck->Mag_Err_Cnt1[i]= 0;
			pREcheck->Mag_Err_Cnt2[i]= 0;
			pREcheck->Mag_Err_Cnt3[i]= 0;
			pREcheck->MagErrCnt[i]=0;	
			pREcheck->Mag_err[i]  = POSE_OK;
			
		}
		
	}
}

/***********************************************
说明：飞轮异常诊断

***********************************************/
void WhlImuErrJudge(void)
{
	UINT8 i = 0;
	UINT8 ChlON = 0;
	UINT8 ChlOFF = 0;
	UINT8 ONOFF = POWER_OFF;	
	UINT32 tmp_cnt = 0;
	UINT8		cmdDataBuff[50] = {0};
	sTaskInfo* tmpTaskInfo = NULL;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttErrCtlPara_t 	*pREcheck =NULL;	
	WhlPara_t *PWhl =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;}
	pREcheck=&tmpAtt->sErrCtlPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	pImpt=&tmpAtt->sIMPTPara;
	pWMod =&tmpAtt->sModePara;
	for (i = 0; i < WHEEL_NUM; ++i)
	{
		if (POSE_OK != tmpConst->AllowWhlONOFF[i])
		{
			pREcheck->Whl_Err_Cnt1[i] = 0;
			pREcheck->Whl_Err_Cnt2[i] = 0;
			pREcheck->Whl_Err_Cnt3[i] = 0;	
			pREcheck->WhlOutErrCnt[i]=0;
			pREcheck->WhlTemErrCnt[i] = 0;
			pREcheck->Whl_Diag[i] =POSE_OK;
			pREcheck->Whl_Err[i] = POSE_OK;
		}
	}
	switch (pWMod->WorkMode)
	{
		case	ATTMOD_UNCTL:
		case	ATTMOD_RATEDMP:
		case	ATTMOD_MagTOSUN:	/*不进行飞轮检测*/
		for (i = 0; i < WHEEL_NUM; ++i)
		{
			if (pImpt->Whl_Use[i] == POSE_OK)
			{
				tmp_cnt++;
			}
		}
		PWhl->Whl_UseCnt = tmp_cnt;
		break;
		case    ATTMOD_WhlTOSUN:
		case    ATTMOD_ONLTOEARTH:
		case    ATTMOD_WHEELTOEARTH:
		case    ATTMOD_ATTAJUSTPRE:
		case    ATTMOD_ORBITCTL:
		case    ATTMOD_ATTAJUST:
		case    ATTMOD_ONLTOTAR:
		//case    ATTMOD_NOCOLL:
		for (i = 0; i < WHEEL_NUM; ++i)
		{
			if (pREcheck->Whl_PowerOnCnt[i] >= tmpConst->Whl_Delay_CntLimt)
			{
				//飞轮输出异常计数
				if(PWhl->WhlIn_Valid[i]==0x55)
				{pREcheck->WhlOutErrCnt[i]=0;}
				else
				{
					if(pREcheck->WhlOutErrCnt[i]<60000)
					{pREcheck->WhlOutErrCnt[i]++;}
				}
				//轴温异常计数
				if(PWhl->Whl_Tmper[i]<(TYPE_CAL)tmpConst->WhlTemperDiagThr)
				{pREcheck->WhlTemErrCnt[i]=0;}
				else
				{
					if(pREcheck->WhlTemErrCnt[i]<60000)
					{pREcheck->WhlTemErrCnt[i]++;}
				}
				//飞轮异常
				if((pREcheck->WhlOutErrCnt[i]>=tmpConst->WhlDiag_ErrThr*2)||(pREcheck->WhlTemErrCnt[i]>=tmpConst->WhlDiag_TimeThr*2))
				{
					pREcheck->Whl_Err[i] = POSE_NO;
				}
				else
				{
					pREcheck->Whl_Err[i] = POSE_OK;
				}
			} 

			if (POSE_OK == tmpConst->AllowWhlONOFF[i])
			{
				ChlON = OC58_WHLX_ON + i*2;
				ChlOFF = OC58_WHLX_ON + i*2 + 1;
			
				//飞轮单机加断电
				ImuPowerOnOff(	ChlON, ChlOFF,  ONOFF,
				&pREcheck->Whl_Err_Cnt1[i] ,  &pREcheck->Whl_Err_Cnt2[i], &pREcheck->Whl_Err_Cnt3[i],
				tmpConst->Whl_Err_CntLimt ,   tmpConst->Whl_ReSet_CntLimt ,  tmpConst->Whl_OK_CntLimt , tmpConst->Whl_ReSetGAP_CntLimt ,
				pREcheck->Whl_Err[i],  &pREcheck->Whl_Diag[i] );
			
				if (POSE_NO == pREcheck->Whl_Diag[i])
				{
					pImpt->Whl_Use[i] = POSE_NO;
				}
			} 
			else
			{
				pREcheck->Whl_Diag[i] =POSE_OK;
			}

			if (pImpt->Whl_Use[i] == POSE_OK)
			{
				tmp_cnt++;
			}
		}
	
		PWhl->Whl_UseCnt = tmp_cnt;
	
		if ((POSE_OK == tmpConst->AllowWhlONOFF[0])&&(POSE_OK == tmpConst->AllowWhlONOFF[1])&&(POSE_OK == tmpConst->AllowWhlONOFF[2])&&(POSE_OK == tmpConst->AllowWhlONOFF[3]))
		{
			if (PWhl->Whl_UseCnt >= 3)
			{
				//PWhl->Whl_OK = POSE_OK;
				;
			}
			else
			{
				//PWhl->Whl_OK = POSE_NO;
	
				//切换到新模式后，将相关计数器清零
				memset(cmdDataBuff, 0x00, 50);
				tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
				DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
				if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
				{
					/* 在姿控的指令处理中 进行 ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
					BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
				}
			}
		}
		break;	
			
		default:
		for (i = 0; i < WHEEL_NUM; ++i)
		{
			pREcheck->WhlOutErrCnt[i] =0;
			pREcheck->WhlTemErrCnt[i] =0;
		}
		break;
	}
	
}


/***********************************************
说明：PPU异常诊断

***********************************************/
void PPUImuErrJudge(void)
{
//	UINT8 PPUOn[8]			= {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	//UINT8 PPUOff[8]			= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	UINT8 PPUOff[8]	= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	INT16 TemH[4] ={0};
	TYPE_CAL TemR[4] ={0.0},Temper[4] ={0.0},TempBAR[4] ={0.0};
	TYPE_CAL TemA =-6.01188f;
	TYPE_CAL TemB =4622.53337f;
	TYPE_CAL TemC =-86421.72414f;
	TYPE_CAL TemD =273.15f;
	TYPE_CAL TemEXP =2.718281828f;
	INT16 *pTemper =NULL;
	UINT8 ChlON = 0;
	UINT8 ChlOFF = 0;
	UINT8 TmpPPU_Err = POSE_OK;
	UINT8 i = 0;
	UINT8 ONOFF = POWER_ON;

	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttErrCtlPara_t 	*pREcheck;	
	AttIMPTPara_t *pImpt;	
	PPUPara_t *PPPU;	
	sAttModPara_t *pWMod;			
	sDevInfo * tmpDevInfo;
		
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	
	pREcheck=&tmpAtt->sErrCtlPara;
	pImpt=&tmpAtt->sIMPTPara;
	PPPU = &tmpAtt->sPerPara.PPUPara;
	pWMod =&tmpAtt->sModePara;
	
	//获取AD采集数据
	tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
	pTemper =  (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
	////读取PPU温度
	TemH[0] = *(pTemper + SW22_BOTTLE1-1);		//气瓶测温1
	TemH[1] = *(pTemper + SW23_BOTTLE2-1);		//气瓶测温1
	TemH[2]= *(pTemper + SW24_PIPE1-1);			//管路测温1
	TemH[3]= *(pTemper + SW50_PIPE2-1);			//管路测温2  

	for(i=0;i<4;i++)
	{
		if (((3849.3-TemH[i])<1801.8)&&((3849.3-TemH[i]) > 0))
		{
			//求电阻值
			if(POSE_ABSF(3849.3-TemH[i])<POSE_ZERO)
			{
				continue;
			}
			TemR[i] = 10000*(1801.8/(3849.3-TemH[i]) - 1);
			//求温度中间变量
			TempBAR[i] =TemB*TemB - 4*TemC*(TemA -log(TemR[i])/log(TemEXP));
			if (TempBAR[i] > POSE_ZERO)
			{
				if (POSE_ABSF(POSE_SQRTF(TempBAR[i]) - TemB)>POSE_ZERO)
				{
					Temper[i] =2*(TemC)/(POSE_SQRTF(TempBAR[i]) - TemB) - TemD;
				}
			}
		}
	}
	PPPU->PPUT_BOTTLE_Tmper1 = Temper[0];
	PPPU->PPUT_BOTTLE_Tmper2 = Temper[1];
	PPPU->PPUT_PIPE_Tmper1 = Temper[2];
	PPPU->PPUT_PIPE_Tmper2 = Temper[3];

	if (pREcheck->PPU_PowerOnCnt >= tmpConst->PPU_Delay_CntLimt)
	{
		//ppu输出异常计数
		if(PPPU->PPUIn_Valid!=POSE_OK)/*PPU数据无效*/
		{
			if(pREcheck->PPUOutErrCnt<60000)
			{pREcheck->PPUOutErrCnt++;}
		}
		else
		{
			pREcheck->PPUOutErrCnt=0;
		}


		if(0x01 == PPPU->PPUIn_FIREOK)/*点火成功标志为1*/
		{
			if(PPPU->PPUIn_ACurrent < (((TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTALimit)/10.0))                //20231228
			{pREcheck->PPUIn_ACurCnt=0;}
			else
			{
				if(pREcheck->PPUIn_ACurCnt<60000)
				{pREcheck->PPUIn_ACurCnt++;}
			}
		
			if(PPPU->PPUIn_AVoltage < (TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTVLimit)
			{pREcheck->PPUIn_VCurCnt=0;}
			else
			{
				if(pREcheck->PPUIn_VCurCnt<60000)
				{pREcheck->PPUIn_VCurCnt++;}
			}
		}
		else
		{
			pREcheck->PPUIn_ACurCnt=0;
			pREcheck->PPUIn_VCurCnt=0;
		}

		if ((POSE_NO == PPPU->PPUOUT_Sta )||(POSE_IN == PPPU->PPUOUT_Sta))
		{pREcheck->PPUOUT_FIRECnt=0;}
		else
		{
			if(pREcheck->PPUOUT_FIRECnt<60000)
			{pREcheck->PPUOUT_FIRECnt++;}
		}		

		if ((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode))
		{
			if (((Temper[0]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[0]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[1]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[1]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))
				&&((Temper[2]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[2]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[3]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[3]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit))))
			{
				if(pREcheck->PPUOUT_TemperECnt<60000)
				{pREcheck->PPUOUT_TemperECnt++;		}
			}
			else
			{
				pREcheck->PPUOUT_TemperECnt=0;	
			}
		}
		else
		{
			pREcheck->PPUOUT_TemperECnt=0;
		}	
	

		//if(((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode)))&&(POSE_OK == tmpConst->AllowPPUONOFF))
		if((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err | 0x03 ;
			//pImpt->PPUUseStatus = POSE_NO;
			//if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
			if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
			{
				pImpt->PPUUseStatus = POSE_NO;
			}
		}
		else
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err & 0xFC ;
			
		}


		if(((0x01 == PPPU->PPUIn_FIREOK)&&((pREcheck->PPUIn_ACurCnt>= (tmpConst->PPUTATimeLimit*2))||(pREcheck->PPUIn_VCurCnt>=(tmpConst->PPUTVTimeLimit*2))))&&(POSE_OK == tmpConst->AllowPPUONOFF))
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err | 0x0C ;
			//pImpt->PPUUseStatus = POSE_NO;
			if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
			{
				pImpt->PPUUseStatus = POSE_NO;
			}
		}
		else
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err & 0xF3 ;
			
		}

	
		if(((pREcheck->PPUOUT_FIRECnt>=tmpConst->AttCmdFlashPara.PPUTFireLimit*2)&&(PPPU->PPUIn_FIREOK!=0x01))&&(POSE_OK == tmpConst->AllowPPUONOFF))
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err | 0x30 ;
			if ((POSE_NO==tmpConst->AttCmdFlashPara.WorkModeChangeAuto) ||(POSE_OK == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
			{
				//pImpt->PPUUseStatus = POSE_NO;
				if((ATTMOD_ORBITCTL == pWMod->WorkMode)||(ATTMOD_ATTAJUSTPRE == pWMod->WorkMode))/*轨控期间*/
				{
					pImpt->PPUUseStatus = POSE_NO;
				}
			}
			else if ((POSE_OK==tmpConst->AttCmdFlashPara.WorkModeChangeAuto)&&(POSE_NO == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
			{
				/*轨控异常，当前拍的关PPU动作*/
				dev_can_write(DEV_NO_CAN0, PPUOff, 8);
				PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
				tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_IN;
				/*2.5S后关PPU电源*/
				PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
				PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
					
				PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_IN;
				PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
			} 
			else
			{
				;
			}				
		}
		else
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
			
		}
	
		if((pREcheck->PPUOUT_TemperECnt>=tmpConst->PPUTemperTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err | 0xC0;
			//pImpt->PPUUseStatus = POSE_NO;
			if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
			{
				pImpt->PPUUseStatus = POSE_NO;
			}
		}
		else
		{
			pREcheck->PPU_Err = pREcheck->PPU_Err & 0x3F;
		}

		//if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
		//{
			//pREcheck->PPU_Err_Cnt1 = 0;  
			//pREcheck->PPU_Err_Cnt2 = 0;  
			//pREcheck->PPU_Err_Cnt3 = 0;  
			//return;	
		//}
		
		//if (ATTMOD_ORBITCTL ==pWMod->WorkMode)
		if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
		{
			pREcheck->PPU_Err_Cnt1 = 0;
			pREcheck->PPU_Err_Cnt2 = 0;
			pREcheck->PPU_Err_Cnt3 = 0;
			if (POSE_OK == tmpConst->AllowPPUONOFF)
			{
				;
			}
			else
			{
				pREcheck->PPU_Diag =POSE_OK;
				pREcheck->PPU_Err = 0;
				pREcheck->PPUOutErrCnt=0;
				pREcheck->PPUIn_ACurCnt=0;
				pREcheck->PPUIn_VCurCnt=0;
				pREcheck->PPUOUT_FIRECnt=0;
				pREcheck->PPUOUT_TemperECnt=0;				
			}
			return;	
		}		
	} 

	if (0x03 == (pREcheck->PPU_Err & 0x03))
	{
		TmpPPU_Err = POSE_NO;
	}

	if (POSE_OK == tmpConst->AllowPPUONOFF)
	{
		ChlON = OC68_PPU_ON ;
		ChlOFF = OC73_PPU_OFF;
	
		//PPU单机加断电
		ImuPowerOnOff(	ChlON, ChlOFF,  ONOFF,
		&pREcheck->PPU_Err_Cnt1,  &pREcheck->PPU_Err_Cnt2, &pREcheck->PPU_Err_Cnt3,
		tmpConst->PPU_Err_CntLimt ,   tmpConst->PPU_ReSet_CntLimt ,  tmpConst->PPU_OK_CntLimt , tmpConst->PPU_ReSetGAP_CntLimt ,
		TmpPPU_Err,  &pREcheck->PPU_Diag );
	
		if (POSE_NO == pREcheck->PPU_Diag)
		{
			pImpt->PPUUseStatus  = POSE_NO;
		}
	} 
	else
	{
		pREcheck->PPU_Err_Cnt1 = 0;
		pREcheck->PPU_Err_Cnt2 = 0;
		pREcheck->PPU_Err_Cnt3 = 0;
		pREcheck->PPU_Diag =POSE_OK;
		pREcheck->PPU_Err = 0;
		pREcheck->PPUOutErrCnt=0;
		pREcheck->PPUIn_ACurCnt=0;
		pREcheck->PPUIn_VCurCnt=0;
		pREcheck->PPUOUT_FIRECnt=0;
		pREcheck->PPUOUT_TemperECnt=0;
	}
}


/*******************************************************************************************
说明：闭环姿态输出长时间无效判断
输出：0x55表示长时间姿态丢失，0表示姿态未丢失
*******************************************************************************************/
UINT8 AttLose(UINT8 i)
{
	UINT8 result=0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttErrCtlPara_t *pREcheck =NULL;	
	
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return 0;}
	pREcheck=&tmpAtt->sErrCtlPara;
	
	if(POSE_OK == tmpAtt->sDeterPara.CLPAttValid)
	{
		pREcheck->AttLoseCnt=0;
		return 0;
	}
	else
	{
		if(pREcheck->AttLoseCnt<60000)
		{pREcheck->AttLoseCnt++;}
	}
	
	if(pREcheck->AttLoseCnt>=tmpConst->TimeThr_AttLoseDiag[i]*2)
	{result=0x55;}
	
	return result;
}

/***********************************************
说明：姿态丢失判断
***********************************************/
void AttLoseJudge(void)
{
	UINT8 cmdDataBuff[50] = {0};
	UINT8 PPUOff[8]	= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	sTaskInfo* tmpTaskInfo = NULL;
	UINT8 	tmpNewMod = 0x00;
	UINT16 	tmpNewModID = 0x00;
	
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
	sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
	sAttModPara_t *pWMod;
	sAttOrbitCtlInfo_t *pOrb;
	sAttTARCCtlInfo_t *pTARC;
	PPUPara_t *PPPU = NULL;	

	if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
	{return;}
	pWMod=&tmpAtt->sModePara;
	pOrb=&tmpAtt->sOrbitInp;
	pTARC=&tmpAtt->sTARCInp;
	PPPU = &tmpAtt->sPerPara.PPUPara;	
	tmpNewMod = pWMod->WorkMode;
	
	
	switch(pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP:
		if(POSE_OK == AttLose(0))
		{
			tmpNewMod = ATTMOD_WhlTOSUN;
			tmpNewModID =  ATT_CMDMODE_WhlTOSUN;
		}
		break;
		
		case ATTMOD_WhlTOSUN:
		if(POSE_OK == AttLose(1))
		{
			tmpNewMod = ATTMOD_MagTOSUN;   //gg20240522js
			tmpNewModID =  ATT_CMDMODE_MagTOSUN;
		}
		break;
		
		case ATTMOD_MagTOSUN:
		if(POSE_OK == AttLose(2))
		{
			tmpNewMod = ATTMOD_UNCTL;
			tmpNewModID =  ATT_CMDMODE_UNCTL;

		}
		break;
		
		case ATTMOD_ONLTOEARTH:
		if(POSE_OK == AttLose(3))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
		}
		break;
		
		case ATTMOD_WHEELTOEARTH:
		if(POSE_OK == AttLose(4))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
		}
		break;
		
		case ATTMOD_ATTAJUSTPRE:
		if(POSE_OK == AttLose(5))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
			pOrb->CurOrbCtrlPcak_ID = 0;
			pOrb->CurOrbCtrlPcak_PreID =0xFF;
			PPPU->PPUT_FIR_Cnt = 0;
			memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
			memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
			pOrb->OrbDataIn_OK = POSE_NO;
			tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
		}
		break;
		
		case ATTMOD_ORBITCTL:
		if(POSE_OK == AttLose(6))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
			pOrb->CurOrbCtrlPcak_ID = 0;
			pOrb->CurOrbCtrlPcak_PreID =0xFF;
			PPPU->PPUT_FIR_Cnt = 0;
			memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
			memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
			pOrb->OrbDataIn_OK = POSE_NO;
			
			/*轨控时故障诊断异常，当前拍的关PPU动作*/
			dev_can_write(DEV_NO_CAN0, PPUOff, 8);
			PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
			tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
			/*2.5S后关PPU电源*/
			PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
			PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
			
			PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
			PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
			
		}
		break;
		
		case ATTMOD_ATTAJUST:
		if(POSE_OK == AttLose(7))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
			pTARC->CurTARCtrlPcak_ID=0;
			pTARC->CurTARCtrlPcak_PreID =0xFF;
			pTARC->TARDataIn_OK = POSE_NO;
			memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
			memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));			
		}
		break;
		
		case ATTMOD_ONLTOTAR:
		if(POSE_OK == AttLose(8))
		{
			//发速率阻尼指令
			tmpNewMod = ATTMOD_RATEDMP;
			tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
			pTARC->CurTARCtrlPcak_ID=0;
			pTARC->CurTARCtrlPcak_PreID =0xFF;
			pTARC->TARDataIn_OK = POSE_NO;
			memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
			memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));	
		}
		break;	
		default:
			break;	
	}

	if(tmpNewMod != pWMod->WorkMode)
	{
		memset(cmdDataBuff, 0x00, 50);
		tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
		DoSetInt32To8(tmpNewModID, cmdDataBuff);
		if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
		{
			/* 在姿控的指令处理中 进行 ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
			BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
		}
	}
	
}


/***********************************************
说明：姿态超差判断
***********************************************/
void AttExJudge(void)
{
	UINT8 cmdDataBuff[50] = {0};
	UINT8 PPUOff[8]	= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	sTaskInfo* tmpTaskInfo = NULL;
	UINT8 	tmpNewMod = 0x00;
	UINT16 	tmpNewModID = 0x00;
	UINT8 i =0;
	UINT8 tmpAttNOK[3] = {POSE_OK,POSE_OK,POSE_OK};
	UINT8 tmpWhlOK = POSE_OK;
	TYPE_CAL tmpMod =0.0;
	TYPE_CAL tmpAngE[3] ={0.0};//姿态角偏差
	TYPE_CAL tmpAngRateE[3] ={0.0};//姿态角速度偏差
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	WhlPara_t *PWhl =NULL;	
	sAttModPara_t *pWMod =NULL;
	sAttErrCtlPara_t *pREcheck =NULL;
	WhlPara_t	*pWhl =NULL;
	AttIMPTPara_t 	*pIMU =NULL;
	PPUPara_t *PPPU = NULL;
	
	sAttOrbitCtlInfo_t *pOrb =NULL;
	sAttTARCCtlInfo_t *pTARC =NULL;
	sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
	sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;

	if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
	return;
	PWhl = &tmpAtt->sPerPara.WhlPara;	
	pWMod=&tmpAtt->sModePara;
	pREcheck=&tmpAtt->sErrCtlPara;	
	pWhl=&tmpAtt->sPerPara.WhlPara;
	pIMU=&tmpAtt->sIMPTPara;

	pOrb=&tmpAtt->sOrbitInp;
	pTARC=&tmpAtt->sTARCInp;
	PPPU = &tmpAtt->sPerPara.PPUPara;
	tmpNewMod = pWMod->WorkMode;
	
	switch(pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP:
		{
			CalNormal(tmpAtt->sDeterPara.CLPAttAngRat, 3, &tmpMod);
			//每30s记录一次闭环姿态角速度矢量模值
			for(i=0;i<60;i++)
			{pREcheck->AngRateMod_RATEDMP[i]=pREcheck->AngRateMod_RATEDMP[i+1];}
			pREcheck->AngRateMod_RATEDMP[60]=tmpMod;

			//角速度矢量模值与30s前差值判断
			if ((pREcheck->AngRateMod_RATEDMP[60]-pREcheck->AngRateMod_RATEDMP[0])<(tmpConst->RateThr_AttEx[0]))
			{
				pREcheck->AttExCnt=0;
				memset(pREcheck->AttExFlg,0,3*sizeof(UINT8));
			}
			else
			{
				memset(pREcheck->AttExFlg,0x55,3*sizeof(UINT8));
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}

			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[0]*2))
			{		
				//进对日定向状态
				if (PWhl->Whl_UseCnt >= 3)
				{
					tmpNewMod = ATTMOD_WhlTOSUN;
					tmpNewModID =  ATT_CMDMODE_WhlTOSUN;
				}
				else
				{
					tmpNewMod = ATTMOD_MagTOSUN;
					tmpNewModID =  ATT_CMDMODE_MagTOSUN;
				}
				////置陀螺不可用（考虑冷备份，只给加电陀螺置不可用）
				for(i=0;i<GYRO_NUM;i++)
				{
					if(POSE_OK == pREcheck->GyroIn_PowerOn[i])
					{
						pIMU->GyroUseState[i] = POSE_NO;//使用状态置0
					}
				}
			}
			break;
		}

		case ATTMOD_WhlTOSUN:
		{
			for(i=0;i<3;i++)
			{
				if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[1])
				{
					tmpAttNOK[i] = POSE_OK;
				}
				else
				{
					tmpAttNOK[i] = POSE_NO;
				}
			}
			
			if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
			{
				pREcheck->AttExCnt=0;
			}
			else
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			
			for(i=0;i<3;i++)
			{
				pREcheck->AttExFlg[i]=tmpAttNOK[i];
			}
			
			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[1]*2))
			{
				//切磁控对日
				tmpNewMod = ATTMOD_MagTOSUN;
				tmpNewModID =  ATT_CMDMODE_MagTOSUN;
				pREcheck->AttExCnt = 0;
			}
			break;			
		}
	
		case ATTMOD_MagTOSUN:
		{
			for(i=0;i<3;i++)
			{
				if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[2])
				{
					tmpAttNOK[i] = POSE_OK;
				}
				else
				{
					tmpAttNOK[i] = POSE_NO;
				}
			}
			
			if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
			{
				pREcheck->AttExCnt=0;
			}
			else
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			for(i=0;i<3;i++)
			{
				pREcheck->AttExFlg[i]=tmpAttNOK[i];
			}
			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[2]*2))
			{
				//出口待确认
				tmpNewMod = ATTMOD_UNCTL;
				tmpNewModID =  ATT_CMDMODE_UNCTL;

			}
			break;			
		}  
		
		case ATTMOD_ONLTOEARTH : 
		{
		    //超差判断
		    for(i=0;i<3;i++)
		    {
			    if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[3])
			    {
				    tmpAttNOK[i] = POSE_OK;
			    }
				else
				{
				    tmpAttNOK[i] = POSE_NO;			
				}
		    }

			if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
		    {
			    pREcheck->AttExCnt=0;
		    }
		    else
		    {
			    if(pREcheck->AttExCnt<60000)
			    {pREcheck->AttExCnt++;}
		    }
			for(i=0;i<3;i++)
			{
				pREcheck->AttExFlg[i]=tmpAttNOK[i];
			}
		    //连续超差
		    if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[3]*2))
		    {			
				tmpNewMod = ATTMOD_RATEDMP;
				tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
		    }
		    break;
		} 

		case ATTMOD_WHEELTOEARTH:
		{
			//超差判断
			for(i=0;i<3;i++)
			{
				if((POSE_ABSF(tmpAtt->sDeterPara.CLPAttAng[i])>tmpConst->AngThr_AttEx[4])||(POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i])>tmpConst->RateThr_AttEx[4]))
				{pREcheck->AttExFlg[i]=0x55;}
				else
				{pREcheck->AttExFlg[i]=0;}
			}
			//XY超差计数
			if((POSE_OK == pREcheck->AttExFlg[0])||(POSE_OK == pREcheck->AttExFlg[1]))
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			else
			{pREcheck->AttExCnt=0;}
			//Z超差计数
			if(POSE_OK == pREcheck->AttExFlg[2])
			{
				if(pREcheck->YawCnt<60000)
				{pREcheck->YawCnt++;}
			}
			else
			{pREcheck->YawCnt=0;}
			//连续超差
			if((pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[4]*2))||
			(pREcheck->YawCnt>=(tmpConst->YawCntLimit*2)))
			{
				pREcheck->YawCnt=0;
				tmpNewMod = ATTMOD_RATEDMP;
				tmpNewModID =  ATT_CMDMODE_MODSUNCAP;
			}
			break;
		}
		
		case ATTMOD_ATTAJUSTPRE:
		{
		    //超差判断
		    for(i=0;i<3;i++)
		    {
			    if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[5])
			    {
				    tmpAttNOK[i] = POSE_OK;
			    }
				else
				{
				    tmpAttNOK[i] = POSE_NO;			
				}
		    }

			if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
		    {
			    pREcheck->AttExCnt=0;
		    }
		    else
		    {
			    if(pREcheck->AttExCnt<60000)
			    {pREcheck->AttExCnt++;}
		    }
			
			for(i=0;i<3;i++)
			{
				pREcheck->AttExFlg[i]=tmpAttNOK[i];
			}
			
		    //连续超差
		    if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[5]*2))
		    {
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =  ATT_CMDMODE_ONLTOEARTH;
				pOrb->CurOrbCtrlPcak_ID = 0;
				pOrb->CurOrbCtrlPcak_PreID =0xFF;
				PPPU->PPUT_FIR_Cnt = 0;
				pOrb->OrbDataIn_OK = POSE_NO;
				memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
				memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
				tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
		    }
		    break;
		}
		case ATTMOD_ORBITCTL:
		{
			//超差判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
				
				if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[6])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[6]))
				{pREcheck->AttExFlg[i]=0x55;}
				else
				{pREcheck->AttExFlg[i]=0;}
			}
			//任意飞轮转速大于6000rpm
			for(i=0;i<4;i++)
			{
				//if (pWhl->Whl_Rate[i] > 6000*RPM2RADS)
				if (POSE_ABSF(pWhl->Whl_Rate[i]) > (628.32 * RADS2RPM))
				{
					tmpWhlOK = POSE_NO;
					break;
				}
			}
			//超差计数
			if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 == pREcheck->AttExFlg[2]) || (POSE_NO == tmpWhlOK))
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			else
			{pREcheck->AttExCnt=0;}
			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[6]*2))
			{
				//发送进入对地定向指令
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =  ATT_CMDMODE_ONLTOEARTH;
				pOrb->CurOrbCtrlPcak_ID = 0;
				pOrb->CurOrbCtrlPcak_PreID =0xFF;
				PPPU->PPUT_FIR_Cnt = 0;
				pOrb->OrbDataIn_OK = POSE_NO;
				memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
				memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
				
				/*轨控时故障诊断异常超差，当前拍的关PPU动作*/
				dev_can_write(DEV_NO_CAN0, PPUOff, 8);
				PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
				tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
				/*2.5S后关PPU电源*/
				PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
				PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
				
				PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
				PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
			}
			break;
		}

		case ATTMOD_ATTAJUST:
		{
			//超差判断
			for(i=0;i<3;i++)
			{
				if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[7])
				{
					tmpAttNOK[i] = POSE_OK;
				}
				else
				{
					tmpAttNOK[i] = POSE_NO;		
				}
			}
			
			if((POSE_NO ==tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
			{
				pREcheck->AttExCnt=0;
			}
			else
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			
			for(i=0;i<3;i++)
			{
				pREcheck->AttExFlg[i]=tmpAttNOK[i];
			}
			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[7]*2))
			{
				//发送进入对地定向指令
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =  ATT_CMDMODE_ONLTOEARTH;
				pTARC->CurTARCtrlPcak_ID=0;
				pTARC->CurTARCtrlPcak_PreID =0xFF;
				pTARC->TARDataIn_OK = POSE_NO;
				memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
				memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
			}
			break;
		}

		case ATTMOD_ONLTOTAR:
		{
			//超差判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
				
				if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[8])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[8]))
				{pREcheck->AttExFlg[i]=0x55;}
				else
				{pREcheck->AttExFlg[i]=0;}
			}

			//超差计数
			if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 ==pREcheck->AttExFlg[2]) )
			{
				if(pREcheck->AttExCnt<60000)
				{pREcheck->AttExCnt++;}
			}
			else
			{pREcheck->AttExCnt=0;}
			//连续超差
			if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[8]*2))
			{
				//发送进入对地定向指令
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =  ATT_CMDMODE_ONLTOEARTH;
				pTARC->CurTARCtrlPcak_ID=0;
				pTARC->CurTARCtrlPcak_PreID =0xFF;
				pTARC->TARDataIn_OK = POSE_NO;
				memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
				memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
			}
			break;
		}
		default:
		break;
	}
	
	if(tmpNewMod != pWMod->WorkMode)
	{
		memset(cmdDataBuff, 0x00, 50);
		tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
		DoSetInt32To8(tmpNewModID, cmdDataBuff);
		if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
		{
			/* 在姿控的指令处理中 */
			BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
		}
	}
}

/***********************************************
说明：姿态不稳定判断
***********************************************/
void AttUnStaJudge(void)
{
	UINT8		cmdDataBuff[50] = {0};
	sTaskInfo* tmpTaskInfo = NULL;
	UINT8 i =0;
	UINT8 	tmpNewMod = 0x00;
	UINT16 	tmpNewModID = 0x00;
	TYPE_CAL tmpAngE[3]= {0.0f};//姿态角偏差
	TYPE_CAL tmpAngRateE[3] ={0.0f};//姿态角速度偏差
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttModPara_t *pWMod =NULL;	
	sAttErrCtlPara_t *pREcheck =NULL;	
	WhlPara_t *PWhl =NULL;		
	sAttOrbitCtlInfo_t *pOrb =NULL;
	sAttTARCCtlInfo_t *pTARC =NULL;
	sAttDeterPara_t *pDeter =NULL;	
	PPUPara_t *PPPU = NULL;
	sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
	sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;	

	if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
	{return;}	
	pWMod=&tmpAtt->sModePara;
	pREcheck=&tmpAtt->sErrCtlPara;
	//pCtrl=&tmpAtt->sCtlPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	tmpNewMod = pWMod->WorkMode;	
	pOrb=&tmpAtt->sOrbitInp;
	pTARC=&tmpAtt->sTARCInp;
	pDeter=&tmpAtt->sDeterPara;
	PPPU = &tmpAtt->sPerPara.PPUPara;
		
	switch(pWMod->WorkMode)
	{	
		//阻尼
		case ATTMOD_RATEDMP:
		{
			if(pREcheck->AttUnStaCnt<60000)
			{pREcheck->AttUnStaCnt++;}
			if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[0]*2))
			{
				//发送进入对日定向指令
				if (PWhl->Whl_UseCnt >= 3)
				{
					tmpNewMod = ATTMOD_WhlTOSUN;
					tmpNewModID =  ATT_CMDMODE_WhlTOSUN;
				}
				else
				{
					tmpNewMod = ATTMOD_MagTOSUN;
					tmpNewModID =   ATT_CMDMODE_MagTOSUN;
				}
			}
			break;
		}

		case ATTMOD_WhlTOSUN:
		{
			//姿态不稳定判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
				
				if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[1])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[1]))
				{pREcheck->AttUnSta[i]=0;}
				else
				{pREcheck->AttUnSta[i]=0x55;}
			}
			if((POSE_OK == pREcheck->AttUnSta[0])||(POSE_OK == pREcheck->AttUnSta[1])||(POSE_OK == pREcheck->AttUnSta[2]))
			{
				if(pREcheck->AttUnStaCnt<60000)
				{pREcheck->AttUnStaCnt++;}
			}
			else
			{pREcheck->AttUnStaCnt=0;}
			//连续不稳定判断
			if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[1]*2))
			{
				//切磁控对日
				tmpNewMod = ATTMOD_MagTOSUN;
				tmpNewModID =   ATT_CMDMODE_MagTOSUN;
			}
			break;
		}

		case ATTMOD_MagTOSUN:
		{
			//姿态不稳定判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				//tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				tmpAngE[i]=pDeter->AngToSun[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
				if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[2])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[2]))
				{pREcheck->AttUnSta[i]=0;}
				else
				{pREcheck->AttUnSta[i]=0x55;}
			}
			if((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))
			{
				if(pREcheck->AttUnStaCnt<60000)
				{pREcheck->AttUnStaCnt++;}
			}
			else
			{pREcheck->AttUnStaCnt=0;}
			
			//连续不稳定判断
			if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[2]*2))
			{
				//发送无控指令
				tmpNewMod = ATTMOD_UNCTL;
				tmpNewModID =   ATT_CMDMODE_UNCTL;
			}
			break;
		}

		case ATTMOD_ONLTOEARTH:
		{
			if(pREcheck->AttUnStaCnt<60000)
			{
				pREcheck->AttUnStaCnt++;
			}

			if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[3]*2))
			{
				tmpNewMod = ATTMOD_RATEDMP;
				tmpNewModID =   ATT_CMDMODE_MODSUNCAP;
			}
			break;
		}

		case ATTMOD_ATTAJUSTPRE:
		{
			if(pREcheck->AttModCnt<60000)
			{pREcheck->AttModCnt++;}
			//姿态不稳定判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
			
				if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[4])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[4]))
				{pREcheck->AttUnSta[i]=0;}
				else
				{pREcheck->AttUnSta[i]=0x55;}
			}
			if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[4]*2)))
			{
				if(pREcheck->AttUnStaCnt<60000)
				{pREcheck->AttUnStaCnt++;}
			}
			else
			{pREcheck->AttUnStaCnt=0;}
			//连续不稳定判断
			if(pREcheck->AttUnStaCnt>= (tmpConst->TimeThr_AttUnSta[4]*2))
			{
				//发送进入对地定向指令
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =   ATT_CMDMODE_ONLTOEARTH;
				tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
				pOrb->CurOrbCtrlPcak_ID = 0;
				pOrb->CurOrbCtrlPcak_PreID =0xFF;
				PPPU->PPUT_FIR_Cnt = 0;
				pOrb->OrbDataIn_OK = POSE_NO;
				memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
				memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
			}
			break;			
		}
	
		case ATTMOD_ATTAJUST:
		{
			if(pREcheck->AttModCnt<60000)
			{pREcheck->AttModCnt++;}
			//姿态不稳定判断
			for(i=0;i<3;i++)
			{
				//计算姿态角偏差
				tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
				DeviAngleLimit(&tmpAngE[i]);
				//计算姿态角速度偏差
				tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
				
				if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[5])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[5]))
				{pREcheck->AttUnSta[i]=0;}
				else
				{pREcheck->AttUnSta[i]=0x55;}
			}
			if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[5]*2)))
			{
				if(pREcheck->AttUnStaCnt<60000)
				{pREcheck->AttUnStaCnt++;}
			}
			else
			{pREcheck->AttUnStaCnt=0;}
			
			if(pREcheck->AttUnStaCnt > (tmpConst->TimeThr_AttUnSta[5]*2))
			{
				tmpNewMod = ATTMOD_ONLTOEARTH;
				tmpNewModID =   ATT_CMDMODE_ONLTOEARTH;
				pTARC->CurTARCtrlPcak_ID=0;
				pTARC->CurTARCtrlPcak_PreID =0xFF;
				pTARC->TARDataIn_OK = POSE_NO;
				memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
				memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));	

			}
			break;
		}

		default:
		break;
	}
	if (tmpNewMod != pWMod->WorkMode)
	{
		memset(cmdDataBuff, 0x00, 50);
		tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
		DoSetInt32To8(tmpNewModID, cmdDataBuff);
		if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
		{
			/* 在姿控的指令处理中 进行*/
			BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
		}
	}
	
}

/************************************************
 说明：模块主函数，模块每周期调用的所有操作
 输入：		
 输出：
************************************************/
void    ZKREAction(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttErrCtlPara_t *pREcheck =NULL;	
	sAttDeterPara_t *pDeter =NULL;	
	PPUPara_t *PPPU = NULL;
	UINT8 PPUOn2[8]	= {0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00};	
	if((NULL==tmpAtt) || (NULL==tmpConst))
		return;	
	pREcheck=&tmpAtt->sErrCtlPara;
	pDeter=&tmpAtt->sDeterPara;
	PPPU = &tmpAtt->sPerPara.PPUPara;
	
	//陀螺异常诊断
	GyroImuErrJudge();

	//星敏异常诊断
	SsImuErrJudge();

	//模拟磁强计异常诊断
	MagImuErrJudge();

	//飞轮异常诊断
	WhlImuErrJudge();
	
	//PPU异常诊断	
	PPUImuErrJudge();
	
	/*处理因正常轨控退出开PPU电源操作，以及轨控异常退出关PPU操作，关动作优先开*/
	if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_Err_Flag)/*电推轨控退出*/
	{
		if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt <= 250)/*对计数值做个最大值保护*/
		PPPU->PPU_OrbitCtl_Quit_Err_Cnt++;
		
		if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt >= 5)/*2.5秒后，发送电推供电关指令*/
		{
			OCOut(OC73_PPU_OFF, 0, 16);
			PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
			PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_NO;
		}
	}
	else if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*电推轨控正常退出*/
	{
		if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*对计数值做个最大值保护*/
		PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
		
		if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 5)/*5秒后，发送电推供电开指令*/
		{
			OCOut(OC68_PPU_ON, 0, 16);
			PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
			PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
		}
	}
	else if(POSE_IN == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*二次点火*/
	{
		if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*对计数值做个最大值保护*/
		PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
		
		if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt == 5)/*2.5秒后，发送电推供电开指令*/
		{
			OCOut(OC68_PPU_ON, 0, 16);
		}
		
		if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 65)/*30秒后，发送电推供电开指令*/
		{
			//PPU开机指令
			dev_can_write(DEV_NO_CAN0,PPUOn2,8);
			PPPU->PPUOUT_Sta = POSE_OK;
			tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_OK;
			PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
			PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
			pREcheck->PPUOUT_FIRECnt=0;
			pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
		}				
    }
	else 
	{;}/*不做处理*/		
	//姿态基准丢失判断
	if(0x55 == tmpConst->AllowAttLoseDiagFlg)
	{
		AttLoseJudge();
	}
	else
	{
		pREcheck->AttLoseCnt=0;
	}
	
	//姿态超差判断
	if((0x55 == tmpConst->AllowAttExDiagFlg)&&(0x55 == pDeter->CLPAttValid))
	{AttExJudge();}
	else
	{
		pREcheck->AttExFlg[0]=0x00;
		pREcheck->AttExFlg[1]=0x00;
		pREcheck->AttExFlg[2]=0x00;
		pREcheck->AttExCnt=0;
	}
	
	//姿态不稳定判断
	if((0x55 == tmpConst->AllowAttStaDiagFlg)&&(0x55 == pDeter->CLPAttValid))
	{AttUnStaJudge(); }  
	else
	{
		pREcheck->AttUnSta[0] = 0;
		pREcheck->AttUnSta[1] = 0;
		pREcheck->AttUnSta[2] = 0;
		pREcheck->AttUnStaCnt=0;		
	}
}