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

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

/*******************************************************
说明：指令分配数据初始化
*******************************************************/
void ZKCtrlCmdInit(void)
{
	UINT8 i = 0x00;
	sAttPriData *tmpAtt = NULL ;
	AttCmdDsp_t  *pCmdDsp = NULL;
	MtPara_t	*pMt = NULL;		
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL ==tmpAtt)
	return;

	pCmdDsp = &tmpAtt->sCmdDspPara;
	pMt=&tmpAtt->sPerPara.MtPara;

	//飞轮
	pCmdDsp->WheelD_MomOfForce_Cmd = 0;
	pCmdDsp->WheelD_RotatSpd_Cmd = 0;
	memset(pCmdDsp->Wheel_TwcCmd, 0, 3*sizeof(TYPE_CAL));
	memset(pCmdDsp->Wheel_LJCmd, 0, 4*sizeof(TYPE_CAL));
	memset(pCmdDsp->Whl_Torque, 0, 4*sizeof(TYPE_CAL));

	//磁力矩器
	for (i = 0; i < MT_NUM; i++)
	{
		pMt->MTOnOff[i] = MT_OFF;
		pMt->MTCtrlOutP0[i] = 0.0;
		pMt->MTDirect[i] =MT_DIRECT_POS;
	}	
}

/***********************************************
说明：PPU开关机处理
输入：
	开关状态 0x55:电推开机，0xAA:电推关机；其他：无操作
输出：
***********************************************/
//void    ZKDev_PPU_OnOffProcess(UINT8 OnState)
//{
	//sAttPriData *tmpAtt = NULL;	
	//UINT8 PPUOff[8]			= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
		//
	//tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
	//
	//if((NULL == tmpAtt) || ((0x55 != (OnState)) && (0xAA != (OnState))))
		//return;
		//
	//if (0xAA == (OnState))/*电推关机*/
	//{
		///*当前拍的关动作*/
		//dev_can_write(DEV_NO_CAN0,PPUOff,8);
		///*下一拍的关动作标志*/
		//tmpAtt->sErrCtlPara.AttPPUOffReq = POSE_OK;
	//}
	////else if (0x55 == (OnState))/*电推开机*/
	////{
		/////*当前拍的开动作*/
		////OCOut(OC68_PPU_ON, 0, 16);
		/////*下一拍的关动作标志*/
		////tmpAtt->sErrCtlPara.AttPPUOnReq = POSE_OK;
	////}
	//
//}

///***********************************************
//说明：轨道控制模块，轨控主函数.
//输入：上注轨控数据，当前模式	
//输出：PPU开关	
//***********************************************/
void    ZKPPUCmdDsp(void)
{	
//	UINT8 PPUOn[8]	= {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	UINT8 PPUOff[8]	= {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
	UINT32 SatTime[2] ={0,0};
	UINT8 cmdDataBuff[50] = {0};
	
	sTaskInfo* tmpTaskInfo = NULL;
	sAttPriData *tmpAtt = NULL;
	PPUPara_t *PPPU = NULL;	
	sAttModPara_t *pMod = NULL;
	AttIMPTPara_t *pImpt = NULL;
	sAttOrbitCtlInfo_t *pOrb =NULL;
	AttCtrlConst_t *tmpConst= NULL;	
	sAttErrCtlPara_t 	*pREcheck= NULL;			
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	
	if(NULL ==tmpAtt|| NULL ==tmpConst)
	return;
	PPPU = &tmpAtt->sPerPara.PPUPara;	
	pMod = &tmpAtt->sModePara;
	pImpt= &tmpAtt->sIMPTPara;
	pOrb=&tmpAtt->sOrbitInp;
	pREcheck=&tmpAtt->sErrCtlPara;	
		
	tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
	memset(cmdDataBuff, 0x00, 50);
	
	
	GetTime(SatTime);		//获取当前星上时
	
	//if((ATTMOD_ATTAJUSTPRE == pMod->WorkMode) || (ATTMOD_ORBITCTL == pMod->WorkMode))/*轨控期间*/
	if(ATTMOD_ORBITCTL == pMod->WorkMode)/*轨控期间*/
	{
		if (pImpt->PPUUseStatus == POSE_NO)/*电推进状态为不可用*/
		{
			//DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
			pREcheck->PPU_Diag = POSE_NO;
			DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
			pOrb->CurOrbCtrlPcak_ID = 0;
			pOrb->CurOrbCtrlPcak_PreID =0xFF;
			pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
			PPPU->PPUT_FIR_Cnt = 0;
			pOrb->OrbDataIn_OK = POSE_NO;
			if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
			{
				/* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
				BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
			}
			
			/*轨控异常，当前拍的关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;
			return ;
		}
	}

	if(POSE_OK == pOrb->OrbDataIn_OK)
	{
		/*判断当前时间，是否在轨控时间段内*/
		if (((SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+30)) &&
			(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength-30))
			))
		{
			if (ATTMOD_ORBITCTL != pMod->WorkMode)
			{
				/*不在的话退出*/
				//DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
				pOrb->CurOrbCtrlPcak_ID = 0;
				pOrb->CurOrbCtrlPcak_PreID =0xFF;
				pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
				PPPU->PPUT_FIR_Cnt = 0;
				pOrb->OrbDataIn_OK = POSE_NO;
				//if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
				//{
					///* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
					//BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
				//}
			
				/*轨控异常，当前拍的关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;
				return;				
			}

			//if ((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
			//{
				////if ((POSE_NO==tmpConst->WorkModeChangeAuto)||(POSE_OK == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
				////{
					///*不在的话退出*/
					//DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
					//pOrb->CurOrbCtrlPcak_ID = 0;
					//pOrb->CurOrbCtrlPcak_PreID =0xFF;
					//pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
					//PPPU->PPUT_FIR_Cnt = 0;
					//pOrb->OrbDataIn_OK = POSE_NO;
					//if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
					//{
						///* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
						//BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
					//}
				//
					///*轨控异常，当前拍的关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;
					//return;
				////}
				////else if (POSE_OK==tmpConst->WorkModeChangeAuto)
				////{
					/////*轨控异常，当前拍的关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_IN;
					////PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
					////return;
				////} 
				////else
				////{
					////;
				////}	
			//}			
		}
	}

	
	if((POSE_OK == pOrb->OrbDataIn_OK) && (ATTMOD_ORBITCTL == pMod->WorkMode))
	{
		/*判断当前时间，是否在轨控时间段内*/
		if (!(
				(SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime-2)) &&
				(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength+2))
			))
		{
			/*不在的话退出*/
			DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
			pOrb->CurOrbCtrlPcak_ID = 0;
			pOrb->CurOrbCtrlPcak_PreID =0xFF;
			pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
			PPPU->PPUT_FIR_Cnt = 0;
			pOrb->OrbDataIn_OK = POSE_NO;
			if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
			{
				/* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
				BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
			}
			
			/*轨控异常，当前拍的关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;

			return;
		}
	}
}


/*******************************************************
说明：发送飞轮指令
输入：ID  飞轮转速（RPM） 飞轮模式
输出：
*******************************************************/
void SetWheel(UINT8 WheelId, TYPE_CAL Value, UINT8 CmdType)
{
	UINT8   i = 0;
	UINT8   sum = 0;
	UINT16  tmpLen = 0 ;
	UINT16  WhlLen = 8;
	INT16   tmpI =0 ;
	UINT8   tmpWhl_order[8] = {0X01,WHL_CMD_CODE_RATE,0xFF,0x0,0x0,0x0,0x0,0x0};  //飞轮控制指令组包
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	sAttDataPrePara_t *pData = NULL;
	AttCmdDsp_t  *pCmdDsp = NULL;
	sDevInfo * tmpDevInfo = NULL;
	WhlPara_t *PWhl = NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL ==tmpAtt) || (NULL ==tmpConst))
	return;
	pData = &tmpAtt->sPerPara;
	pCmdDsp = &tmpAtt->sCmdDspPara;
	tmpDevInfo = GETDEVINFO(DEV_NO_UART8 + WheelId - 1);
	PWhl = &tmpAtt->sPerPara.WhlPara;

	tmpWhl_order[0] = tmpConst->Whl_ID[WheelId -1];
	if (WHL_CMD_CODE_RATE ==  CmdType)  /* 转速指令 */
	{
		Value = Value * RPM2RADS;					
		DoSetFloatTo8(Value,tmpWhl_order+3);
		sum = 0;
		for(i = 0; i < 7; i++)
		{sum += tmpWhl_order[i];}
		sum = 0xFF - sum;			
		tmpWhl_order[7]= sum;

		tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
		if(tmpLen != WhlLen)/*表示发送成功*/
		{
			PWhl->WhlIn_DataSta[WheelId-1]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 5, 1 );
		}	
	}
	else if ( WHL_CMD_CODE_TRQ == CmdType )   /* 力矩指令 */
	{
		tmpWhl_order[1]= WHL_CMD_CODE_TRQ;
		if (POSE_ABSF(Value) < 0.1)
		{		
			DoSetFloatTo8(Value,tmpWhl_order+3);
			sum = 0;
			for(i = 0; i < 7; i++)
			{sum += tmpWhl_order[i];}
			sum = 0xFF - sum;			
			tmpWhl_order[7]= sum;
			tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
			if(tmpLen != WhlLen)/*表示发送成功*/
			{
				PWhl->WhlIn_DataSta[WheelId-1]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 6, 1 );
			}
		}
		else
		{
			PWhl->WhlIn_DataSta[WheelId-1]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 6, 1 );
		}
	}
	else if ( WHL_CMD_CODE_CURR == CmdType)   /* 电流指令 */
	{
		tmpWhl_order[1]= WHL_CMD_CODE_CURR;
		if (POSE_ABSF(Value) < 0.1)
		{	
			tmpWhl_order[3] = 0xFF;
			tmpWhl_order[4] = 0xFF; 
			tmpI =(INT16)(Value / 0.036 * 1000000.0 /103.0);			
			DoSetInt16To8(tmpI,tmpWhl_order+5);
			sum = 0;
			for(i = 0; i < 7; i++)
			{sum += tmpWhl_order[i];}
			sum = 0xFF - sum;			
			tmpWhl_order[7]= sum;
			tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
			if(tmpLen != WhlLen)/*表示发送成功*/
			{
				PWhl->WhlIn_DataSta[WheelId-1]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 7, 1 );
			}			
		}
		else
		{
			PWhl->WhlIn_DataSta[WheelId-1]      = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 7, 1 );
		}
	}

	//遥测下发记录
	if( WHL_CMD_CODE_TRQ== CmdType)
	{
		pCmdDsp->Wheel_LJCmd[WheelId-1]=Value;

		if (tmpConst->Whl_J[WheelId-1] > POSE_ZERO)
		{
			pCmdDsp->Whl_Torque[WheelId-1]= pData->WhlPara.Whl_Rate[WheelId-1] + Value  * 0.5 * RADS2RPM  /tmpConst->Whl_J[WheelId-1];
		}
	}
	else if( WHL_CMD_CODE_RATE== CmdType)
	{
		pCmdDsp->Whl_Torque[WheelId-1]=Value * RADS2RPM ;
	}
}

/***********************************************
说明：反作用轮指令分配
输入：
输出：	
返回:	
***********************************************/
void    ZKWhlCmdDsp(void)
{
	UINT8		i =0x00;
	UINT8		j =0x00;
	UINT8		tmpReversibleFlg =0x00;
	UINT8		UnUseWheelId =0x00;//记录下不使用的是哪个飞轮，指令发送给其他三个，1234对应ABCD
	TYPE_CAL	tmpCu[9]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};//飞轮分配矩阵
	TYPE_CAL    tmpCp[9]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
	TYPE_CAL	tmpUse = 0.0;
	TYPE_CAL	tmpTwc[3]={0.0,0.0,0.0};//三个飞轮的控制指令
	TYPE_CAL    tmpTwi_Max =0.0;
	TYPE_CAL    tmpTwc_Max =0.0;
	
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst = NULL;
	sAttModPara_t *pMod = NULL;	
	WhlPara_t *PWhl = NULL;
	AttIMPTPara_t *pImpt = NULL;
	sAttDataPrePara_t *pData = NULL;	
	sAttCtlPara_t  *pCtl = NULL;
	AttCmdDsp_t  *pCmdDsp = NULL;
	sAttErrCtlPara_t 	*pREcheck = NULL;
	
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	if((NULL ==tmpAtt) || (NULL ==tmpConst))
	{
		return;
	}
	pMod =&tmpAtt->sModePara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	pImpt=&tmpAtt->sIMPTPara;
	pData = &tmpAtt->sPerPara;
	pCtl = &tmpAtt->sCtlPara;
	pCmdDsp = &tmpAtt->sCmdDspPara;
	pREcheck=&tmpAtt->sErrCtlPara;

	if((ATTMOD_RATEDMP == pMod->WorkMode)||(ATTMOD_MagTOSUN == pMod->WorkMode)||(ATTMOD_UNCTL == pMod->WorkMode))
	{
		return;
	}

	pREcheck->WhlUseState = 0;
	for(i=0;i<4;i++)
	{
		pREcheck->WhlUseState =(pREcheck->WhlUseState |((POSE_OK == pImpt->Whl_Use[i]) << i));		
	}	
	
	if(count_onebit(pREcheck->WhlUseState,4)<3)
	{
		return;
	}
	
	//计算飞轮分配矩阵tmpCp
	//XYZS/XYZ
	UnUseWheelId=0;
	if((0x0f ==pREcheck->WhlUseState)||(0x07 == pREcheck->WhlUseState))
	{
		memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][0], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][0], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][0], 3*sizeof(TYPE_CAL));
		UnUseWheelId=4;
	}
	//YZS
	else if(0x0e == pREcheck->WhlUseState)
	{
		memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][1], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][1], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][1], 3*sizeof(TYPE_CAL));
		UnUseWheelId=1;
	}
	//XZS
	else if(0x0d == pREcheck->WhlUseState)
	{
		memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][1], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][1], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][1], 3*sizeof(TYPE_CAL));
		tmpCu[0]=tmpConst->AttCmdFlashPara.M_Wheel[0][0];
		tmpCu[3]=tmpConst->AttCmdFlashPara.M_Wheel[1][0];
		tmpCu[6]=tmpConst->AttCmdFlashPara.M_Wheel[2][0];
		UnUseWheelId=2;
	}
	//XYS
	else if(0x0b == pREcheck->WhlUseState)
	{
		memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][0], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][0], 3*sizeof(TYPE_CAL));
		memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][0], 3*sizeof(TYPE_CAL));
		tmpCu[2]=tmpConst->AttCmdFlashPara.M_Wheel[0][3];
		tmpCu[5]=tmpConst->AttCmdFlashPara.M_Wheel[1][3];
		tmpCu[8]=tmpConst->AttCmdFlashPara.M_Wheel[2][3];
		UnUseWheelId=3;
	}
	tmpUse=1.0e-4;
	InvsymMatrix3(tmpCu, tmpCp, &tmpReversibleFlg, &tmpUse);
	if(!tmpReversibleFlg)
	{
		return;
	}
	
	//计算反作用轮控制指令(限幅)tmpTwc
	MatrixProductHL(tmpCp, pCtl->WheelCtrlResult, tmpTwc, 3, 3, 1);
	
	for(i=0;i<3;i++)
	{
		if(POSE_ABSF(tmpTwc[i])>POSE_ABSF(tmpTwc_Max))
		{
			tmpTwi_Max=tmpTwc[i];
			tmpTwc_Max=tmpTwc[i];
		}
	}

	if(POSE_ABSF(tmpTwc_Max)>0.1)
	{
		if (tmpTwc_Max > 0)
		{
			tmpTwc_Max=0.1;
		} 
		else
		{
			tmpTwc_Max=-0.1;
		}

		if(POSE_ABSF(tmpTwi_Max) > POSE_ZERO)
		{
			for(i=0;i<3;i++)
			tmpTwc[i]=tmpTwc[i]*tmpTwc_Max/tmpTwi_Max;
		}
	}

	memcpy(pCmdDsp->Wheel_TwcCmd,tmpTwc,3*sizeof(TYPE_CAL));
	//发送飞轮指令
	if(0 != UnUseWheelId)
	{
		j=0;
		for(i=1;i<=4;i++)
		{
			if(i!=UnUseWheelId)
			{
				if (WHL_CMD_CODE_RATE == PWhl->Whl_WorkMode[i-1])
				{
					if (tmpConst->Whl_J[j] > POSE_ZERO)
					{
						//tmpTwc[j] = pData->WhlPara.Whl_Rate[j] + tmpTwc[j] * 0.5 /PWhl->Whl_J[j]* RADS2RPM  ;
						tmpTwc[j] = pData->WhlPara.Whl_Rate[i-1] + tmpTwc[j] * 0.5 /tmpConst->Whl_J[j]* RADS2RPM  ;
					}
				}
				else if (WHL_CMD_CODE_TRQ == PWhl->Whl_WorkMode[i-1])
				{
					;
				}
				SetWheel(i,tmpTwc[j],PWhl->Whl_WorkMode[i-1]);
				j++;
			}
		}
	}
}

/***********************************************
说明：配平轮指令分配
***********************************************/
void ZKWhlDCmdDsp(void)
{	
	UINT8 i = 0x00;
	TYPE_CAL tmpT[3] ={0.0,0.0,0.0};
	TYPE_CAL tmpWhlM[3] ={0.0,0.0,0.0};
	TYPE_CAL tmpWhlRate = 0.0;
	sAttPriData *tmpAtt =NULL;
	AttCtrlConst_t *tmpConst=NULL;
	AttCmdDsp_t  *pCmdDsp=NULL;
	WhlPara_t	*pWhl=NULL;
	sAttDataPrePara_t *pData=NULL;
	sAttErrCtlPara_t 	*pREcheck=NULL;

	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if(NULL ==tmpAtt || NULL ==tmpConst)
	{
		return;
	}
	for(i=0;i<3;i++)
	{
		tmpWhlM[i]= tmpConst->AttCmdFlashPara.M_Wheel[i][3];
	}
	//pImpt=&tmpAtt->sIMPTPara;
	pCmdDsp = &tmpAtt->sCmdDspPara;
	pWhl=&tmpAtt->sPerPara.WhlPara;
	pData = &tmpAtt->sPerPara;
	pREcheck=&tmpAtt->sErrCtlPara;

	if(0x0f != pREcheck->WhlUseState)
	return;
	
	if(POSE_ABSF(tmpConst->WheelD_NomSpeed-pWhl->Whl_Rate[3])<tmpConst->WheelD_SpeedErrThr)
	{
		//飞轮D转速指令为标称转速
		pCmdDsp->WheelD_RotatSpd_Cmd=tmpConst->WheelD_NomSpeed;
		//发送飞轮D转速指令
		SetWheel(4,pCmdDsp->WheelD_RotatSpd_Cmd,WHL_CMD_CODE_RATE);
	}
	else
	{
		//计算配平轮力矩指令
		pCmdDsp->WheelD_MomOfForce_Cmd=tmpConst->WheelD_Default;
		for(i=0;i<3;i++)
		{
			tmpT[i]=pCmdDsp->Wheel_TwcCmd[i]+tmpWhlM[i]*tmpConst->WheelD_Default;
			if(POSE_ABSF(tmpT[i])>tmpConst->WheelD_AllDefault)
			{
				pCmdDsp->WheelD_MomOfForce_Cmd=0;
				break;
			}
		}
		//发送飞轮D力矩指令
		tmpWhlRate = tmpConst->WheelD_NomSpeed -  pData->WhlPara.Whl_Rate[3];
		if (tmpWhlRate > 0.0)
		{
			tmpWhlRate = 1.0;
		} 
		else
		{
			tmpWhlRate = - 1.0;
		}
		pCmdDsp->WheelD_RotatSpd_Cmd = pData->WhlPara.Whl_Rate[3] + tmpWhlRate * pCmdDsp->WheelD_MomOfForce_Cmd * 0.5 /tmpConst->Whl_J[3] * RADS2RPM  ;
		SetWheel(4,pCmdDsp->WheelD_RotatSpd_Cmd,WHL_CMD_CODE_RATE);
	}
}

/***********************************************
说明：飞轮指令分配，反作用轮通过422控制
***********************************************/
void ActuatorCmdDsp(void)
{
	sAttPriData *tmpAtt = NULL;
	sAttModPara_t *pMod= NULL;
	WhlPara_t	*pWhl= NULL;
	AttCmdDsp_t  *pCmdDsp= NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL == tmpAtt)
	return;
	pMod =&tmpAtt->sModePara;	
	pWhl=&tmpAtt->sPerPara.WhlPara;
	pCmdDsp = &tmpAtt->sCmdDspPara;

#ifdef  MINMODULE_TYPE
	switch (pMod->WorkMode)
	{
		case	ATTMOD_MagTOSUN:
		case	ATTMOD_RATEDMP:   
		{
			if(pCmdDsp->Wheel_Cmd_Cnt<60000)
				pCmdDsp->Wheel_Cmd_Cnt++;
			
			if (pCmdDsp->Wheel_Cmd_Cnt <= 3)
			{
				SetWheel(1,0,WHL_CMD_CODE_CURR);
				SetWheel(2,0,WHL_CMD_CODE_CURR);
				SetWheel(3,0,WHL_CMD_CODE_CURR);
				SetWheel(4,0,WHL_CMD_CODE_CURR);
			}
		}
		break;
		default:
		break;
	}
#else	
	switch (pMod->WorkMode)
	{
		case	ATTMOD_UNCTL:
		break;
		case	ATTMOD_WhlTOSUN:	        //(0x33)轮控对日定向
		case	ATTMOD_ONLTOEARTH:		    //(0x55)对地定向
		case	ATTMOD_WHEELTOEARTH:		//(0x66)稳态对地
		case	ATTMOD_ATTAJUSTPRE:		    //(0x77)轨控前调姿
		case	ATTMOD_ORBITCTL:			//(0x88)轨道控制
		case	ATTMOD_ATTAJUST:			//(0xAA)定向前调姿
		case	ATTMOD_ONLTOTAR:			//(0xBB)对目标定向

		if (pWhl->Whl_UseCnt >= 3)
		{
			ZKWhlCmdDsp();
			ZKWhlDCmdDsp();
		}
		break;
		
		case    ATTMOD_NOCOLL:
		if (pWhl->Whl_UseCnt >= 3)
		{
			ZKWhlCmdDsp();
		}
		break;
		
		case	ATTMOD_RATEDMP:                             //ZMX20230920ADD
		case	ATTMOD_MagTOSUN:
		{
			if(pCmdDsp->Wheel_Cmd_Cnt<60000)
				pCmdDsp->Wheel_Cmd_Cnt++;
			
			if (pCmdDsp->Wheel_Cmd_Cnt <= 3)
			{
				SetWheel(1,0,WHL_CMD_CODE_CURR);
				SetWheel(2,0,WHL_CMD_CODE_CURR);
				SetWheel(3,0,WHL_CMD_CODE_CURR);
				SetWheel(4,0,WHL_CMD_CODE_CURR);
			}
		}
		break;	
					
		default:
		break;
	}

#endif	
}

/********************************************************************
 功能：开关磁力矩器限幅
 输入：
 输出：
 *********************************************************************/
 void MTOcOutProc(TYPE_CAL *MTIn,TYPE_CAL *MTOut, UINT8 *MTOnOff, UINT8 *MTDirect)
{
	UINT8 i= 0x00;
	TYPE_CAL tmpMagMoment = 0.0;
	//TYPE_CAL TIME = 1.0;
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst= NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt)||(NULL==tmpConst))
	{
		return;
	}
	
	for (i = 0; i < MT_NUM; ++i)
	{
		tmpMagMoment = MTIn[i];
		if (POSE_ABSF(tmpMagMoment) > tmpConst->MTOutXYZLimit[i])  //>P0
		{
			MTOnOff[i] = MT_ON;
			if (tmpMagMoment > tmpConst->MTOutXYZLimit[i])
			{
				MTDirect[i] = MT_DIRECT_POS;  //正开
				MTOut[i] = 500* (TYPE_CAL)tmpConst->MTCtrlTIME;
			}
			else
			{
				MTDirect[i] = MT_DIRECT_NEG;  //负开
				MTOut[i] = 500* (TYPE_CAL)tmpConst->MTCtrlTIME;
			}
		}
		else 
		{
			MTOnOff[i] = MT_ON;

			if  (tmpMagMoment< 0)
			{
				MTDirect[i] = MT_DIRECT_NEG;
			}
			else
			{
				MTDirect[i] = MT_DIRECT_POS;				
			}

			MTOut[i] = POSE_ABSF( tmpMagMoment )/tmpConst->MTOutXYZLimit[i] * 500.0* (TYPE_CAL)tmpConst->MTCtrlTIME;
		}

		if (MTOut[i]  < 100.0)
		{
			MTOnOff[i] = MT_OFF;
		}
	}
}

/********************************************************************
 功能：磁力矩器控制
 输入：
 输出：
 *********************************************************************/
void  ZKMtCmdDsp(void)
{
	UINT8		i	= 0x00;
	UINT8		chn = 0x00;
	UINT8		dir = 0x00;
	UINT8		tmpReversibleFlg = 0x00;
	UINT16		delay_ms = 0;
	UINT32		pulse_ms = 0;
	UINT32		tmpCNT = 0;
	TYPE_CAL	tmpMTin[MT_NUM] ={0.0,0.0,0.0,0.0};
	TYPE_CAL	tmpM_MAGCtrl[3][4] ={{0.0F,0.0F,0.0F,0.0F}, {0.0F,0.0F,0.0F,0.0F}, {0.0F,0.0F,0.0F,0.0F}};		//磁力矩器安装矩阵
	TYPE_CAL	tmpM_MAGCtrlFP1[4][3]={{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	tmpM_MAGCtrlFP[4][3]={{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	tmpM_MAGAA[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};				//磁力矩器分配矩阵中间量
	TYPE_CAL	tmpM_MAGAANI[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};				//磁力矩器分配矩阵中间量求逆
	TYPE_CAL	tmpUse = 0.0001F;
	sAttPriData *tmpAtt = NULL;
	AttCtrlConst_t *tmpConst= NULL;
	MtPara_t	*pMt= NULL;
	sAttCtlPara_t *pCtrl= NULL;
	AttIMPTPara_t *pImpt= NULL;
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt)||(NULL==tmpConst))
	{
		return;
	}
	pMt=&tmpAtt->sPerPara.MtPara;	
	pCtrl=&tmpAtt->sCtlPara;
	pImpt=&tmpAtt->sIMPTPara;

	memset(&tmpM_MAGCtrl[0][0], 0, 12*sizeof(TYPE_CAL));
	if (POSE_OK == pImpt->MTUseStatus[0])
	{
		tmpM_MAGCtrl[0][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][0];
		tmpM_MAGCtrl[1][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][0];
		tmpM_MAGCtrl[2][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][0];
	}
		
	if (POSE_OK == pImpt->MTUseStatus[1])
	{
		tmpM_MAGCtrl[0][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][1];
		tmpM_MAGCtrl[1][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][1];
		tmpM_MAGCtrl[2][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][1];
	}

	if (POSE_OK == pImpt->MTUseStatus[2])
	{
		tmpM_MAGCtrl[0][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][2];
		tmpM_MAGCtrl[1][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][2];
		tmpM_MAGCtrl[2][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][2];
	}
	
	if (POSE_OK == pImpt->MTUseStatus[3])
	{
		tmpM_MAGCtrl[0][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][3];
		tmpM_MAGCtrl[1][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][3];
		tmpM_MAGCtrl[2][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][3];
	}
	//计算转置	
    MatrixTransposeHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], 3, 4);
	//单位阵
	MatrixProductHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], tmpM_MAGAA[0], 3, 4, 3);
	//求逆
	//tmpUse=1.0e-4;
	tmpUse=0.0001F;
	InvsymMatrix3(tmpM_MAGAA[0], tmpM_MAGAANI[0], &tmpReversibleFlg, &tmpUse);
	if(!tmpReversibleFlg)
	return;	 
	MatrixProductHL(tmpM_MAGCtrlFP1[0], tmpM_MAGAANI[0], tmpM_MAGCtrlFP[0], 4, 3, 3);

	MatrixProductHL(tmpM_MAGCtrlFP[0], pCtrl->MagCtrlResult, tmpMTin, 4, 3, 1);
		
	pCtrl->MagCtrlXYZZ[0] =tmpMTin[0];
	pCtrl->MagCtrlXYZZ[1] =tmpMTin[1];
	pCtrl->MagCtrlXYZZ[2] =tmpMTin[2];
	pCtrl->MagCtrlXYZZ[3] =tmpMTin[3];
	
	//磁力矩器限幅 
	MTOcOutProc(tmpMTin, pMt->MTCtrlOutP0,  pMt->MTOnOff, pMt->MTDirect );
	
	//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
	tmpCNT = pMt-> MTCtrlTime % 10;
	//if (((tmpCNT >=1)&&(tmpCNT <= (UINT32)tmpConst->MTCtrlTIME))||( 0x88 == pMag->MagUseSwich))
	if ((tmpCNT >=1)&&(tmpCNT <= (UINT32)tmpConst->MTCtrlTIME))
	{
		if (tmpCNT ==1)
		{
			for (i = 0; i < MT_NUM; i++)
			{	
				//输出磁力矩占控比
				//chn:通道号，0x11:X, 0x22:Y, 0x33:Z，0x44:S
				//dir:方向，0：正向，1：反向
				//delay_ms:启动延时，单位毫秒 （有效值 1--1000）
				//pluse_ms:脉冲宽阔，单位毫秒 （有效值 1--1800000）
				chn = chn + 0x11;
				if (MT_DIRECT_POS == pMt->MTDirect[i])
				{
					dir = 0;
				}
				else if (MT_DIRECT_NEG == pMt->MTDirect[i])
				{
					dir = 1;
				}
				pulse_ms = (UINT32)(pMt->MTCtrlOutP0[i]);				
	
				if (MT_ON == pMt->MTOnOff[i])
				{
					magnetic_out( chn, dir, delay_ms, pulse_ms);
				}
				else
				{
					//输出磁力矩占控比为0
					dir = 0;
					pulse_ms = 0;
					magnetic_out( chn, dir, delay_ms, pulse_ms);
				}
			}
		}
	} 
	else
	{
		//输出磁力矩占控比为0
		for (i = 0; i < MT_NUM; i++)
		{
			chn = chn + 0x11;
			dir = 0;
			pulse_ms = 0;
			magnetic_out( chn, dir, delay_ms, pulse_ms);
		}
	}
	
}

/***********************************************
说明：指令分配模块主函数，所有动作均指定

注意:  分离后启动
***********************************************/
void    ZKCmdDspAct(void)
{

#ifdef  MINMODULE_TYPE
	//指令初始化
	ZKCtrlCmdInit();
	//磁力矩器
	ZKMtCmdDsp();
	//飞轮指令分配主模块
	ActuatorCmdDsp();
#else
	//指令初始化
	ZKCtrlCmdInit();
	//飞轮指令分配主模块
	ActuatorCmdDsp();
	//磁力矩器
	ZKMtCmdDsp();
	//PPU
	ZKPPUCmdDsp();
#endif
}