156 lines
3.1 KiB
C
156 lines
3.1 KiB
C
/*
|
|
* 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();
|
|
}
|
|
|
|
}
|