951 lines
26 KiB
C
951 lines
26 KiB
C
/*
|
||
* Created: 2022/11/4 11:02:40
|
||
* Author: wangzk zhengmengxing
|
||
*/
|
||
#include "..\PrjCommon\CommonDef.h"
|
||
#include "..\PrjCommon\DevDefine.h"
|
||
|
||
#include "AttMath.h"
|
||
#include "AttCtrlMain.h"
|
||
|
||
//extern sAttPriData * m_pZKPriInfo;
|
||
/************************************************
|
||
说明：初始化参数,进程开始调用一次
|
||
输入：pCtlPara:控制参数结构
|
||
返回 :
|
||
************************************************/
|
||
void ZKPosCtlInit(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
//sAttOrbitCtlInfo_t *pOrbCtrl;
|
||
//sAttCtlPara_t *pWCtrl;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
//pOrbCtrl = &tmpAtt->sOrbitInp;
|
||
//pWCtrl=&tmpAtt->sCtlPara;
|
||
|
||
//pOrbCtrl->CurOrbCtrlPcak_ReceiveFlg = 0x00;
|
||
|
||
//磁控相关参数初始化
|
||
|
||
//PD控制算法参数初始化
|
||
//tmpConst->AttCmdFlashPara.kp_ToSun_PD[0] = 2.6832f;
|
||
//tmpConst->AttCmdFlashPara.kp_ToSun_PD[1] = 3.1585f;
|
||
//tmpConst->AttCmdFlashPara.kp_ToSun_PD[2] = 1.3033f;
|
||
//tmpConst->AttCmdFlashPara.kd_ToSun_PD[0] = 58.694f ;
|
||
//tmpConst->AttCmdFlashPara.kd_ToSun_PD[1] = 69.0921f ;
|
||
//tmpConst->AttCmdFlashPara.kd_ToSun_PD[2] = 22.8084f ;
|
||
//tmpConst->AttCmdFlashPara.w0max_PD = 0.1f * ANG2RADIAN;
|
||
|
||
tmpConst->AttCmdFlashPara.kp_ToSun_PD[0] = 0.98064f;
|
||
tmpConst->AttCmdFlashPara.kp_ToSun_PD[1] = 1.1544f;
|
||
tmpConst->AttCmdFlashPara.kp_ToSun_PD[2] = 0.773f;
|
||
tmpConst->AttCmdFlashPara.kd_ToSun_PD[0] = 27.0464f ;
|
||
tmpConst->AttCmdFlashPara.kd_ToSun_PD[1] = 31.8378f ;
|
||
tmpConst->AttCmdFlashPara.kd_ToSun_PD[2] = 13.388f ;
|
||
tmpConst->AttCmdFlashPara.w0max_PD = 0.1f * ANG2RADIAN;
|
||
|
||
tmpConst->kp_NoToSun_PD[0] = 0.6949f;
|
||
tmpConst->kp_NoToSun_PD[1] = 2.6546f;
|
||
tmpConst->kp_NoToSun_PD[2] = 3.2654f;
|
||
tmpConst->kd_NoToSun_PD[0] = 7.6008f ;
|
||
tmpConst->kd_NoToSun_PD[1] = 29.0342f ;
|
||
tmpConst->kd_NoToSun_PD[2] = 35.7145f ;
|
||
tmpConst->w0max_NoPD = 0.2f * ANG2RADIAN;
|
||
tmpConst->PDNo_Ts = 0.2f;
|
||
tmpConst->AttCmdFlashPara.PD_Ts = 3.0f;
|
||
//PID控制算法参数初始化
|
||
//tmpConst->AttCmdFlashPara.kp_PID[0] = 2.6832f;
|
||
//tmpConst->AttCmdFlashPara.kp_PID[1] = 3.1585f;
|
||
//tmpConst->AttCmdFlashPara.kp_PID[2] = 1.3033f;
|
||
//tmpConst->AttCmdFlashPara.kd_PID[0] = 58.694f;
|
||
//tmpConst->AttCmdFlashPara.kd_PID[1] = 69.0921f;
|
||
//tmpConst->AttCmdFlashPara.kd_PID[2] = 22.8084f;
|
||
//tmpConst->AttCmdFlashPara.ki_PID[0] = 0.0008f;
|
||
//tmpConst->AttCmdFlashPara.ki_PID[1] = 0.0009f;
|
||
//tmpConst->AttCmdFlashPara.ki_PID[2] = 0.0002f;
|
||
//tmpConst->AttCmdFlashPara.w0max_PID = 0.1f * ANG2RADIAN;
|
||
tmpConst->AttCmdFlashPara.kp_PID[0] = 0.98064f;
|
||
tmpConst->AttCmdFlashPara.kp_PID[1] = 1.1544f;
|
||
tmpConst->AttCmdFlashPara.kp_PID[2] = 0.773f;
|
||
tmpConst->AttCmdFlashPara.kd_PID[0] = 27.0464f;
|
||
tmpConst->AttCmdFlashPara.kd_PID[1] = 31.8378f;
|
||
tmpConst->AttCmdFlashPara.kd_PID[2] = 13.388f;
|
||
tmpConst->AttCmdFlashPara.ki_PID[0] = 0.0004f;
|
||
tmpConst->AttCmdFlashPara.ki_PID[1] = 0.0004f;
|
||
tmpConst->AttCmdFlashPara.ki_PID[2] = 0.0002f;
|
||
tmpConst->AttCmdFlashPara.w0max_PID = 0.1f * ANG2RADIAN;
|
||
//tmpConst->kp_NoPID[0] = 0.5;
|
||
//tmpConst->kp_NoPID[1] = 2.2;
|
||
//tmpConst->kp_NoPID[2] = 3.6;
|
||
//tmpConst->kd_NoPID[0] = 5.0;
|
||
//tmpConst->kd_NoPID[1] = 25.9;
|
||
//tmpConst->kd_NoPID[2] = 42.2;
|
||
//tmpConst->ki_NoPID[0] = 0.0008;
|
||
//tmpConst->ki_NoPID[1] = 0.0009;
|
||
//tmpConst->ki_NoPID[2] = 0.0002;
|
||
|
||
tmpConst->kp_NoPID[0] = 0.6949f;
|
||
tmpConst->kp_NoPID[1] = 2.6546f;
|
||
tmpConst->kp_NoPID[2] = 3.2654f;
|
||
tmpConst->kd_NoPID[0] = 7.6008f;
|
||
tmpConst->kd_NoPID[1] = 29.0342f;
|
||
tmpConst->kd_NoPID[2] = 35.7145f;
|
||
tmpConst->ki_NoPID[0] = 0.0002f;
|
||
tmpConst->ki_NoPID[1] = 0.0008f;
|
||
tmpConst->ki_NoPID[2] = 0.0009f;
|
||
tmpConst->w0max_NoPID = 0.20f * ANG2RADIAN;
|
||
tmpConst->PIDNo_Ts = 0.2f;
|
||
tmpConst->AttCmdFlashPara.PID_Ts = 4.0f;
|
||
|
||
//飞轮控制参数
|
||
//tmpConst->Whl_J[0] = 0.0064;
|
||
//tmpConst->Whl_J[1] = 0.0064;
|
||
//tmpConst->Whl_J[2] = 0.0064;
|
||
//tmpConst->Whl_J[3] = 0.0064;
|
||
|
||
//memset(pWCtrl->WheelCtrlResultPre, 0, 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
/************************************************
|
||
说明：初始化参数,每周期调用一次
|
||
输入：pCtlPara:控制参数结构
|
||
************************************************/
|
||
void ZKPosCtlOutInit(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttCtlPara_t *pWCtrl =NULL;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
|
||
pWCtrl=&tmpAtt->sCtlPara;
|
||
//控制指令
|
||
memset(pWCtrl->Mag_DeltBb, 0.0, 3*sizeof(TYPE_CAL));
|
||
memset(pWCtrl->WheelCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
|
||
|
||
}
|
||
/***********************************************
|
||
说明：磁速率计算
|
||
***********************************************/
|
||
//Bdot=当前拍-上一拍磁矢量
|
||
void MagDataDeltaProc(TYPE_CAL *cMagBb, TYPE_CAL *cMagBbPre, TYPE_CAL *cMagDelta, TYPE_CAL cDeltaT)
|
||
{
|
||
UINT32 i = 0;
|
||
if(POSE_ABSF(cDeltaT) >POSE_ZERO)
|
||
{
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
cMagDelta[i] = (cMagBb[i] - cMagBbPre[i]) / cDeltaT;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
//磁控速率阻尼算法
|
||
***********************************************/
|
||
void CtrlMagBdotDamp(TYPE_CAL *cDeltBb, TYPE_CAL *cMagT)
|
||
{
|
||
UINT8 i =0;
|
||
TYPE_CAL tmpBdot[3] = { 0.0 };
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return; }
|
||
memcpy( tmpBdot, cDeltBb, 12 );
|
||
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
/* 死区处理*/
|
||
if ((cDeltBb[i] > -POSE_ABSF(tmpConst->Mag_DampBdotMin)) && (cDeltBb[i] < POSE_ABSF(tmpConst->Mag_DampBdotMin)))
|
||
{
|
||
tmpBdot[i] = 0;
|
||
}
|
||
else
|
||
{
|
||
;
|
||
}
|
||
//cMagT[i] = -tmpConst->Mag_Bb_CtrlMax * ((float)CalPFlag(tmpBdot[i]));
|
||
if (tmpBdot[i]>POSE_ZERO)
|
||
{
|
||
cMagT[i] = -tmpConst->Mag_Bb_CtrlMax;
|
||
}
|
||
else if (tmpBdot[i]<POSE_ZERO)
|
||
{
|
||
cMagT[i] = tmpConst->Mag_Bb_CtrlMax;
|
||
}
|
||
else
|
||
{
|
||
cMagT[i] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
//选择计算Bdot的方式\计算磁控阻尼控制力矩
|
||
void MagDmpMomentCal(void)
|
||
{
|
||
UINT32 tmpCNT = 0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//AttCtrlConst_t *tmpConst;
|
||
MagPara_t *PMag =NULL;
|
||
sAttCtlPara_t *pWCtrl =NULL;
|
||
sAttDeterPara_t*pDeter =NULL;
|
||
MagPara_t *pMag =NULL;
|
||
//sAttEnvPara_t *pEnv;
|
||
MtPara_t *pMt =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
|
||
PMag = &tmpAtt->sPerPara.MagPara;
|
||
pWCtrl=&tmpAtt->sCtlPara;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
//pEnv = &tmpAtt->sEnvPara;
|
||
pMt=&tmpAtt->sPerPara.MtPara;
|
||
|
||
//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
|
||
tmpCNT = pMt-> MTCtrlTime % 10;
|
||
|
||
if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != pDeter->Wi_Vld))
|
||
{
|
||
if ((1 == tmpCNT)||( 0x88 == pMag->MagUseSwich))
|
||
{
|
||
cross( &PMag->Mag_Bb[0], &pDeter->Wi[0], &pWCtrl->Mag_DeltBb[0] );
|
||
CtrlMagBdotDamp( pWCtrl->Mag_DeltBb, pWCtrl->MagCtrlResult);
|
||
}
|
||
}
|
||
else if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != PMag->Mag_UsePre))
|
||
{
|
||
if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
|
||
{
|
||
/* 使用前后拍差来计算Bdot */
|
||
//MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),tmpConst->Mag_BdotltaT );
|
||
MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),5.0f );
|
||
CtrlMagBdotDamp( pWCtrl->Mag_DeltBb, pWCtrl->MagCtrlResult);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pWCtrl->MagCtrlResult[0] = 0.0f;
|
||
pWCtrl->MagCtrlResult[1] = 0.0f;
|
||
pWCtrl->MagCtrlResult[2] = 0.0f;
|
||
}
|
||
//printf("--FL1--\n");
|
||
}
|
||
|
||
|
||
//磁控对日控制算法
|
||
void MagSunMomentCal(void)
|
||
{
|
||
UINT32 tmpCNT = 0;
|
||
UINT32 i = 0;
|
||
UINT32 k = 0;
|
||
UINT8 tempM1OK = POSE_OK;
|
||
TYPE_CAL tempXb[3] = { 1.0f , 0.0f , 0.0f };
|
||
TYPE_CAL tempDt[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempN[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempA = 0.0f;
|
||
TYPE_CAL tempAlpha = 0.0f;
|
||
TYPE_CAL tempB = 0.0f;
|
||
TYPE_CAL tempBeta = 0.0f;
|
||
TYPE_CAL tempTXB[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempM1[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempWXB[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempW0[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tempM2[3] = { 0.0F , 0.0F , 0.0F };
|
||
TYPE_CAL tmpDtM=0.0f;
|
||
//TYPE_CAL tmpDtK1=0.05,tmpDtK2=1.0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
MagPara_t *PMag =NULL;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
sAttDeterPara_t*pDeter =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
sAttCtlPara_t *pWCtrl =NULL;
|
||
MagPara_t *pMag =NULL;
|
||
MtPara_t *pMt =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
PMag = &tmpAtt->sPerPara.MagPara;
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pWMod =&tmpAtt->sModePara;
|
||
pWCtrl=&tmpAtt->sCtlPara;
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
pMt=&tmpAtt->sPerPara.MtPara;
|
||
|
||
/*-----------------------------M1-------------------------*/
|
||
//计算Tm
|
||
if (0x55 == pWMod->WModSadaOpen)
|
||
{
|
||
tempXb[0] = 0.0f ;
|
||
tempXb[1] = 1.0f ;
|
||
tempXb[2] = 0.0f ;
|
||
}
|
||
else
|
||
{
|
||
tempXb[0] = 0.0f ;
|
||
tempXb[1] = 0.0f ;
|
||
tempXb[2] = -1.0f ;
|
||
}
|
||
cross( &tempXb[0], pDeter->SunVecb , &tempDt[0] );
|
||
|
||
//计算Tn
|
||
cross( &tempDt[0], &tempXb[0] , &tempN[0] );
|
||
CalNormal(tempN, 3, &tmpDtM);
|
||
|
||
if(tmpDtM < POSE_ZERO)
|
||
{
|
||
tempM1OK = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
tempN[0] = tempN[0]/tmpDtM;
|
||
tempN[1] = tempN[1]/tmpDtM;
|
||
tempN[2] = tempN[2]/tmpDtM;
|
||
}
|
||
tmpCNT = pMt-> MTCtrlTime % 10;
|
||
if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
|
||
{
|
||
//计算期望力矩与卫星本体系磁场强度夹角，
|
||
Posedot( &tempN[0], &PMag->Mag_UBb[0], 3, &tempA );
|
||
tempAlpha = POSE_ACOSF(tempA);
|
||
if ((tempAlpha > tmpConst->Mag_ATTTOSUN ) && ((tempAlpha < (180.0* ANG2RADIAN-tmpConst->Mag_ATTTOSUN))&&(tmpConst->Mag_ATTTOSUN< 90.0*ANG2RADIAN))&&(POSE_OK ==tempM1OK ))
|
||
{
|
||
//计算卫星-Zb轴与太阳矢量夹角β
|
||
Posedot( &pDeter->SunVecb[0], &tempXb[0], 3, &tempB );
|
||
tempBeta = POSE_ACOSF(tempB);
|
||
//计算太阳指向控制磁矩M1
|
||
cross( &tempN[0], &PMag->Mag_Bb[0] , &tempTXB[0] );
|
||
if (pWMod->WModSadaOpen== 0x55)
|
||
{
|
||
tempM1[0] = tmpConst->Mag_KpFBZK[0]*tempBeta*tempTXB[0];
|
||
tempM1[1] = tmpConst->Mag_KpFBZK[1]*tempBeta*tempTXB[1];
|
||
tempM1[2] = tmpConst->Mag_KpFBZK[2]*tempBeta*tempTXB[2];
|
||
}
|
||
else
|
||
{
|
||
tempM1[0] = tmpConst->Mag_Kp[0]*tempBeta*tempTXB[0];
|
||
tempM1[1] = tmpConst->Mag_Kp[1]*tempBeta*tempTXB[1];
|
||
tempM1[2] = tmpConst->Mag_Kp[2]*tempBeta*tempTXB[2];
|
||
}
|
||
//printf("pCtrl12: %d \n", tmpCNT);
|
||
}
|
||
else
|
||
{
|
||
tempM1[0] =0.0f;
|
||
tempM1[1] =0.0f;
|
||
tempM1[2] =0.0f;
|
||
}
|
||
|
||
/*-----------------------------M2-------------------------*/
|
||
if (0x55 == pWMod->WModSadaOpen)
|
||
{
|
||
//tempW0[0] = pDeter->AttRateTarRe[0] ;
|
||
//tempW0[1] = pDeter->AttRateTarRe[1] ; //弧度
|
||
tempW0[1] = tmpConst->Mag_FBZKWTOSUN;
|
||
}
|
||
else
|
||
{
|
||
//tempW0[2] = pDeter->AttRateTarRe[2] ; //弧度
|
||
tempW0[2] = tmpConst->Mag_WTOSUN;
|
||
}
|
||
cross( &tempW0[0], &PMag->Mag_Bb[0] , &tempWXB[0] );
|
||
|
||
if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != pDeter->Wi_Vld))
|
||
{
|
||
cross( &PMag->Mag_Bb[0], &pDeter->Wi[0], &pWCtrl->Mag_DeltBb[0] );
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
if (pWMod->WModSadaOpen== 0x55)
|
||
{
|
||
tempM2[i] = tmpConst->Mag_KdFBZK[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
|
||
}
|
||
else
|
||
{
|
||
tempM2[i] = tmpConst->Mag_Kd[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
|
||
}
|
||
}
|
||
//printf("pCtrl12: %d \n", tmpCNT);
|
||
}
|
||
else if ((POSE_NO != PMag->Mag_Use ) && (POSE_NO != PMag->Mag_UsePre))
|
||
{
|
||
/* 使用前后拍差来计算Bdot */
|
||
//MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),tmpConst->Mag_BdotltaT );
|
||
MagDataDeltaProc(PMag->Mag_Bb, PMag->Mag_BbPre, &(pWCtrl->Mag_DeltBb[0]),5.0f );
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
if (pWMod->WModSadaOpen== 0x55)
|
||
{
|
||
tempM2[i] = tmpConst->Mag_KdFBZK[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
|
||
}
|
||
else
|
||
{
|
||
tempM2[i] = tmpConst->Mag_Kd[i]*(pCtrl->Mag_DeltBb[i] + tempWXB[i]);
|
||
}
|
||
}
|
||
}else{;}
|
||
|
||
for (k = 0; k < 3; ++k)
|
||
{
|
||
pCtrl->MagCtrlResult[k] = - tempM1[k] - tempM2[k];
|
||
}
|
||
}
|
||
//printf("pCtrl12: %d \n", tmpCNT);
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：飞轮PD控制算法
|
||
***********************************************/
|
||
void WheelCalc_PD(void)
|
||
{
|
||
TYPE_CAL tmpDeltaA[3]= {0.0}, tmpDeltaW[3]= {0.0};
|
||
UINT8 i=0;
|
||
TYPE_CAL tmpTc[3]= {0.0};
|
||
TYPE_CAL tmpP[3]= {0.0}, tmpD[3]= {0.0};
|
||
TYPE_CAL tmpTs =0.0;
|
||
TYPE_CAL tmpHwb[3]= {0.0}, tmpCw[12]= {0.0},tmpSJ[9]= {0.0},tmpHj[4]= {0.0}, tmpHXW[3]= {0.0},tmpHb[3]= {0.0}, tmpHIbi[3]= {0.0};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttGuidLawPara_t *pGuide =NULL;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
WhlPara_t *PWhl =NULL;
|
||
TYPE_CAL tmpW0 =0.0;
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pGuide=&tmpAtt->sGuidLawPara;
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
|
||
|
||
if (POSE_OK == pWMod->WModSadaOpen)
|
||
{
|
||
memcpy(tmpP, tmpConst->AttCmdFlashPara.kp_ToSun_PD, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpD, tmpConst->AttCmdFlashPara.kd_ToSun_PD, 3*sizeof(TYPE_CAL));
|
||
tmpW0 = tmpConst->AttCmdFlashPara.w0max_PD;
|
||
tmpTs = tmpConst->AttCmdFlashPara.PD_Ts;
|
||
}
|
||
else
|
||
{
|
||
memcpy(tmpP, tmpConst->kp_NoToSun_PD, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpD, tmpConst->kd_NoToSun_PD, 3*sizeof(TYPE_CAL));
|
||
tmpW0 = tmpConst->w0max_NoPD;
|
||
tmpTs = tmpConst->PDNo_Ts;
|
||
}
|
||
|
||
//计算惯性系下整星三轴角动量
|
||
memcpy(tmpSJ, tmpConst->SAT_J, 9*sizeof(TYPE_CAL));
|
||
memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));
|
||
for(i=0;i<4;i++)
|
||
{
|
||
tmpHj[i]= PWhl->Whl_Momentum[i];
|
||
}
|
||
//计算本体系飞轮角动量
|
||
MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
|
||
//计算惯性系下整星三轴角动量
|
||
MatrixProductHL(tmpSJ, pDeter->Wi, tmpHIbi, 3, 3, 1);
|
||
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpHb[i] = tmpHwb[i] + tmpHIbi[i];
|
||
}
|
||
cross(tmpHb, pDeter->Wi, tmpHXW);
|
||
|
||
//计算偏差角和偏差角速度
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpDeltaA[i]=pDeter->CLPAttAng[i]-pGuide->TargAng[i];
|
||
DeviAngleLimit(&tmpDeltaA[i]);
|
||
tmpDeltaW[i]=pDeter->CLPAttAngRat[i]-pGuide->TargAngRat[i];
|
||
}
|
||
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpTc[i]=tmpP[i]*tmpDeltaA[i]+tmpD[i]*tmpDeltaW[i] + tmpHXW[i];
|
||
}
|
||
|
||
//限幅
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if(POSE_ABSF(pDeter->CLPAttAngRat[i])>tmpW0)
|
||
{
|
||
if ((pDeter->CLPAttAngRat[i]>POSE_ZERO)&&(tmpTc[i]<POSE_ZERO))
|
||
{
|
||
tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]- tmpW0);
|
||
}
|
||
else if((pDeter->CLPAttAngRat[i]<POSE_ZERO)&&(tmpTc[i]>POSE_ZERO))
|
||
{
|
||
tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]+ tmpW0);
|
||
}
|
||
}
|
||
}
|
||
|
||
//一阶惯性
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
|
||
(1.0f/(1.0f+tmpTs))*tmpTc[i];
|
||
}
|
||
//记录前次数据
|
||
memcpy(pCtrl->WheelCtrlResultPre, pCtrl->WheelCtrlResult, 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：飞轮PID控制算法
|
||
***********************************************/
|
||
void WheelCalc_PID(void)
|
||
{
|
||
TYPE_CAL tmpP[3] ={0.0f}, tmpD[3] ={0.0f}, tmpI[3] ={0.0f};
|
||
TYPE_CAL tmpDeltaA[3] ={0.0f}, tmpDeltaW[3] ={0.0f};
|
||
UINT8 i =0;
|
||
UINT8 tmp_cnt = 0;
|
||
TYPE_CAL tmpAerr[3] ={0.0f};
|
||
TYPE_CAL tmpTc[3] ={0.0f};
|
||
TYPE_CAL tmpTs =0.0f;
|
||
TYPE_CAL tmpW0 =0.0f;
|
||
TYPE_CAL tmpHwb[3] ={0.0f}, tmpCw[12] ={0.0f},tmpSJ[9] ={0.0f},tmpHj[4] ={0.0f}, tmpHXW[3] ={0.0f},tmpHb[3] ={0.0f}, tmpHIbi[3] ={0.0f};
|
||
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttGuidLawPara_t *pGuide =NULL;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
WhlPara_t *PWhl =NULL;
|
||
AttIMPTPara_t *pImpt =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pGuide=&tmpAtt->sGuidLawPara;
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
//tmpTs=tmpConst->AttCmdFlashPara.PID_Ts;
|
||
pWMod=&tmpAtt->sModePara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
|
||
if (POSE_OK == pWMod->WModSadaOpen)
|
||
{
|
||
memcpy(tmpP, tmpConst->AttCmdFlashPara.kp_PID, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpD, tmpConst->AttCmdFlashPara.kd_PID, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpI, tmpConst->AttCmdFlashPara.ki_PID, 3*sizeof(TYPE_CAL));
|
||
tmpW0 = tmpConst->AttCmdFlashPara.w0max_PID;
|
||
tmpTs=tmpConst->AttCmdFlashPara.PID_Ts;
|
||
}
|
||
else
|
||
{
|
||
memcpy(tmpP, tmpConst->kp_NoPID, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpD, tmpConst->kd_NoPID, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpI, tmpConst->ki_NoPID, 3*sizeof(TYPE_CAL));
|
||
tmpW0 = tmpConst->w0max_NoPID;
|
||
tmpTs=tmpConst->PIDNo_Ts;
|
||
}
|
||
|
||
memcpy(tmpAerr, pCtrl->SumAerrPre, 3*sizeof(TYPE_CAL));
|
||
|
||
//计算惯性系下整星三轴角动量
|
||
memcpy(tmpSJ, tmpConst->SAT_J, 9*sizeof(TYPE_CAL));
|
||
memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));
|
||
for(i=0;i<4;i++)
|
||
{
|
||
tmpHj[i]= PWhl->Whl_Momentum[i];
|
||
}
|
||
//计算本体系飞轮角动量
|
||
MatrixProductHL(tmpCw, tmpHj, tmpHwb, 3, 4, 1);
|
||
//计算惯性系下整星三轴角动量
|
||
MatrixProductHL(tmpSJ, pDeter->Wi, tmpHIbi, 3, 3, 1);
|
||
//计算整星角动量
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpHb[i] = tmpHwb[i] + tmpHIbi[i];
|
||
}
|
||
cross(tmpHb, pDeter->Wi, tmpHXW);
|
||
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if ((POSE_OK == pImpt->Whl_Use[i])&&(POSE_OK == PWhl->WhlIn_Valid[i]))
|
||
{
|
||
if(tmp_cnt<255)
|
||
{tmp_cnt++;}
|
||
}
|
||
}
|
||
|
||
if (tmp_cnt < 3)
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpHXW[i] =0.0f;
|
||
}
|
||
}
|
||
|
||
//计算偏差角和偏差角速度
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpDeltaA[i]=pDeter->CLPAttAng[i]-pGuide->TargAng[i];
|
||
DeviAngleLimit(&tmpDeltaA[i]);
|
||
tmpDeltaW[i]=pDeter->CLPAttAngRat[i]-pGuide->TargAngRat[i];
|
||
tmpAerr[i]+=tmpDeltaA[i];//角度偏差积分
|
||
}
|
||
memcpy(pCtrl->SumAerrPre, tmpAerr, 3*sizeof(TYPE_CAL));
|
||
|
||
//计算三轴控制指令
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpTc[i]=tmpP[i]*tmpDeltaA[i]+tmpD[i]*tmpDeltaW[i]+tmpI[i]*pCtrl->SumAerrPre[i] + tmpHXW[i];
|
||
}
|
||
|
||
//限幅
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if(POSE_ABSF(pDeter->CLPAttAngRat[i])>tmpW0 )
|
||
{
|
||
//printf("C: %d \n", i);
|
||
if ((pDeter->CLPAttAngRat[i]>POSE_ZERO)&&(tmpTc[i]<POSE_ZERO))
|
||
{
|
||
tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]- tmpW0 );
|
||
}
|
||
else if((pDeter->CLPAttAngRat[i]<POSE_ZERO)&&(tmpTc[i]>POSE_ZERO))
|
||
{
|
||
tmpTc[i]=tmpD[i]*(pDeter->CLPAttAngRat[i]+ tmpW0);
|
||
}
|
||
else{;}
|
||
}
|
||
}
|
||
//一阶惯性
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pCtrl->WheelCtrlResult[i]=(tmpTs/(1.0f+tmpTs))*pCtrl->WheelCtrlResultPre[i]+
|
||
(1.0f/(1.0f+tmpTs))*tmpTc[i];
|
||
}
|
||
//记录前次数据
|
||
memcpy(pCtrl->WheelCtrlResultPre, pCtrl->WheelCtrlResult, 3*sizeof(TYPE_CAL));
|
||
//printf("pCtrl11: %f, |%f, |%f \n", pCtrl->WheelCtrlResult[0],pCtrl->WheelCtrlResult[1],pCtrl->WheelCtrlResult[2]);
|
||
//printf("pCtrl12: %f, |%f, |%f \n", pCtrl->WheelCtrlResultPre[0],pCtrl->WheelCtrlResultPre[1],pCtrl->WheelCtrlResultPre[2]);
|
||
}
|
||
|
||
|
||
/********************飞轮角动量计算 *****************/
|
||
void Whl_Momentum_Count(void)
|
||
{
|
||
UINT32 i = 0;
|
||
TYPE_CAL tmpCw[12] ={0.0f}, tmpJ[4] ={0.0f};
|
||
TYPE_CAL tmpHj[4] ={0.0f};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
//sAttErrCtlPara_t *pREcheck;
|
||
WhlPara_t *PWhl =NULL;
|
||
//AttIMPTPara_t *pImpt;
|
||
//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;
|
||
//计算本体系合角动量(复用)
|
||
memcpy(tmpJ, tmpConst->Whl_J, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpCw, tmpConst->AttCmdFlashPara.M_Wheel, 12*sizeof(TYPE_CAL));
|
||
|
||
//计算四个轮子的角动量
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if (POSE_OK == PWhl->WhlIn_Valid[i])
|
||
{
|
||
PWhl->Whl_Momentum[i] = tmpJ[i] * PWhl->Whl_Rate[i]*RPM2RADS;
|
||
}
|
||
else
|
||
{
|
||
PWhl->Whl_Momentum[i] = 0;
|
||
}
|
||
}
|
||
////计算卫星本体下飞轮三轴角动量
|
||
//MatrixProductHL(tmpConst->AttCmdFlashPara.M_Wheel, PWhl->Whl_Momentum, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//计算本体系三轴角动量
|
||
|
||
for(i=0;i<4;i++)
|
||
{
|
||
tmpHj[i]= PWhl->Whl_Momentum[i] - tmpConst->Whl_HCent[i]; //tempHCent可更改
|
||
}
|
||
MatrixProductHL(tmpCw, tmpHj, PWhl->Whl_Momentum_XYZ, 3, 4, 1);//计算本体系飞轮角动量
|
||
|
||
CalNormal(PWhl->Whl_Momentum_XYZ, 3, &PWhl->Whl_Momentum_Sum);
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：磁力矩器卸载算法，卸载飞轮+星体
|
||
***********************************************/
|
||
void CtrlWheelReleaseProc(void)
|
||
{
|
||
UINT8 i = 0;
|
||
|
||
//TYPE_CAL tmpH[4];
|
||
TYPE_CAL tmpHwb[3] ={0.0f};
|
||
//TYPE_CAL tmpHb[3],tmpHIbi[3];
|
||
TYPE_CAL tmpk1 =0.0f,tmpk2 =0.0f;
|
||
TYPE_CAL tmpModB =0.0f;
|
||
TYPE_CAL tmpModH =0.0f;
|
||
//TYPE_CAL tmpModHDET;
|
||
TYPE_CAL tmpLamd =0.0f;
|
||
TYPE_CAL tmpang =0.0f;
|
||
TYPE_CAL tmpJB[3] ={0.0f};
|
||
TYPE_CAL tempBXH[3] = { 0.0F , 0.0F , 0.0F };
|
||
UINT32 tmpCNT = 0;
|
||
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
sAttDataPrePara_t *pData =NULL;
|
||
MagPara_t *pMag =NULL;
|
||
WhlPara_t *PWhl =NULL;
|
||
MtPara_t *pMt =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
pData=&tmpAtt->sPerPara;
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
pMt=&tmpAtt->sPerPara.MtPara;
|
||
|
||
if(POSE_NO == pDeter->Wi_Vld)
|
||
{
|
||
memset(pCtrl->MagCtrlResult, 0, 3*sizeof(TYPE_CAL));
|
||
return;
|
||
}
|
||
|
||
tmpCNT = pMt-> MTCtrlTime % 10;
|
||
if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpHwb[i] = PWhl->Whl_Momentum_XYZ[i];
|
||
}
|
||
//计算三轴推力器卸载指令
|
||
CalNormal(&pData->MagPara.Mag_Bb[0], 3, &tmpModB);
|
||
CalNormal(&tmpHwb[0], 3, &tmpModH);
|
||
|
||
//计算卫星本体系下卸载标志
|
||
tmpk1=tmpConst->k1_dump;
|
||
tmpk2=tmpConst->k2_dump;
|
||
if(tmpModH>tmpk1)
|
||
{pCtrl->IsDump=POSE_OK;}
|
||
else if((tmpModH>tmpk2)&&(POSE_OK==pCtrl->IsDump))
|
||
{pCtrl->IsDump=POSE_OK;}
|
||
else
|
||
{pCtrl->IsDump=POSE_NO;}
|
||
|
||
//计算磁卸载角
|
||
if ((tmpModB * tmpModH) > POSE_ZERO)
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpJB[i] = tmpHwb[i]/(tmpModB * tmpModH);
|
||
}
|
||
Posedot(pData->MagPara.Mag_Bb,tmpJB,3, &tmpLamd); //结果为弧度
|
||
tmpang = POSE_ACOSF(tmpLamd);
|
||
}
|
||
else
|
||
{
|
||
tmpang = 0;
|
||
}
|
||
|
||
//计算磁力矩器控制器控制指令
|
||
if ((tmpang>tmpConst->MTAtt_dump)&&(tmpang< (180.0 *ANG2RADIAN - tmpConst->MTAtt_dump))&&(pCtrl->IsDump==POSE_OK))
|
||
{
|
||
cross( pData->MagPara.Mag_Bb, tmpHwb , tempBXH );
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (tmpModB > POSE_ZERO)
|
||
{
|
||
pCtrl->MagCtrlResult[i] = - tmpConst->ku_dump * tempBXH[i] /tmpModB /tmpModB ;
|
||
}
|
||
else
|
||
{
|
||
pCtrl->MagCtrlResult[i]=0.0f;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
|
||
}
|
||
}
|
||
}
|
||
|
||
void CtrlModPD2PID(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL == tmpAtt) || (NULL == tmpConst))
|
||
{return;}
|
||
TYPE_CAL tmpAng = 5.0f * ANG2RADIAN;
|
||
TYPE_CAL tmpAngRat = 0.1f * ANG2RADIAN;
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
if ((0x55 == pDeter->CLPAttValid) &&
|
||
(POSE_ABSF(pDeter->CLPAttAng[0])<tmpAng) &&
|
||
(POSE_ABSF(pDeter->CLPAttAng[1])<tmpAng) &&
|
||
(POSE_ABSF(pDeter->CLPAttAng[2])<tmpAng) &&
|
||
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpAngRat) &&
|
||
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpAngRat)&&
|
||
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpAngRat))
|
||
{
|
||
if(pCtrl->PD2PIDCnt<60000)
|
||
{pCtrl->PD2PIDCnt++;}
|
||
if (20 <= pCtrl->PD2PIDCnt)
|
||
{
|
||
pCtrl->CtrlPD2PID = POSE_OK;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pCtrl->PD2PIDCnt = 0;
|
||
}
|
||
if (POSE_OK == pCtrl->CtrlPD2PID)
|
||
{
|
||
WheelCalc_PID();
|
||
//pCtrl->AttCtrl_TypeTEST=2;
|
||
}
|
||
else
|
||
{
|
||
WheelCalc_PD();
|
||
//pCtrl->AttCtrl_TypeTEST=1;
|
||
}
|
||
}
|
||
|
||
|
||
/************************************************
|
||
说明：姿态控制算法，根据使用状态进行控制模式选择
|
||
输入： 控制模式标志
|
||
输出：
|
||
************************************************/
|
||
void ZKPosCtlAct(sAttDeterPara_t *pAttDet, sAttGuidLawPara_t *pAttGuid, sAttCtlPara_t *pAttCtrl)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttCtlPara_t *pCtrl =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
pCtrl=&tmpAtt->sCtlPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
|
||
#ifdef MINMODULE_TYPE
|
||
//初始化
|
||
memset(pCtrl->Mag_DeltBb, 0.0f, 3*sizeof(TYPE_CAL));
|
||
//计算飞轮角动量和三轴角动量
|
||
Whl_Momentum_Count();
|
||
switch (tmpAtt->sModePara.WorkMode)
|
||
{
|
||
case ATTMOD_RATEDMP: /*速率阻尼*/
|
||
MagDmpMomentCal();
|
||
break;
|
||
case ATTMOD_MagTOSUN: //磁控对日
|
||
MagSunMomentCal();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
#else
|
||
//初始化
|
||
ZKPosCtlOutInit();
|
||
//计算飞轮角动量和三轴角动量
|
||
Whl_Momentum_Count();
|
||
//pAttCtrl->AttCtrl_TypeTEST=0;
|
||
switch (tmpAtt->sModePara.WorkMode)
|
||
{
|
||
case ATTMOD_BREAKPRE: /*星箭分离前*/
|
||
//pAttCtrl->AttCtrl_Type=0;
|
||
break;
|
||
case ATTMOD_NOCOLL: /*碰撞规避模式*/
|
||
if (pWMod->WModCnt1 >= 40)
|
||
{ WheelCalc_PD();}
|
||
else
|
||
{
|
||
memset(pCtrl->WheelCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
|
||
memset(pCtrl->WheelCtrlResultPre, 0.0f, 3*sizeof(TYPE_CAL));
|
||
}
|
||
//pAttCtrl->AttCtrl_Type=1;
|
||
break;
|
||
case ATTMOD_RATEDMP: /*速率阻尼*/
|
||
MagDmpMomentCal();
|
||
//pAttCtrl->AttCtrl_Type=2;
|
||
break;
|
||
//case ATTMOD_TOSUN: /*对日定向*/
|
||
case ATTMOD_MagTOSUN: //磁控对日
|
||
MagSunMomentCal();
|
||
break;
|
||
case ATTMOD_WhlTOSUN: //轮控对日
|
||
WheelCalc_PD();
|
||
CtrlWheelReleaseProc();
|
||
//pAttCtrl->AttCtrl_Type=1;
|
||
break;
|
||
case ATTMOD_ONLTOEARTH: /*对地定向*/
|
||
WheelCalc_PD();
|
||
memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
|
||
//pAttCtrl->AttCtrl_Type=1;
|
||
break;
|
||
case ATTMOD_WHEELTOEARTH: /*稳态对地*/
|
||
CtrlModPD2PID();
|
||
CtrlWheelReleaseProc();
|
||
break;
|
||
case ATTMOD_ATTAJUSTPRE: /*轨控前调姿*/
|
||
WheelCalc_PD();
|
||
CtrlWheelReleaseProc();
|
||
//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
|
||
break;
|
||
case ATTMOD_ORBITCTL: /*轨道控制*/
|
||
CtrlModPD2PID();
|
||
CtrlWheelReleaseProc();
|
||
break;
|
||
case ATTMOD_ATTAJUST: /*定向前调姿*/
|
||
WheelCalc_PD();
|
||
CtrlWheelReleaseProc();
|
||
//memset(pCtrl->MagCtrlResult, 0.0, 3*sizeof(TYPE_CAL));
|
||
break;
|
||
case ATTMOD_ONLTOTAR: /*对目标定向*/
|
||
CtrlModPD2PID();
|
||
CtrlWheelReleaseProc();
|
||
break;
|
||
case ATTMOD_UNCTL: /*无控模式*/
|
||
memset(pCtrl->WheelCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
|
||
memset(pCtrl->MagCtrlResult, 0.0f, 3*sizeof(TYPE_CAL));
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
#endif
|
||
}
|