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


//初始化，在进程初始化时候调用
void    ZKDYInit(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttGuidLawPara_t *pGuide=&tmpAtt->sGuidLawPara;
	if(NULL==tmpAtt)
	{
		return;		
	}
	
	memset(pGuide->TargAng, 0.0, 3*sizeof(TYPE_CAL));
	memset(pGuide->TargAngRat, 0.0, 3*sizeof(TYPE_CAL));
	pGuide->AttGuid_Type=0;					//导引律模式
}


/***********************************************
说明：对日定向导引律
***********************************************/
void ZKDYDirSun(void)
{
	sAttPriData *tmpAtt = NULL;
	sAttDeterPara_t *pDeter = NULL;
	sAttEnvPara_t *pEnv = NULL;
	sAttGuidLawPara_t *pGuide = NULL;		
	tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	if(NULL==tmpAtt)
	{
		return;
	}
	pDeter = &tmpAtt->sDeterPara;
	pEnv = &tmpAtt->sEnvPara;
	pGuide=&tmpAtt->sGuidLawPara;
	
	if(0==pDeter->CLPAttValid)
	{
		return;		
	}

	if(0x55==pEnv->bInOutShadow)
	{
		memcpy(pGuide->TargAng, pDeter->CLPAttAng, 3*sizeof(TYPE_CAL));
		return;
	}
	
	if(0x55 == pDeter->AngToSun_ValidFlg)
	{
		if(pGuide->CaptureSunCnt<200)
		pGuide->CaptureSunCnt++;
		
		if(pGuide->CaptureSunCnt>=5)
		{
			memset(pGuide->TargAng, 0.0, 3*sizeof(TYPE_CAL));
			memset(pGuide->TargAngRat, 0.0, 3*sizeof(TYPE_CAL));
			return;
		}
		else
		{
			//控制卫星旋转搜索太阳
			if (pDeter->CLPAttAngRatPre[0] < 0)
			{
				pGuide->TargAngRat[0] = - 0.1 * ANG2RADIAN;      //zmx20230915	
			}
			else
			{
				pGuide->TargAngRat[0] = 0.1 * ANG2RADIAN;      //zmx20230915	
			}
		}
	}
	else
	{
		pGuide->CaptureSunCnt=0;	
		//控制卫星旋转搜索太阳
		if (pDeter->CLPAttAngRatPre[0] < 0)
		{
			pGuide->TargAngRat[0] = - 0.1 * ANG2RADIAN;      //zmx20230915
		}
		else
		{
			pGuide->TargAngRat[0] = 0.1 * ANG2RADIAN;      //zmx20230915
		}
	}
}


/***********************************************
说明：固定目标导引律
***********************************************/
void ZKDYTarget(void)
{
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttGuidLawPara_t *pGuide=&tmpAtt->sGuidLawPara;	

	if(NULL==tmpAtt)
	{
		return;		
	}

	if((ATTMOD_MagTOSUN == tmpAtt->sModePara.WorkMode)
	||(ATTMOD_WHEELTOEARTH == tmpAtt->sModePara.WorkMode)
	||(ATTMOD_ATTAJUSTPRE == tmpAtt->sModePara.WorkMode)
	||(ATTMOD_ORBITCTL == tmpAtt->sModePara.WorkMode)
	||(ATTMOD_ATTAJUST ==tmpAtt->sModePara.WorkMode))
	{
		pGuide->TargAng[0]=0;
		pGuide->TargAng[1]=0;
		pGuide->TargAng[2]=0;
	}

	if((ATTMOD_ONLTOEARTH ==tmpAtt->sModePara.WorkMode)||(ATTMOD_ATTAJUST ==tmpAtt->sModePara.WorkMode))
	{
		pGuide->TargAng[2]=tmpAtt->sDeterPara.CLPAttAng[2];
	}
}

/************************************************
 说明：导引率使用
 输入：导引率模式
 输出：
************************************************/
void	ZKDYAction(void)
{   
	sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
	sAttGuidLawPara_t *pGuide=&tmpAtt->sGuidLawPara;
	
	if(NULL==tmpAtt)
	{
		return;		
	}

	//初始化
	ZKDYInit();
	if((ATTMOD_WhlTOSUN ==tmpAtt->sModePara.WorkMode))
	{
		//对日定向导引律
		pGuide->AttGuid_Type=1;
		ZKDYDirSun();
	}
	else
	{
		//固定目标导引律
		pGuide->AttGuid_Type=2;
		ZKDYTarget();
	}
	
}