增加代码知识库；修复文档处理内容；增加API设置
This commit is contained in:
950
RAG-TEST-TOOLS/PrjAttCtrlMng/AttPosCtrl.c
Normal file
950
RAG-TEST-TOOLS/PrjAttCtrlMng/AttPosCtrl.c
Normal file
@@ -0,0 +1,950 @@
|
||||
/*
|
||||
* 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
|
||||
}
|
||||
Reference in New Issue
Block a user