zhoujl/rag_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7aa3ce32949f0133ae87f5ce39beac4256737495
rag_agent/RAG-TEST-TOOLS/PrjAttCtrlMng/AttPosGet.c

1250 lines
36 KiB
C
Raw Normal View History

增加代码知识库；修复文档处理内容；增加API设置
2026-05-16 20:20:10 +08:00
/*
* Created: 2022/11/4 11:02:40
* Author: wangzk zhengmengxing
*/
#include "..\PrjCommon\CommonDef.h"
#include "AttMath.h"
#include "AttCtrlMain.h"
//#include "../PrjTaskMng/TaskMng.h"
/************************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Σ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>г<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
************************************************/
void ZKPGInit(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
sAttDeterPara_t *pDeter =NULL;
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
pDeter=&tmpAtt->sDeterPara;
pDeter->AttDeter_Type =0;
#ifdef MINMODULE_TYPE
//<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
pDeter->AngToSun_ValidFlg=0x55; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־
pDeter->CalAngToSun_Source=0;
//<2F>ջ<EFBFBD><D5BB><EFBFBD>̬<EFBFBD><CCAC>
memset(pDeter->CLPAttAng,0.0,3*sizeof(TYPE_CAL));
pDeter->CLPAttValid = POSE_OK;
#else
//<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
pDeter->AngToSun_ValidFlg=0x55; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־
pDeter->CalAngToSun_Source=0;
//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>̬
memset(pDeter->Qo,0.0,4*sizeof(TYPE_CAL));
memset(pDeter->AttRateOrb,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AttAngTar,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AttAngOrb,0,3*sizeof(TYPE_CAL));
memset(pDeter->SunVeco,0.0,3*sizeof(TYPE_CAL));
pDeter->AttOrb_Vld = POSE_OK; //<2F><><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>Ч<EFBFBD><D0A7>־
//Ŀ<><C4BF><EFBFBD>ο<EFBFBD>ϵ<EFBFBD><CFB5>̬
memset(pDeter->AttAngTarRe,0,3*sizeof(TYPE_CAL));
//memset(pDeter->AttRateTarRe,0.00873,3*sizeof(TYPE_CAL));
pDeter->AttRateTarRe[0] = 0.00873f;
pDeter->AttRateTarRe[1] = 0.00873f;
pDeter->AttRateTarRe[2] = - 0.00873f;
pDeter->AttTarRe_Vld = POSE_OK;
//<2F><><EFBFBD>زο<D8B2>ϵ<EFBFBD><CFB5>̬
memset(pDeter->AttAngOrbCtrl,0,3*sizeof(TYPE_CAL));
memset(pDeter->AttRateOrbCtrl,0,3*sizeof(TYPE_CAL));
pDeter->AttOrbCtrl_Vld = POSE_OK;
//<2F>ջ<EFBFBD><D5BB><EFBFBD>̬<EFBFBD><CCAC>
memset(pDeter->CLPAttAng,0,3*sizeof(TYPE_CAL));
pDeter->CLPAttValid = POSE_OK;
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD>
***********************************************/
void Gyro_AttChoice(void)
{
UINT8 i =0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//AttCtrlConst_t *tmpConst;
GyroPara_t *PGyro[GYRO_NUM];
sAttDeterPara_t *pDeter =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
pDeter=&tmpAtt->sDeterPara;
//if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0x55)//<2F><><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
if( ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))//<2F><><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
{
for(i=0;i<3;i++)
{
if(POSE_OK == PGyro[i]->Gyro_AvailableFlg)
{
memcpy(pDeter->Wi, PGyro[i]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->Wi_Vld=POSE_OK;
pDeter->AttDeter_Type= pDeter->AttDeter_Type | (0x01<<i) ;
break;
}
}
}
//else if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0xAA)//<2F><><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
else if(((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))
{
pDeter->Wi_Vld=POSE_OK;
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg))
{
if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
{
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
}
else
{
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
}
}
else if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
{
if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
{
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
}
else
{
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
}
}
else
{
if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
{
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
}
else
{
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
}
}
}
else if((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))//<2F><><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
{
pDeter->Wi_Vld=POSE_OK;
if ((PGyro[0]->GyroPrior == PGyro[1]->GyroPrior)&&(PGyro[0]->GyroPrior == PGyro[2]->GyroPrior))
{
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
}
else
{
if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
{
if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
{
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
}
else
{
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
}
}
else
{
if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
{
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
}
else
{
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
}
}
}
}
else{;}
for(i=0;i<3;i++)
{
tmpAtt->sPerPara.Gyro_ATTWi[i] = pDeter->Wi[i];
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Gyro_AttDeter(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
GyroPara_t *PGyro[GYRO_NUM];
sAttDeterPara_t *pDeter =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
pDeter=&tmpAtt->sDeterPara;
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
{
Gyro_AttChoice();
}
else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
{
memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->Wi_Vld=POSE_OK;
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
}else{;}
if(POSE_OK == pDeter->QiPre_Vld)//ǰ<><C7B0><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ч
{
AttLongKutaCal(pDeter->WiPre, pDeter->QiPre, pDeter->Qi);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
pDeter->Qi_Vld=POSE_OK;
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
if (pDeter->GyroINT_Time<60000)
{
pDeter->GyroINT_Time++;
}
if (pDeter->GyroINT_Time > (tmpConst->GyroINT_TLimt*2) )
{
pDeter->Qi_Vld=POSE_NO;
pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
}
}
else//ǰ<><C7B0><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ч
{
pDeter->Qi_Vld=POSE_NO;
pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
pDeter->GyroINT_Time= 0;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ּ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
***********************************************/
void QuatDiffCal(TYPE_CAL * pQ_now, TYPE_CAL * pQ_pre, TYPE_CAL * pW)
{
TYPE_CAL tmpDeltQ[4] ={0.0};
TYPE_CAL tmpMatQuat[12] ={0.0};
TYPE_CAL tmpMatDeltQ[4] ={0.0};
UINT8 i =0;
if(CalPFlag(pQ_now[3])==CalPFlag(pQ_pre[3]))
{
for(i=0;i<4;i++)
{tmpDeltQ[i] = pQ_now[i] - pQ_pre[i];}
}
else//Ӧ<>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
for(i=0;i<4;i++)
{tmpDeltQ[i] = -pQ_now[i] - pQ_pre[i];}
}
tmpMatQuat[0] = -pQ_pre[0];
tmpMatQuat[1] = pQ_pre[3];
tmpMatQuat[2] = pQ_pre[2];
tmpMatQuat[3] = -pQ_pre[1];
tmpMatQuat[4] = -pQ_pre[1];
tmpMatQuat[5] = -pQ_pre[2];
tmpMatQuat[6] = pQ_pre[3];
tmpMatQuat[7] = pQ_pre[0];
tmpMatQuat[8] = -pQ_pre[2];
tmpMatQuat[9] = pQ_pre[1];
tmpMatQuat[10] = -pQ_pre[0];
tmpMatQuat[11] = pQ_pre[3];
tmpMatDeltQ[0] = tmpDeltQ[3] / ATTPERIOD_S;
tmpMatDeltQ[1] = tmpDeltQ[0] / ATTPERIOD_S;
tmpMatDeltQ[2] = tmpDeltQ[1] / ATTPERIOD_S;
tmpMatDeltQ[3] = tmpDeltQ[2] / ATTPERIOD_S;
MatrixPlusVectorN(tmpMatQuat, tmpMatDeltQ, pW, 3, 4);
for (i = 0; i < 3; ++i)
{
pW[i] *= 2.0f;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><EFBFBD><EFBFBD>Ƕȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵķ<EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
***********************************************/
void SsGyro_AngDeter(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
GyroPara_t *PGyro[GYRO_NUM];
sAttDeterPara_t *pDeter =NULL;
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
pDeter=&tmpAtt->sDeterPara;
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
{
Gyro_AttChoice();
}
else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
{
memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->Wi_Vld=POSE_OK;
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
}
else if(POSE_OK == pDeter->QiPre_Vld)//ǰ<><C7B0><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ч
{
QuatDiffCal(pDeter->Qi, pDeter->QiPre, pDeter->Wi);//<2F><><EFBFBD>ּ<EFBFBD><D6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
pDeter->Wi_Vld = POSE_OK;
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
}
else
{
//pDeter->Wi_Vld = POSE_NO;
//pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>/<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>/<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>־
***********************************************/
void SsGyro_AttDeter(void)
{
UINT8 i =0;
TYPE_CAL tmpRbi[9] ={0.0};
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
GyroPara_t *PGyro[GYRO_NUM];
SsPara_t *PStar[STAR_NUM];
sAttDeterPara_t *pDeter =NULL;
AttIMPTPara_t *pImpt =NULL;
sAttModPara_t *pWMod =NULL;
PPUPara_t *PPPU =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
pDeter=&tmpAtt->sDeterPara;
pImpt=&tmpAtt->sIMPTPara;
pWMod =&tmpAtt->sModePara;
PPPU = &tmpAtt->sPerPara.PPUPara;
//<2F><>¼ǰ<C2BC>ν<EFBFBD><CEBD><EFBFBD>
pDeter->QiPre_Vld=pDeter->Qi_Vld;
memcpy(pDeter->QiPre,pDeter->Qi,4*sizeof(TYPE_CAL));
memcpy(pDeter->WiPre,pDeter->Wi,3*sizeof(TYPE_CAL));
//<2F><>ʼ<EFBFBD><CABC>
pDeter->Qi_Vld=0;
pDeter->Wi_Vld=0;
memset(pDeter->Qi, 0, 4*sizeof(TYPE_CAL));
memset(pDeter->Wi, 0, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type=0;
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
PStar[0]->ssPrior=tmpConst->SSUsePrior[0];
PStar[1]->ssPrior=tmpConst->SSUsePrior[1];
PStar[2]->ssPrior=tmpConst->SSUsePrior[2];
#ifdef MINMODULE_TYPE
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>ABC<42>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<STAR_NUM;i++)
{
PStar[i]->ssAvailableFlg=POSE_NO;
if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) )
{PStar[i]->ssAvailableFlg=POSE_OK;}
}
#else
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>ABC<42>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<STAR_NUM;i++)
{
PStar[i]->ssAvailableFlg=POSE_NO;
if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) && (PStar[i]->ssCrCheckFlg!=CC_ERROR))
{PStar[i]->ssAvailableFlg=POSE_OK;}
}
#endif
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
PGyro[0]->GyroPrior=tmpConst->GyroUsePrior[0];
PGyro[1]->GyroPrior=tmpConst->GyroUsePrior[1];
PGyro[2]->GyroPrior=tmpConst->GyroUsePrior[2];
#ifdef MINMODULE_TYPE
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<GYRO_NUM;i++)
{
PGyro[i]->Gyro_AvailableFlg=POSE_NO;
if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK))
{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}
}
#else
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<GYRO_NUM;i++)
{
PGyro[i]->Gyro_AvailableFlg=POSE_NO;
if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK)&&(PGyro[i]->GyroCrCheckFlg!=CC_ERROR))
{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}
}
#endif
//if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55 ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //<2F><><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
if (((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg) || (POSE_OK == PGyro[2]->Gyro_AvailableFlg)) ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //<2F><><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
{
//ֻ<><D6BB><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD>
Gyro_AttDeter();
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><DDBE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pDeter->Qi_Vld=0;
pDeter->Wi_Vld=0;
pDeter->AttDeter_Type=0;
}
}
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0x55)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if(((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pDeter->GyroINT_Time = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ԫ<EFBFBD><D4AA>
for(i=0;i<3;i++)
{
if(POSE_OK == PStar[i]->ssAvailableFlg)
{
memcpy(pDeter->Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->Qi_Vld=0x55;
pDeter->Wi_Vld=0x55;
pDeter->AttDeter_Type= pDeter->AttDeter_Type |(0x20<<i) ;
break;
}
}
SsGyro_AngDeter();//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xAA)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if (((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pDeter->GyroINT_Time = 0;
pDeter->Qi_Vld=0x55;
pDeter->Wi_Vld=0x55;
if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg))
{
if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
{
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
}
else
{
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
}
}
else if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))
{
if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
{
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
}
else
{
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
}
}
else
{
if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
{
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
}
else
{
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
}
}
SsGyro_AngDeter();//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xFF)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if((POSE_OK == PStar[0]->ssAvailableFlg)&& (POSE_OK == PStar[1]->ssAvailableFlg)&& (POSE_OK == PStar[2]->ssAvailableFlg))//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pDeter->GyroINT_Time = 0;
pDeter->Qi_Vld=0x55;
pDeter->Wi_Vld=0x55;
if ((PStar[0]->ssPrior == PStar[1]->ssPrior)&&(PStar[0]->ssPrior == PStar[2]->ssPrior))
{
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
}
else
{
if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
{
if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
{
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
}
else
{
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
}
}
else
{
if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
{
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
}
else
{
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
}
}
}
SsGyro_AngDeter();//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}else{;}
//#ifdef MINMODULE_TYPE
//;
//#else
#ifndef MINMODULE_TYPE
GetTime(pDeter->SatTime);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
if((POSE_OK == pDeter->Qi_Vld)&& (ATTMOD_ORBITCTL == pWMod->WorkMode ) &&(0x01 == PPPU->PPUIn_FIREOK))
{
Q2Att(pDeter->Qi, tmpRbi);
MatrixProductHL(tmpRbi, tmpConst->AccThru_Up, pDeter->Ai, 3, 3, 1);
pDeter->PPUVld = POSE_OK;
}
else
{
pDeter->Ai[0] = 0;
pDeter->Ai[1] = 0;
pDeter->Ai[2] = 0;
pDeter->PPUVld = POSE_NO;
}
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+ģ<EFBFBD><EFBFBD>̫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+Xb\+Yb\-Zb<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Cal_AngToSun(void)
{
TYPE_CAL tmpRbi[9] ={0.0};
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//AttCtrlConst_t *tmpConst;
sAttDeterPara_t *pDeter= NULL;
sAttModPara_t *pWMod= NULL;
sAttDataPrePara_t *pDataPre= NULL;
sAttOrbitGetPara_t *pOrbit = NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(tmpAtt==NULL)
{return;}
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
pDeter=&tmpAtt->sDeterPara;
pWMod=&tmpAtt->sModePara;
pDataPre=&tmpAtt->sPerPara ;
pOrbit = &tmpAtt->sOrbitPara;
//<2F><>ʼ<EFBFBD><CABC>
if((POSE_OK == pDeter->Qi_Vld)&&(POSE_OK == pOrbit->MJCLv)) //ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
Q2Att(pDeter->Qi, tmpRbi);
MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVecb, 3, 3, 1);
pDeter->CalAngToSun_Source=0x55;
pDeter->AngToSun_ValidFlg=0x55;
}
else if ((POSE_OK == pDataPre->AssPara.Ass_SunVecVld)&&(CC_ERROR!= pDataPre->AssPara.AssCrCheckFlg))//̫<><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
memcpy(pDeter->SunVecb, pDataPre->AssPara.Ass_SunVecb, 3*sizeof(TYPE_CAL));
pDeter->CalAngToSun_Source=0xAA;
pDeter->AngToSun_ValidFlg=0x55;
}
else
{
pDeter->CalAngToSun_Source=0x00;
pDeter->AngToSun_ValidFlg=0;
}
if (ATTMOD_MagTOSUN == pWMod->WorkMode)
{
if ((POSE_OK !=pWMod->WModSadaOpen))
{
//<2F><><EFBFBD><EFBFBD>-Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
if (POSE_OK == pDeter->AngToSun_ValidFlg)
{
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
pDeter->AngToSun[2]=0.0f;
}
else
{
pDeter->AngToSun[0]=0.0f;
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]=0.0f;
}
}
else
{
//<2F><><EFBFBD><EFBFBD>+Y<><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
if (POSE_OK == pDeter->AngToSun_ValidFlg)
{
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[1]);
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]= POSE_ATAN2F(pDeter->SunVecb[0],pDeter->SunVecb[1]);
}
else
{
pDeter->AngToSun[0]=0.0f;
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]=0.0f;
}
}
}
else if (ATTMOD_WhlTOSUN == pWMod->WorkMode)
{
if ((POSE_OK!= pWMod->WModSadaOpen))
{
//<2F><><EFBFBD><EFBFBD>-Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
if (POSE_OK ==pDeter->AngToSun_ValidFlg)
{
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
pDeter->AngToSun[2]=0.0f;
}
else
{
pDeter->AngToSun[0]=0.0f;
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]=0.0f;
}
}
else
{
//<2F><><EFBFBD><EFBFBD>X<EFBFBD><58><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
if (POSE_OK == pDeter->AngToSun_ValidFlg)
{
pDeter->AngToSun[0]= 0.0f;
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[0]);
pDeter->AngToSun[2]=- POSE_ATAN2F(pDeter->SunVecb[1],pDeter->SunVecb[0]);
}
else
{
pDeter->AngToSun[0]=0.0f;
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]=0.0f;
}
}
}
else if (ATTMOD_UNCTL == pWMod->WorkMode)
{
//<2F><><EFBFBD><EFBFBD>-Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
if (POSE_OK == pDeter->AngToSun_ValidFlg)
{
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
pDeter->AngToSun[2]=0.0f;
}
else
{
pDeter->AngToSun[0]=0.0f;
pDeter->AngToSun[1]=0.0f;
pDeter->AngToSun[2]=0.0f;
}
}else{;}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ų<EFBFBD>ǿ<EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
//<2F>ų<EFBFBD><C5B3><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ų<EFBFBD>ǿ<EFBFBD><C7BF>
void DertCalcBb(void)
{
UINT8 i =0;
UINT32 tmpCNT = 0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//AttCtrlConst_t *tmpConst;
MagPara_t *pMag =NULL;
MtPara_t *pMt =NULL;
sAttEnvPara_t *pEnv =NULL;
sAttModPara_t *pWMod =NULL;
AttIMPTPara_t *pImpt =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(tmpAtt==NULL)
{return;}
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
pMag=&tmpAtt->sPerPara.MagPara;
pMt=&tmpAtt->sPerPara.MtPara;
pEnv = &tmpAtt->sEnvPara;
pWMod=&tmpAtt->sModePara;
pImpt=&tmpAtt->sIMPTPara;
//<2F>жϴ<D0B6>ǿ<EFBFBD><C7BF><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<MAG_NUM;i++)
{
pMag->MagAvailableFlg[i]=POSE_NO;
if((pImpt->MagUseState[i]==POSE_OK) && (pMag->MagIn_Valid[i] ==POSE_OK) && (pMag->MagCrCheckFlg[i]!=CC_ERROR))
{pMag->MagAvailableFlg[i]=POSE_OK;}
}
//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
tmpCNT = pMt-> MTCtrlTime % 10;
if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
{
memcpy(pMag->Mag_BbPre, pMag->Mag_Bb, 3*sizeof(TYPE_CAL));
pMag->Mag_UsePre = pMag->Mag_Use;
}
#ifdef MINMODULE_TYPE
if ( POSE_OK == pMag->MagAvailableFlg[0])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x11;
}
else if (POSE_OK == pMag->MagAvailableFlg[1])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x22;
}
else if (POSE_OK == pMag->MagIn_Valid[2])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x44;
}
else
{
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
pMag->Mag_Use =POSE_NO;
pMag->MagUseSwich =POSE_NO;
}
#else
if ((ATTMOD_RATEDMP == pWMod->WorkMode)|| (ATTMOD_MagTOSUN == pWMod->WorkMode))
{
if ( POSE_OK == pMag->MagAvailableFlg[0])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x11;
}
else if (POSE_OK == pMag->MagAvailableFlg[1])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x22;
}
else if (POSE_OK==pMag->MagIn_Valid[2])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x44;
}
else if (POSE_OK == pEnv->fvIn_Valid)
{
memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x88;
}
else
{
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
pMag->Mag_Use =POSE_NO;
pMag->MagUseSwich =POSE_NO;
}
}
else
{
if (POSE_OK == pEnv->fvIn_Valid)
{
memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x88;
}
else if (POSE_OK == pMag->MagAvailableFlg[0])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x11;
}
else if (POSE_OK == pMag->MagAvailableFlg[1])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use =POSE_OK;
pMag->MagUseSwich = 0x22;
}
else if (POSE_OK==pMag->MagIn_Valid[2])
{
if (1 == tmpCNT)
{
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
}
pMag->Mag_Use = POSE_OK;
pMag->MagUseSwich = 0x44;
}
else
{
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
pMag->Mag_Use =POSE_NO;
pMag->MagUseSwich =POSE_NO;
}
}
if (POSE_OK == pMag->Mag_Use)
{
for(i=0; i<3; i++)
{pMag->Mag_UBb[i] = pMag->Mag_Bb[i] ;}
unitary( &pMag->Mag_UBb[0], 3 ); // <20><>һ<EFBFBD><D2BB>
}
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȡ<EFBFBD>̫<EFBFBD><EFBFBD>ʸ<EFBFBD><EFBFBD>
***********************************************/
void AttDeterOrb(void)
{
TYPE_CAL tmpRbi[9] ={0.0}, tmpRoi[9] ={0.0}, tmpRio[9] ={0.0}, tmpRbo[9] ={0.0};
TYPE_CAL tmpWoi[3] ={0.0};
TYPE_CAL tmpWbo[3] ={0.0};
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
sAttDeterPara_t *pDeter =NULL;
sAttOrbitGetPara_t *pOrbitIn =NULL;
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(tmpAtt==NULL)
{return;}
pDeter=&tmpAtt->sDeterPara;
pOrbitIn=&tmpAtt->sOrbitPara;
if((0 == pDeter->Qi_Vld)||(0 == pOrbitIn->OrbitVld))
{
pDeter->AttOrb_Vld=0;
return;
}
pDeter->AttOrb_Vld=0x55;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>
Q2Att(pDeter->Qi, tmpRbi);
CalTransMatrix_IO(pOrbitIn->JPos, pOrbitIn->JVel, tmpRoi);
MatrixTransposeHL(tmpRoi, tmpRio, 3, 3);
MatrixProductHL(tmpRbi, tmpRio, tmpRbo, 3, 3, 3);
Matrix2Eul312A(tmpRbo, pDeter->AttAngOrb);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>Ԫ<EFBFBD><D4AA>
Eul312A2Quat(pDeter->AttAngOrb, pDeter->Qo);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>
MatrixProductHL(tmpRoi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVeco, 3, 3, 1);
if(0 == pDeter->Wi_Vld)
{
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>
AglSpdOofICal(pOrbitIn->JPos, pOrbitIn->JVel, tmpWoi);
//pDeter->OrbAttRate_Wo = tmpWoi[1];
MatrixProductHL(tmpRbo, tmpWoi, tmpWbo, 3, 3, 1);
tmpWbo[0]=pDeter->Wi[0]-tmpWbo[0];
tmpWbo[1]=pDeter->Wi[1]-tmpWbo[1];
tmpWbo[2]=pDeter->Wi[2]-tmpWbo[2];
memcpy(pDeter->AttRateOrb,tmpWbo,3*sizeof(TYPE_CAL));
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>زο<EFBFBD>ϵ<EFBFBD><EFBFBD>̬
***********************************************/
void AttDeterOrbCtrl(void)
{
TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵŷ<CFB5><C5B7><EFBFBD><EFBFBD>,<2C><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
sAttOrbitCtlInfo_t *pOrb =NULL;
sAttDeterPara_t *pDeter =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
pOrb=&tmpAtt->sOrbitInp;
pDeter=&tmpAtt->sDeterPara;
pDeter->AttOrbCtrl_Vld=0;
memset(pDeter->AttAngOrbCtrl,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AttRateOrbCtrl,0.0,3*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>Ϊ<EFBFBD><CEAA><EFBFBD>ص<EFBFBD><D8B5>ˣ<EFBFBD>0X66<36><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>0X77<37><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD>0X8<58><38><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>Ч<EFBFBD><D0A7>־Ϊ<D6BE><CEAA>Ч
if((ATTMOD_ATTAJUSTPRE!= tmpAtt->sModePara.WorkMode)&&(ATTMOD_ORBITCTL!= tmpAtt->sModePara.WorkMode))
{return;}
if (0x55 == pDeter->AttOrb_Vld)
{
pDeter->AttOrbCtrl_Vld=0x55;
}
else
{
pDeter->AttOrbCtrl_Vld = 0x00;
return;
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
memcpy(pDeter->AttAngTar,pOrb->OrbCtrlPackCur.OrbCtrAng,3*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>Ԫ<EFBFBD><D4AA>Qbo<62>õ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Rbo
Q2Att(pDeter->Qo, tmpRbo);
//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>RCo
Att2M(pDeter->AttAngTar, tmpRCo);
MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
//<2F><><EFBFBD>زο<D8B2>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
//<2F><><EFBFBD>زο<D8B2>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>
Matrix2Eul312A(tmpRbc, pDeter->AttAngOrbCtrl);
//<2F><><EFBFBD>ǹ<EFBFBD><C7B9>زο<D8B2>ϵ<EFBFBD><CFB5><EFBFBD>ٶȼ<D9B6>Ϊ<EFBFBD><CEAA><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶ<EFBFBD>
memcpy(pDeter->AttRateOrbCtrl,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD>ϵ<EFBFBD><EFBFBD>̬
***********************************************/
void AttDeterTarRe(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
sAttDeterPara_t *pDeter =NULL;
sAttModPara_t *pWMod =NULL;
sAttTARCCtlInfo_t *pTAR =NULL;
//sAttOrbitCtlInfo_t *pOrb;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(tmpAtt==NULL)
{return;}
pDeter=&tmpAtt->sDeterPara;
pWMod=&tmpAtt->sModePara;
pTAR=&tmpAtt->sTARCInp;
//pOrb=&tmpAtt->sTARCInp;
TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>Ŀ<EFBFBD><C4BF>ϵŷ<CFB5><C5B7><EFBFBD><EFBFBD>,<2C><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
memset(pDeter->AttAngTarRe,0.0,3*sizeof(TYPE_CAL));
memset(pDeter->AttRateTarRe,0.0,3*sizeof(TYPE_CAL));
pDeter->AttTarRe_Vld = 0x00;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ģʽΪĿ<CEAA>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD>0X99<39><39><EFBFBD><EFBFBD>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD>0XAA<41><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC>㣻<EFBFBD><E3A3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD>
if((pWMod->WorkMode!=ATTMOD_ATTAJUST)&&(pWMod->WorkMode!=ATTMOD_ONLTOTAR))
{return;}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>Ч<EFBFBD><D0A7>־Ϊ<D6BE><CEAA>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9>زο<D8B2>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>Ч<EFBFBD><D0A7>־Ϊ<D6BE><CEAA>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־Ϊ<D6BE><CEAA>Ч
if (POSE_OK == pDeter->AttOrb_Vld)
{
pDeter->AttTarRe_Vld = 0x55;
}
else
{
pDeter->AttTarRe_Vld = 0x00;
return;
}
//<2F><>ȡĿ<C8A1><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
memcpy(pDeter->AttAngTar,pTAR->TARCtrlPackCur.TARCtrAng,3*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>Ԫ<EFBFBD><D4AA>Qbo<62>õ<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Rbo
Q2Att(pDeter->Qo, tmpRbo);
//<2F><><EFBFBD><EFBFBD>ϵĿ<CFB5><C4BF><EFBFBD><EFBFBD>ת<EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>RCo
Att2M(pDeter->AttAngTar, tmpRCo);
MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
//Ŀ<><C4BF><EFBFBD>ο<EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
//Ŀ<><C4BF><EFBFBD>ο<EFBFBD>ϵ<EFBFBD><CFB5>̬<EFBFBD><CCAC>
Matrix2Eul312A(tmpRbc, pDeter->AttAngTarRe);
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ο<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶȼ<D9B6>Ϊ<EFBFBD><CEAA><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶ<EFBFBD>
memcpy(pDeter->AttRateTarRe,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>̬ȷ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺ģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>
***********************************************/
void LoopAttDeter(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
sAttDeterPara_t *pDeter =NULL;
sAttModPara_t *pWMod =NULL;
MagPara_t *pMag =NULL;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL ==tmpAtt)
{return;}
pDeter=&tmpAtt->sDeterPara;
pWMod=&tmpAtt->sModePara;
pMag=&tmpAtt->sPerPara.MagPara;
pDeter->CLPAttValid=0;
memset(pDeter->CLPAttAngRatPre,0.0,3*sizeof(TYPE_CAL));
memcpy(pDeter->CLPAttAngRatPre, pDeter->CLPAttAngRat, 3*sizeof(TYPE_CAL));
memset(pDeter->CLPAttAng, 0.0, 3*sizeof(TYPE_CAL));
memset(pDeter->CLPAttAngRat, 0.0, 3*sizeof(TYPE_CAL));
#ifdef MINMODULE_TYPE
switch (pWMod->WorkMode)
{
case ATTMOD_RATEDMP:
case ATTMOD_MagTOSUN:
if (POSE_OK == pMag->Mag_Use )
{
pDeter->CLPAttValid= 0x55;
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
}
break;
default:
break;
}
#else
switch (pWMod->WorkMode)
{
case ATTMOD_UNCTL:
case ATTMOD_NOCOLL:
if (0x55 == pDeter->Wi_Vld )
{
pDeter->CLPAttValid= 0x55;
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
}
break;
case ATTMOD_RATEDMP: //zmx20231007
case ATTMOD_MagTOSUN:
if (POSE_OK == pMag->Mag_Use)
{
pDeter->CLPAttValid= 0x55;
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
}
break;
case ATTMOD_WhlTOSUN:
if(0x55 == pDeter->Wi_Vld)
{
pDeter->CLPAttValid=0x55; //zmx20231009
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
if (pDeter->AngToSun_ValidFlg==0x55)
{
//pDeter->CLPAttValid=0x55;
pDeter->CLPAttAng[0]=pDeter->AngToSun[0];
pDeter->CLPAttAng[1]=pDeter->AngToSun[1];
pDeter->CLPAttAng[2]=pDeter->AngToSun[2];
}
}
break;
case ATTMOD_ONLTOEARTH:
case ATTMOD_WHEELTOEARTH:
if(0x55 == pDeter->AttOrb_Vld)
{
pDeter->CLPAttValid=0x55;
memcpy(pDeter->CLPAttAng, pDeter->AttAngOrb, 3*sizeof(TYPE_CAL));
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrb, 3*sizeof(TYPE_CAL));
}
break;
case ATTMOD_ATTAJUST:
case ATTMOD_ONLTOTAR:
if(0x55 == pDeter->AttTarRe_Vld)
{
pDeter->CLPAttValid=0x55;
memcpy(pDeter->CLPAttAng, pDeter->AttAngTarRe, 3*sizeof(TYPE_CAL));
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateTarRe, 3*sizeof(TYPE_CAL));
}
break;
case ATTMOD_ATTAJUSTPRE:
case ATTMOD_ORBITCTL:
if(0x55 == pDeter->AttOrbCtrl_Vld)
{
pDeter->CLPAttValid=0x55;
memcpy(pDeter->CLPAttAng, pDeter->AttAngOrbCtrl, 3*sizeof(TYPE_CAL));
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrbCtrl, 3*sizeof(TYPE_CAL));
}
break;
default:
break;
}
#endif
}
/************************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>ӿڣ<EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ζ<EFBFBD><EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˼<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺wbi:<EFBFBD><EFBFBD><EFBFBD>ݹ<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <EFBFBD><EFBFBD>rad/s<EFBFBD><EFBFBD>
startFlag:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
<EFBFBD><EFBFBD><EFBFBD><EFBFBD> :<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ1<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <EFBFBD><EFBFBD>rad/s<EFBFBD><EFBFBD>
************************************************/
void ZKPGAct(void)
{
#ifdef MINMODULE_TYPE
//<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
ZKPGInit();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD>
SsGyro_AttDeter();
//<2F>ų<EFBFBD><C5B3><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ų<EFBFBD>ǿ<EFBFBD><C7BF>
DertCalcBb();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-Zb<5A><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
Cal_AngToSun();
//<2F>ջ<EFBFBD><D5BB><EFBFBD>̬ȷ<CCAC><C8B7>
LoopAttDeter();
#else
//<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
ZKPGInit();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD>
SsGyro_AttDeter();
//<2F>ų<EFBFBD><C5B3><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ų<EFBFBD>ǿ<EFBFBD><C7BF>
DertCalcBb();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-Zb<5A><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>
Cal_AngToSun();
//printf("ZL0\n");
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>̬
AttDeterOrb();
//printf("ZL1\n");
//Ŀ<><C4BF><EFBFBD>ο<EFBFBD>ϵ<EFBFBD><CFB5>̬
AttDeterTarRe();
//printf("ZL2\n");
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ع<EFBFBD><D8B9><EFBFBD>ϵ<EFBFBD><CFB5>̬
AttDeterOrbCtrl();
//printf("ZL3\n");
//<2F>ջ<EFBFBD><D5BB><EFBFBD>̬ȷ<CCAC><C8B7>
LoopAttDeter();
#endif
}
Reference in New Issue Copy Permalink