/*
 * Created: 2022/11/4 11:02:40
 *  Author:  wangzk zhengmengxing
 */ 
#include	"..\PrjCommon\CommonDef.h"
//#include	"..\PrjCommon\BlockRing.h"
#include	"..\PrjCommon\DataStorDefine.h"
#include	"..\PrjCommon\DevDefine.h"
//#include	"..\PrjCommon\DevDrvs.h"
//#include	"..\PrjPowCtrlMng\PowCtlMng.h"
#include 	"AttMath.h"
#include 	"AttCtrlMain.h"

/********************************************************************/
//extern sAttPriData	*m_pZKPriInfo;

/***********************************************
说明：姿控工作模式选择初始化,在模式切换时候进行部分参数初始化
输入：无
输出：
返回:
注意: 模式切换时候调用一次
***********************************************/
void 	ZKModChgInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCmdDsp_t  *pCmdDsp= NULL;
	if(NULL == tmpAtt)
	{return;}
	
	pCmdDsp = &tmpAtt->sCmdDspPara;
	
	//切换到新模式后，将相关计数器清零
#ifdef  MINMODULE_TYPE
	tmpAtt->sModePara.WModCnt2[0]=0;							//2、速率阻尼
	tmpAtt->sModePara.WModCnt2[1]=0;							//2、速率阻尼
#else
	//pAttCtlPara->sWModePara.WModCnt1=0;						//1、碰撞规避模式
	tmpAtt->sModePara.WModCnt2[0]=0;							//2、速率阻尼
	tmpAtt->sModePara.WModCnt2[1]=0;							//2、速率阻尼
	tmpAtt->sModePara.WModCnt3=0;								//3、轮控对日定向
	tmpAtt->sModePara.WModCnt4=0;								//4、磁控对日定向
	tmpAtt->sModePara.WModCnt5=0;								//5、对地定向
	tmpAtt->sModePara.WModCnt6=0;								//6、稳态对地
	tmpAtt->sModePara.WModCnt7=0;								//7、轨控前调姿
	tmpAtt->sModePara.WModCnt8=0;								//8、轨道控制
	tmpAtt->sModePara.WModCntA=0;								//11、对目标定向
	
	pCmdDsp->Wheel_Cmd_Cnt = 0;	/*飞轮电流指令发送计数*/
	
	tmpAtt->sCtlPara.PD2PIDCnt=0;								//PD转PID计数
	tmpAtt->sCtlPara.CtrlPD2PID=0;								//
	memset(tmpAtt->sCtlPara.SumAerrPre, 0.0, 3*sizeof(TYPE_CAL));	//PID积分器清零
	
	tmpAtt->sErrCtlPara.AttLoseCnt=0;//模式切换时不正常计数清零
	
	tmpAtt->sErrCtlPara.AttExCnt=0;
	tmpAtt->sErrCtlPara.AttExFlg[0]=0x00;
	tmpAtt->sErrCtlPara.AttExFlg[1]=0x00;
	tmpAtt->sErrCtlPara.AttExFlg[2]=0x00;
	
	tmpAtt->sErrCtlPara.AttUnStaCnt=0;
	tmpAtt->sErrCtlPara.AttModCnt=0;
	tmpAtt->sErrCtlPara.AttUnSta[0] = 0;
	tmpAtt->sErrCtlPara.AttUnSta[1] = 0;
	tmpAtt->sErrCtlPara.AttUnSta[2] = 0;
	tmpAtt->sErrCtlPara.YawCnt = 0;	

#endif	
}

/***********************************************
说明：设置当前模式
输入：工作模式	
输出：当前工作模式	
返回: 无	
注意: 	
***********************************************/
void 	ZKModDoSet(UINT8 newWMode)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;

	if(NULL == tmpAtt)
	{return;}

	if (newWMode == tmpAtt->sModePara.WorkMode)
	{return;}

	
	tmpAtt->sModePara.WorkModePre = tmpAtt->sModePara.WorkMode;
	tmpAtt->sModePara.WorkMode = newWMode;
	tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;
}

/***********************************************
说明：姿控工作模式选择初始化,参数初始化
注意: 开始调用一次
***********************************************/
void	ZKModInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	sAttModPara_t *pWMod =NULL;	
	AttCmdDsp_t  *pCmdDsp= NULL;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}	
	pWMod =&tmpAtt->sModePara;
	pCmdDsp = &tmpAtt->sCmdDspPara;
	//工作模式初始化为碰撞规避
#ifdef  MINMODULE_TYPE
	pWMod->WorkMode = ATTMOD_RATEDMP;
#else
	pWMod->WorkMode = ATTMOD_NOCOLL;
#endif
	pWMod->ReDataBK = POSE_NO;
	pWMod->ATTFlashSwitch= POSE_NO;	
	pWMod->Mod2UnctlFirst=0x55;
	pWMod->WorkModePre = ATTMOD_NOCOLL;
	tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;

	pWMod->ZKPowOnFirst =0;						//首次分离/计算机冷启动
	pWMod->ZKPowOnStep =0;						//首次分离/计算机冷启动加电序列
	//pWMod->ModeTRATEDMP =0;	
	tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO;	//二次点火允许标志，默认禁止
	
	//定义连续判断时长（单位为s）（可上注）
	//tmpConst->judge_time1 =200;
	tmpConst->judge_time1 =180;      //ZMX20240115
	tmpConst->judge_time2 = 60;
	tmpConst->judge_time3=30;
	tmpConst->AttCmdFlashPara.judgeWait_time=6000;
	tmpConst->judge_time4 = 0;
	tmpConst->judge_time5 = 30;
	tmpConst->judge_time6 = 0;
	tmpConst->judge_time7 = 0;
	tmpConst->judge_time8 = 0;
	tmpConst->judge_time9 = 30;
	tmpConst->judge_timeA = 0;
	tmpConst->judge_timeB = 120;
	
	//判断阈值初始化（可上注）
	tmpConst->RateThr_2T3 = 0.3f * ANG2RADIAN;		//0.3°/s
	tmpConst->HWHLThr_2T3 = 0.5f;
	tmpConst->AngThr_3TB = 2.0f * ANG2RADIAN;		//2.0° --对日定向转无控模式角度阈值
	tmpConst->RateThr_3TB = 0.1f * ANG2RADIAN;		//0.1°/s--对日定向转无控模式角速度阈值
	tmpConst->AngThr_5T6 = 2.0f * ANG2RADIAN;   		//对地定向模式切换到稳定对地模式角度阈值
	tmpConst->RateThr_5T6 = 0.1f * ANG2RADIAN;       //对地定向模式切换到稳定对地模式角速度阈值
	tmpConst->AngThr_9TA = 2.0f * ANG2RADIAN;   		
	tmpConst->RateThr_9TA = 0.1f * ANG2RADIAN;       

	//计数器初始化
	pWMod->WModCnt2[0]=0;					    //2、速率阻尼
	pWMod->WModCnt2[1]=0;						//2、速率阻尼
	pWMod->WModCnt3=0;							//3、轮控对日定向
	pWMod->WModCnt4=0;							//4、磁控对日定向
	pWMod->WModCnt5=0;							//5、对地定向
	pWMod->WModCnt6=0;							//6、稳态对地
	pWMod->WModCnt7=0;							//7、轨控前调姿
	pWMod->WModCnt8=0;							//8、轨道控制
	pWMod->WModCntA=0;							//11、对目标定向
	pWMod->WModCntB= 0;
	pWMod->WModCnt1 = 0;
	
	pCmdDsp->Wheel_Cmd_Cnt = 0;
	
}


/***********************************************
说明：姿控工作模式中计数的管理，
输入：当前工作模式，闭环姿态数据	
输出：模式计数值
返回:	
注意: 每0.5秒调用一次	
***********************************************/
void ZKDoModCal(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttModPara_t *pWMod =NULL;
	sAttDeterPara_t *pDeter =NULL;
	WhlPara_t *PWhl =NULL;
	//AttIMPTPara_t *pImpt;	
		
	//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pWMod=&tmpAtt->sModePara;
	pDeter=&tmpAtt->sDeterPara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	//pImpt=&tmpAtt->sIMPTPara;
	
#ifdef  MINMODULE_TYPE
	switch(pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP://速率阻尼转对日判断计数
		{   
			//滚动、俯仰和偏航各轴的惯性角速率绝对值连续10s不大于0.3°/s&&闭环姿态数据有效
			if(
			(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
			(pDeter->CLPAttValid==0x55))
			{
				if(pWMod->WModCnt2[0]<60000)
				{pWMod->WModCnt2[0]++;}
			}
			else
			{
				pWMod->WModCnt2[0]=0;
			}

			//反作用轮卸载角动量和小于0.5Nms
			if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
			{
				if(pWMod->WModCnt2[1]<60000)
				{pWMod->WModCnt2[1]++;}
			}
			else
			{
				pWMod->WModCnt2[1]=0;
			}
			break;
		}
		default:
		{
			break;
		}
	}
#else
	switch(pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP://速率阻尼转对日判断计数
		{   
			//连续60s，闭环姿态数据有效标志为有效，闭环姿态角速度（滚动、俯仰和偏航各轴的惯性姿态角速率），在±0.3°/s内
			if(
			(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
			(0x55 == pDeter->CLPAttValid))
			{

				if(pWMod->WModCnt2[0]<60000)
				{pWMod->WModCnt2[0]++;}
			}
			else
			{
				pWMod->WModCnt2[0]=0;
			}

			/*连续60s，输出转速有效标志有效且反作用轮卸载三轴角动量和模小于0.5Nms*/
			//反作用轮卸载角动量和小于0.5Nms
			if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
			{
				if(pWMod->WModCnt2[1]<60000)
				{pWMod->WModCnt2[1]++;}
			}
			else
			{
				pWMod->WModCnt2[1]=0;	
			}			
			break;
		}
		case ATTMOD_WhlTOSUN://对日定向转无控模式
		//case ATTMOD_MagTOSUN://对日定向转无控模式
		{
			if((0x55 == pDeter->CLPAttValid) && (0x00 == pWMod->WModSadaOpen) &&       //帆板展开标志为未展开状态  注意帆板未展开未00，展开为55
			//(POSE_ABSF(pDeter->CLPAttAng[1])<=tmpConst->AngThr_3TB) &&
			//(POSE_ABSF(pDeter->CLPAttAng[2])<=tmpConst->AngThr_3TB) &&
			(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_3TB) &&                      //20230926
			(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_3TB) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_3TB) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_3TB) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_3TB))
			{
				if(pWMod->WModCnt3<60000)
				{pWMod->WModCnt3++;}
			}
			else {pWMod->WModCnt3=0;}
			break;
		}
		
		case ATTMOD_UNCTL://无控模式转对日定向
		{
			if (0x55 == pWMod->WModSadaOpen)
			{
				if(pWMod->WModCntB<60000)    //！！！WModCntB需要备份
				{pWMod->WModCntB++;}
			}
			break;
		}
		
		case ATTMOD_ONLTOEARTH://对地定向模式切换到稳定对地模式条
		{
			if((0x55 == pDeter->CLPAttValid) &&
			(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_5T6) &&
			(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_5T6) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_5T6) &&
			(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_5T6)
			//(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
			)
			{
				if(pWMod->WModCnt5<60000)
				{pWMod->WModCnt5++;}
			}
			else {pWMod->WModCnt5=0;}
			break;
		}

		//case ATTMOD_ATTAJUST://对地定向模式切换到稳定对地模式条
		//{
			//if((0x55 == pDeter->CLPAttValid) &&
			//(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_9TA) &&
			//(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_9TA) &&
			//(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_9TA) &&
			//(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_9TA)
			////(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
			//)
			//{
				//if(pWMod->WModCnt9<60000)
				//{pWMod->WModCnt9++;}
			//}
			//else {pWMod->WModCnt9=0;}
			//break;
		//}
		default:
		{
			break;
		}
	}
#endif
}

/***********************************************
说明：姿控工作模式自主切换判断，判断使用哪种模式
输入：当前工作模式，模式指令状态，闭环姿态数据
输出：工作模式	
返回: 无
注意: 	
***********************************************/
void 	ZKModeSel_Auto(void)
{
	UINT8 	tmpNewMod =0;
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;	
	sAttModPara_t *pWMod =NULL;
	WhlPara_t *PWhl =NULL;

	if((NULL==tmpAtt) || (NULL==tmpConst))
	{return;}
	pWMod=&tmpAtt->sModePara;
	PWhl = &tmpAtt->sPerPara.WhlPara;
	tmpNewMod=pWMod->WorkMode;
		
#ifdef  MINMODULE_TYPE
	switch(pWMod->WorkMode)
	{
		case ATTMOD_RATEDMP:
		{
			if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
			||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
			{
				tmpNewMod = ATTMOD_MagTOSUN;
				pWMod->WModCnt2[0]=0;
				pWMod->WModCnt2[1]=0;
			}
			break;
		}
	
		default:
		{
			break;
		}
	}
#else
	switch(pWMod->WorkMode)
	{
		case ATTMOD_NOCOLL://碰撞规避模式
		{
			if (pWMod->WModCnt1>= (tmpConst->judge_time1*2) || (POSE_OK == pWMod->WModSadaOpen))
			{
				tmpNewMod = ATTMOD_RATEDMP;
			}
			break;
		}
		case ATTMOD_RATEDMP://速率阻尼
		{
			if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
			||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
			{
				if (PWhl->Whl_UseCnt >= 3)
				{
					tmpNewMod = ATTMOD_WhlTOSUN;
				}
				else
				{
					tmpNewMod = ATTMOD_MagTOSUN;	
				}	
			}
			break;
		}
		case ATTMOD_WhlTOSUN:
		{	
			if((pWMod->WModCnt3>=(tmpConst->judge_time3 * 2))&&(pWMod->WModCnt1>=(tmpConst->AttCmdFlashPara.judgeWait_time * 2))) 
			{
				//pWMod->Mod2UnctlFirst=0x55;
				tmpNewMod = ATTMOD_UNCTL;	
			}
			break;
		}
		case ATTMOD_UNCTL:
		{
			if (0x55 == pWMod->Mod2UnctlFirst)
			{
				if(pWMod->WModCntB>= (tmpConst->judge_timeB* 2)) //连续120s
				{
					tmpNewMod = ATTMOD_RATEDMP;
					pWMod->Mod2UnctlFirst=0x00;
				}
			}
			break;
		}
		case ATTMOD_ONLTOEARTH:
		{
			if(pWMod->WModCnt5>=(tmpConst->judge_time5* 2))  //连续30s
			{
				tmpNewMod = ATTMOD_WHEELTOEARTH;
			}
			break;
		}
				
		default:
		{
			break;
		}
	}
#endif
	tmpAtt->sModePara.ModeSwitchFlg = POSE_NO;
	ZKModDoSet(tmpNewMod);
}


/***********************************************
说明：姿控工作模式选择主函数
注意: 每0.5秒调用一次
***********************************************/
void ZKDoModeAct(void)
{
	//AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
//
	//if(NULL==tmpConst)
	//{
		//return;
	//}
	ZKDoModCal();
	ZKModeSel_Auto();
}