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

2137 lines
56 KiB
C
Raw Permalink 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 "..\PrjCommon\DevDefine.h"
#include "AttMath.h"
#include "AttCtrlMain.h"
#include "..\PrjTelCtrlMng\TelCtrlMng.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>
***********************************************/
void ZKDPInit(void)
{
UINT32 i = 0;
sAttPriData *tmpAtt = NULL;
SsPara_t *pStar= NULL;
GyroPara_t *pGyro= NULL;
AssPara_t *pAss= NULL;
MagPara_t *pMag= NULL;
WhlPara_t *PWhl= NULL;
PPUPara_t *PPPU= NULL;
tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL==tmpAtt)
{
return;
}
pAss = &tmpAtt->sPerPara.AssPara;
pMag = &tmpAtt->sPerPara.MagPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
PPPU = &tmpAtt->sPerPara.PPUPara;
//<2F><><EFBFBD><EFBFBD>
for (i = 0; i < STAR_NUM; i++)
{
pStar = &tmpAtt->sPerPara.SsPara[i];
pStar->ssIn_AttVld = POSE_NO;
pStar->ssIn_DataSta = 0;
pStar->ssCrCheckFlg = NO_CC;
memset(pStar->ssIn_W, 0, 3*sizeof(TYPE_CAL));
pStar->ssExpose_Qbi[0] = 0;
pStar->ssExpose_Qbi[1] = 0;
pStar->ssExpose_Qbi[2] = 0;
pStar->ssExpose_Qbi[3] = 1;
memset(pStar->ssExpose_Wbi, 0, 3*sizeof(TYPE_CAL));
}
tmpAtt->sPerPara.Ss_Qi[0] = 0;
tmpAtt->sPerPara.Ss_Qi[1] = 0;
tmpAtt->sPerPara.Ss_Qi[2] = 0;
tmpAtt->sPerPara.Ss_Qi[3] = 1;
memset(tmpAtt->sPerPara.Ss_Wi, 0.0, 3*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD>
for (i = 0; i < GYRO_NUM; i++)
{
pGyro = &tmpAtt->sPerPara.GyroPara[i];
pGyro->GyroIn_AttVld = POSE_NO;
pGyro->Gyro_ComTHUse[0] = POSE_OK;
pGyro->Gyro_ComTHUse[1] = POSE_OK;
pGyro->Gyro_ComTHUse[2] = POSE_OK;
pGyro->GyroIn_DataSta = 0;
pGyro->GyroCrCheckFlg =NO_CC;
memset(pGyro->GyroIn_Tmper, 0, 3*sizeof(TYPE_CAL));
memset(pGyro->Gyro_Wi, 0, 3*sizeof(TYPE_CAL));
}
memset(tmpAtt->sPerPara.GyroIn_CombWi, 0, 3*sizeof(TYPE_CAL));
memset(tmpAtt->sPerPara.Gyro_ATTWi, 0, 3*sizeof(TYPE_CAL));
tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;
//ģ<><C4A3>̫<EFBFBD><CCAB>
for (i = 0; i < ASS_NUM; i++)
{
pAss->AssIn_AttVld[i]= POSE_NO;
memset(pAss->AssIn_sensor[i], 0, 4*sizeof(TYPE_CAL));
pAss->AssCrCheckFlg =NO_CC;
}
pAss->Ass_SunVecb[0]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunVecb[1]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunVecb[2]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunVecVld= POSE_NO; //ģ<><C4A3>̫<EFBFBD><CCAB>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־
pAss->Ass_SunVecbF[0]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunVecbF[1]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunVecbF[2]= 0; //ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>XYZ
pAss->Ass_SunATT[0] = 0;
pAss->Ass_SunATT[1] = 0;
pAss->Ass_SunOK= POSE_NO;
pAss->ADIn_Date= POSE_OK;
//<2F><>ǿ<EFBFBD><C7BF>
for (i = 0; i < MAG_NUM; i++)
{
pMag->MagIn_Valid[i]= POSE_NO; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF>A/B<><42>Ч<EFBFBD><D0A7>־
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memset(pMag->Mag_Bc_MeaAB[i],0,3*sizeof(TYPE_CAL));
memset(pMag->Mag_Bb_MeaAB[i],0,3*sizeof(TYPE_CAL));
}
pMag->MagIn_Valid[2]= POSE_NO; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF>A/B<><42>Ч<EFBFBD><D0A7>־
pMag->MagIn_DataSta= 0;
pMag->MagUseSwich= 0;
pMag->Mag_Bb_MeaZH[0] =0;
pMag->Mag_Bb_MeaZH[1] =0;
pMag->Mag_Bb_MeaZH[2] =0;
for (i = 0; i < MAG_NUM; i++)
{
pMag->MagCrCheckFlg[i]= NO_CC;
pMag->MagAvailableFlg[i] = POSE_OK;
}
memset(pMag->Mag_UBb,0,3*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD>
for (i = 0; i < WHEEL_NUM; i++)
{
PWhl->WhlIn_DataSta[i] = 0;
PWhl->WhlIn_Valid[i] = POSE_NO;
PWhl->Whl_Current[i]= 0.0f;
PWhl->Whl_Tmper[i] = 0.0f;
PWhl->Whl_Momentum[i]= 0; //<2F>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD>ֵĽǶ<C4BD><C7B6><EFBFBD>
}
for(i=0;i<3;i++)
{
PWhl->Whl_Momentum_XYZ[i]= 0.0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD><C7B6><EFBFBD>
}
PWhl->Whl_Momentum_Sum = 0;
#ifndef MINMODULE_TYPE
//<2F><><EFBFBD><EFBFBD>
PPPU->PPUIn_Valid = POSE_NO; //PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־
PPPU->PPUIn_DataSta = 0; //PPU <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чԭ<D0A7><D4AD>
PPPU->PPUIn_ReCnt= 0; //PPUң<55><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
PPPU->PPUIn_AVoltage = 0; //<2F>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>ѹ
PPPU->PPUIn_ACurrent = 0; //<2F>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>
PPPU->PPUT_BOTTLE_Tmper1 = 0.0f;
PPPU->PPUT_BOTTLE_Tmper2 = 0.0f;
PPPU->PPUT_PIPE_Tmper1 = 0.0f;
PPPU->PPUT_PIPE_Tmper2 = 0.0f;
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A\Bԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD>ΪС<EFBFBD><EFBFBD>
***********************************************/
void Ss_DataGet(void ) //ST-APS2-2B(X)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
UINT8 sum = 0;
UINT8 i = 0;
UINT8 j = 0;
UINT16 tmpHEAD[3] = {0,0,0};
UINT16 tmpINFIN[3] ={0,0,0};
INT32 tempQdata[4] = {0,0,0,1};
INT16 tempWdata = 0;
INT32 tempQdataC = 0;
INT16 tempWdataC = 0;
sAttPriData *tmpAtt = NULL;
sDevInfo * tmpDevInfo[3]= {NULL};
SsPara_t *PStar[STAR_NUM]= {NULL};
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL==tmpAtt)
{
return;
}
//<2F><>ȡ<EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD>ݵ<EFBFBD>ַ
tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART15); //<2F><><EFBFBD><EFBFBD>A<EFBFBD><41>803<30><33>
tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART16); //<2F><><EFBFBD><EFBFBD>B<EFBFBD><42>803<30><33>
tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART4); //<2F><><EFBFBD><EFBFBD>C<EFBFBD><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
//<2F><>¼ǰ<C2BC><C7B0>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (i = 0; i < STAR_NUM; i++)
{
memcpy(PStar[i]->ssIn_Qpre, PStar[i]->ssIn_Q, 4*sizeof(TYPE_CAL));
//<2F><>ʼ<EFBFBD><CABC>
PStar[i]->ssIn_Q[0] = 0;
PStar[i]->ssIn_Q[1] = 0;
PStar[i]->ssIn_Q[2] = 0;
PStar[i]->ssIn_Q[3] = 1;
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ
for (i = 0; i < STAR_NUM; i++)
{
tmpHEAD[i] = (tmpDevInfo[i]->cDevYCBuffer[0 + 20]<<8)|tmpDevInfo[i]->cDevYCBuffer[0 + 20+1];
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2>
tmpINFIN[0] = (tmpDevInfo[0]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + 20+3];
tmpINFIN[1] = (tmpDevInfo[1]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + 20+3];
tmpINFIN[2] = tmpDevInfo[2]->cDevYCBuffer[0 + 20+2];
//<2F><><EFBFBD><EFBFBD>AB<41><42><EFBFBD>ݻ<EFBFBD>ȡ
for (i = 0; i < (STAR_NUM -1); i++)
{
if (INFO_SIG_CHANGE == tmpDevInfo[i] ->cDevYCBuffer[0])
{
if ((STARAB_HEAD != tmpHEAD[i]) || (STARAB_ID != tmpINFIN[i]))
{
PStar[i]->ssIn_AttVld = POSE_NO;
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta , 1, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ|ID<49>Ŵ<EFBFBD><C5B4><EFBFBD> */
}
else
{
sum = 0;
/*<2A>ж<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
for(j = 0; j < (STARAB_LEN - 1); j++)
{
sum += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];
}
if ((0xFF & sum) != tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+STARAB_LEN-1])
{
PStar[i]->ssIn_AttVld = POSE_NO;
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 2, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>Ͳ<EFBFBD><CDB2><EFBFBD> */
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>״̬<D7B4><CCAC>1<EFBFBD>ж<EFBFBD>
if (0x20 == (tmpDevInfo[i]->cDevYCBuffer[20+20] & 0x30))
{
PStar[i]->ssIn_AttVld = POSE_NO;
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 3, 1 ); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
}
else
{
PStar[i]->ssIn_AttVld = POSE_OK;
}
}
}
}
else
{
PStar[i]->ssIn_AttVld = POSE_NO;
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 0, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD> */
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>AB<41><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (POSE_OK == PStar[i]->ssIn_AttVld)
{
//<2F><>Ԫ<EFBFBD><D4AA>
for(j = 0; j < 4; j++)
{
tempQdata[j] = DoGetInt32From8(&(tmpDevInfo[i]->cDevYCBuffer[20+4*j+4]));
}
PStar[i]->ssIn_Q[3] = tempQdata[0]/2147483647.5f;
PStar[i]->ssIn_Q[0] = tempQdata[1]/2147483647.5f;
PStar[i]->ssIn_Q[1] = tempQdata[2]/2147483647.5f;
PStar[i]->ssIn_Q[2] = tempQdata[3]/2147483647.5f;
//<2F><><EFBFBD>ٶ<EFBFBD>
for(j = 0; j < 3; j++)
{
tempWdata = DoGetInt16From8(&tmpDevInfo[i]->cDevYCBuffer[20+2*j+45]);
PStar[i]->ssIn_W[j] = ((float)tempWdata)*ANG2RADIAN*POSE_2T13;
}
PStar[i]->ssIn_W[2] = - PStar[i]->ssIn_W[2];
}
}
//<2F><><EFBFBD><EFBFBD>C<EFBFBD><43><EFBFBD>ݻ<EFBFBD>ȡ
if (INFO_SIG_CHANGE == tmpDevInfo[2] ->cDevYCBuffer[0])
{
if ((STARC_HEAD != tmpHEAD[2]) || (STARC_ID != tmpINFIN[2]))
{
PStar[2]->ssIn_AttVld = POSE_NO;
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta , 1, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ|ID<49>Ŵ<EFBFBD><C5B4><EFBFBD> */
}
else
{
sum = 0;
/*<2A>ж<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
for(j = 2; j < (STARC_LEN - 1); j++)
{sum += tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+j];}
if ((0xFF & sum) != tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+STARC_LEN - 1])
{
PStar[2]->ssIn_AttVld = POSE_NO;
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 2, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>Ͳ<EFBFBD><CDB2><EFBFBD> */
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>״̬<D7B4><CCAC>1<EFBFBD>ж<EFBFBD>
if ((tmpDevInfo[2]->cDevYCBuffer[20+33] & 0x01) != 0x01)
{
PStar[2]->ssIn_AttVld = POSE_NO;
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 3, 1 ); /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
}
else
{
PStar[2]->ssIn_AttVld = POSE_OK;
}
}
}
}
else
{
PStar[2]->ssIn_AttVld = POSE_NO;
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 0, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD> */
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (POSE_OK == PStar[2]->ssIn_AttVld)
{
//<2F><>Ԫ<EFBFBD><D4AA>
for(j = 0; j < 4; j++)
{
tempQdataC = DoGetInt32From8(&tmpDevInfo[2]->cDevYCBuffer[20+4*j+3]);
PStar[2]->ssIn_Q[j] = ((float)tempQdataC)/2147483647.0f;
}
//<2F><><EFBFBD>ٶ<EFBFBD>
for(j = 0; j < 3; j++)
{
tempWdataC = DoGetInt16From8(&tmpDevInfo[2]->cDevYCBuffer[20+2*j+42]);
PStar[2]->ssIn_W[j] = ((float)tempWdataC)*ANG2RADIAN*POSE_2T11;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Ss_DataPre(void)
{
UINT8 i = 0;
UINT8 j = 0;
TYPE_CAL tmpRbs[9] = {0.0};
TYPE_CAL tmpRsi[9] = {0.0};
TYPE_CAL tmpRbi[9] = {0.0};
TYPE_CAL tmp_SS[3][3][3] = {0.0}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
SsPara_t *PStar[STAR_NUM] = {NULL};
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt)||(NULL==tmpConst))
return;
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
memcpy(&(tmp_SS[0][0][0]), &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9*sizeof(TYPE_CAL));
memcpy(&(tmp_SS[1][0][0]), &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9*sizeof(TYPE_CAL));
memcpy(&(tmp_SS[2][0][0]), &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9*sizeof(TYPE_CAL));
for (i = 0; i < STAR_NUM; i++)
{
if ((POSE_OK ==PStar[i]->ssIn_AttVld))
{
if (POSE_ABSF(PStar[i]->ssIn_Qpre[0]-PStar[i]->ssIn_Q[0])< SENSOR_ZERO
&&POSE_ABSF(PStar[i]->ssIn_Qpre[1]-PStar[i]->ssIn_Q[1])< SENSOR_ZERO
&&POSE_ABSF(PStar[i]->ssIn_Qpre[2]-PStar[i]->ssIn_Q[2])< SENSOR_ZERO
&&POSE_ABSF(PStar[i]->ssIn_Qpre[3]-PStar[i]->ssIn_Q[3])< SENSOR_ZERO)
{
if(PStar[i]->ssExpose_cnt<255)
{
PStar[i]->ssExpose_cnt++;
}
}
else
{
PStar[i]->ssExpose_cnt =0;
}
if (PStar[i]->ssExpose_cnt >= 5)
{
PStar[i]->ssIn_AttVld = POSE_NO;
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 4, 1 );
}
else
{
if(PStar[i]->ssIn_Q[3]<0)
{
for (j = 0; j < 4; j++)
{
PStar[i]->ssIn_Q[j] = - PStar[i]->ssIn_Q[j];
}
}
Q2Att(PStar[i]->ssIn_Q, tmpRsi);
memcpy(tmpRbs, &(tmp_SS[i][0][0]), 9 * sizeof(TYPE_CAL));
MatrixProductHL(tmpRbs, tmpRsi, tmpRbi, 3, 3, 3);
Att2Q(tmpRbi, PStar[i]->ssExpose_Qbi);
MatrixProductHL(tmpRbs, PStar[i]->ssIn_W, PStar[i]->ssExpose_Wbi, 3, 3, 1);
PStar[i]->ssIn_AttVld=POSE_OK;
}
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Gyro_DataGet(void)
{
UINT8 i = 0;
UINT8 j = 0;
UINT8 k = 0;
INT32 tmpDtInt = 0;
INT16 tmpDtTmper = 0;
UINT8 sum[GYRO_NUM] = {0,0,0};
UINT16 tmpHEAD[GYRO_NUM] = {0,0,0};
UINT8 tmpLONG[GYRO_NUM] = {0,0,0};
UINT16 tmpDATAHEAD[GYRO_NUM] = {0,0,0};
TYPE_CAL tmpGyroIn_Speed[3]= {0.0,0.0,0.0};
TYPE_CAL tmpkGyroIn_Speed[3]= {0.0,0.0,0.0};
UINT8 tmpcnt[GYRO_NUM][3] = {{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK}};
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sDevInfo * tmpDevInfo[GYRO_NUM] = {NULL};
GyroPara_t *PGyro[GYRO_NUM] = {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];
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>ݵ<EFBFBD>ַ
tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART5) ;
tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART6) ;
tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART7) ;
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>ݰ<EFBFBD>ͷ
tmpHEAD[0]=(tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
tmpHEAD[1]=(tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
tmpHEAD[2]=(tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
tmpDATAHEAD[0] = GYRO_DATA_HEADA;
tmpDATAHEAD[1] = GYRO_DATA_HEADA;
tmpDATAHEAD[2] = GYRO_DATA_HEADC;
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>ݳ<EFBFBD>
tmpLONG[0] = GYRO_DATA_LENA;
tmpLONG[1] = GYRO_DATA_LENA;
tmpLONG[2] = GYRO_DATA_LENC;
//<2F><>¼ǰ<C2BC><C7B0>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (i = 0; i < GYRO_NUM; i++)
{
memcpy(PGyro[i]->GyroIn_SpeedPre, PGyro[i]->GyroIn_Speed, 3*sizeof(TYPE_CAL));
//<2F><>ʼ<EFBFBD><CABC>
memset(PGyro[i]->GyroIn_Speed, 0, 3*sizeof(TYPE_CAL));
}
for (i = 0; i < GYRO_NUM ; ++i)
{
if (INFO_SIG_CHANGE == tmpDevInfo[i]->cDevYCBuffer[0])
{
if (tmpHEAD[i] == tmpDATAHEAD[i])
{
/*<2A>ж<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
for(j = 0; j < tmpLONG[i] - 1; j++)
{
sum[i] += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];//<2F><><EFBFBD>ݵİ<DDB5><C4B0><EFBFBD>
}
if ((0xFF & sum[i]) == tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+tmpLONG[i] - 1])
{
PGyro[i]->GyroIn_AttVld = POSE_OK;
}
else
{
PGyro[i]->GyroIn_AttVld = POSE_NO;
PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 2, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD>A<EFBFBD><41><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD>ʹ<EFBFBD><CDB4><EFBFBD> */
}
}
else
{
PGyro[i]->GyroIn_AttVld = POSE_NO;
PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta, 1, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD>A<EFBFBD><41>ͷ|<7C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
}
}
else
{
PGyro[i]->GyroIn_AttVld = POSE_NO;
PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 0, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD>A<EFBFBD><41><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD> */
}
}
//<2F><><EFBFBD><EFBFBD>AB<41><42><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>
for (j = 0; j < 2 ; ++j)
{
if (POSE_OK ==PGyro[j]->GyroIn_AttVld)
{
for (i = 0; i < 3 ; ++i)
{
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>/s
tmpDtInt = DoGetInt32From8(&(tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+4*i+9]));
tmpGyroIn_Speed[i] = (float)tmpDtInt * (1.0E-5)*ANG2RADIAN ;
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ȣ<C2B6><C8A3><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
tmpDtTmper= tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+21]<<8|tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+22];
PGyro[j]->GyroIn_Tmper[i] = (float)(tmpDtTmper)*(1.0E-2);
}
MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[j][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);
for (i = 0; i < 3 ; ++i)
{
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>/s
PGyro[j]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[j][i] ;
}
}
}
//<2F><><EFBFBD><EFBFBD>C<EFBFBD><43><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>
if (POSE_OK == PGyro[2]->GyroIn_AttVld)
{
for (i = 0; i < 3 ; ++i)
{
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>/s
tmpGyroIn_Speed[i] = ((PoseGetInt32From24(&(tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+3*i+9]))))*POSE_POW(2,-18)*ANG2RADIAN ;
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ȣ<C2B6><C8A3><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
tmpDtTmper= tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+19]<<8|tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+20];
PGyro[2]->GyroIn_Tmper[i] = (float)(tmpDtTmper)*POSE_POW(2,-8);
}
MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[2][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);
for (i = 0; i < 3 ; ++i)
{
//XYZ<59><5A><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>,<2C><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>/s
PGyro[2]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[2][i] ;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for (i = 0; i < GYRO_NUM ; ++i)
{
if (POSE_OK == PGyro[i]->GyroIn_AttVld)
{ for (k = 0; k < 3 ; ++k)
{
if (POSE_ABSF(PGyro[i]->GyroIn_SpeedPre[k] - PGyro[i]->GyroIn_Speed[k]) < POSE_ZERO)
{
if (PGyro[i]->Gyro_ComCnt[k] < 60000)
{
PGyro[i]->Gyro_ComCnt[k]++;
}
}
else
{
PGyro[i]->Gyro_ComCnt[k] =0;
}
if (POSE_ABSF(PGyro[i]->GyroIn_Speed[k]) > 0.436)
{
tmpcnt[i][k] = POSE_NO;
}
else
{
tmpcnt[i][k] = POSE_OK;
}
if ((PGyro[i]->Gyro_ComCnt[k] < 5)&& (tmpcnt[i][k] == POSE_OK))
{
PGyro[i]->Gyro_ComTHUse[k] = POSE_OK;
}
else
{
PGyro[i]->Gyro_ComTHUse[k] = POSE_NO;
PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 3+k, 1 );
}
}
}
else
{
PGyro[i]->Gyro_ComTHUse[0] = POSE_NO;
PGyro[i]->Gyro_ComTHUse[1] = POSE_NO;
PGyro[i]->Gyro_ComTHUse[2] = POSE_NO;
}
if ((POSE_NO == PGyro[i]->Gyro_ComTHUse[0])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[1])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[2]))
{
PGyro[i]->GyroIn_AttVld= POSE_NO;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Gyro_DataPre(void)
{
TYPE_CAL tmpRbs[9]= {0.0};
TYPE_CAL tmpRbi[3]= {0.0,0.0,0.0};//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>XYZ
UINT8 i = 0;
UINT8 k = 0;
UINT8 tmpGyro = POSE_NO;
UINT8 tmpComTHUse[3] = {0,0,0};
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
GyroPara_t *PGyro[GYRO_NUM] = {NULL};
AttIMPTPara_t *pIMU = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt)||(NULL==tmpConst))
{
return;
}
pIMU=&tmpAtt->sIMPTPara;
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
if ((0x55 == PGyro[0]->GyroIn_AttVld) || (0x55 == PGyro[1]->GyroIn_AttVld) || (0x55 == PGyro[2]->GyroIn_AttVld))
{
for (i = 0; i < GYRO_NUM ; ++i)
{
tmpGyro = POSE_NO;
if (POSE_OK == PGyro[i]->GyroIn_AttVld)
{
for (k = 0; k < 3 ; ++k)
{
tmpRbi[k] = PGyro[i]->GyroIn_Speed[k];
}
tmpGyro = POSE_OK;
}
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_GYRO[i][0][0]), 9 * sizeof(TYPE_CAL));
if (tmpGyro == POSE_OK)
{
MatrixProductHL(tmpRbs, tmpRbi, PGyro[i]->Gyro_Wi, 3, 3, 1);
}
}
}
if((POSE_NO == pIMU->GyroUseState[0]) && (POSE_NO == pIMU->GyroUseState[1]) && (POSE_NO == pIMU->GyroUseState[2]))
{
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
{
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][0];
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][0];
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][0];
tmpRbi[0] = PGyro[0]->GyroIn_Speed[0];
tmpComTHUse[0] = 0x55;
}
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
{
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][0];
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][0];
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][0];
tmpRbi[0] = PGyro[1]->GyroIn_Speed[0];
tmpComTHUse[0] = 0x55;
}
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
{
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][0];
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][0];
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][0];
tmpRbi[0] = PGyro[2]->GyroIn_Speed[0];
tmpComTHUse[0] = 0x55;
}
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
{
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][1];
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][1];
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][1];
tmpRbi[1] = PGyro[0]->GyroIn_Speed[1];
tmpComTHUse[1] = 0x55;
}
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
{
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][1];
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][1];
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][1];
tmpRbi[1] = PGyro[1]->GyroIn_Speed[1];
tmpComTHUse[1] = 0x55;
}
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
{
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][1];
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][1];
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][1];
tmpRbi[1] = PGyro[2]->GyroIn_Speed[1];
tmpComTHUse[1] = 0x55;
}
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
{
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][2];
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][2];
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][2];
tmpRbi[2] = PGyro[0]->GyroIn_Speed[2];
tmpComTHUse[2] = 0x55;
}
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
{
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][2];
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][2];
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][2];
tmpRbi[2] = PGyro[1]->GyroIn_Speed[2];
tmpComTHUse[2] = 0x55;
}
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
{
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][2];
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][2];
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][2];
tmpRbi[2] = PGyro[2]->GyroIn_Speed[2];
tmpComTHUse[2] = 0x55;
}
if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]) )
{
tmpAtt->sPerPara.GyroIn_Comb = POSE_OK;
MatrixProductHL(tmpRbs, tmpRbi, tmpAtt->sPerPara.GyroIn_CombWi, 3, 3, 1);
}
else
{
tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡģ<EFBFBD><EFBFBD>̫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Ass_DataGet(void )
{
UINT32 i = 0;
UINT32 j = 0;
INT16 tmpDtInt = 0;
INT16 *pAss16 = NULL;
sDevInfo * tmpDevInfo = NULL;
sAttPriData *tmpAtt = NULL;
AssPara_t *pAss = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL==tmpAtt)
{
return;
}
pAss = &tmpAtt->sPerPara.AssPara;
//UINT32 ID[6] = {97,113,101,117,121,125};
UINT32 ID[6] = {SA1_TM_XP1-1, SA9_TM_YN1-1,SA5_TM_XN1-1,SA13_TM_YP1-1,SA17_TM_ZN1-1,SA21_TM_P1-1};
//<2F><>ȡAD<41>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
pAss16 = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
//ģ<><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (INFO_SIG_CHANGE ==tmpDevInfo->cDevYCBuffer[0])
{
for (i= 0; i < ASS_NUM; i++)
{
for (j= 0; j < 4; j++)
{
//<2F><>ȡģ<C8A1><C4A3>̫<EFBFBD><CCAB>6*4<><34><EFBFBD><EFBFBD>Ƭ<EFBFBD><C6AC>ѹ
tmpDtInt = *(pAss16 + ID[i] +j);
pAss->AssIn_sensor[i][j] = (((TYPE_CAL)(tmpDtInt))*10/4095.0-5.0)/966.0*1000.0 ;
}
}
pAss->ADIn_Date = POSE_OK;
}
else
{
pAss->ADIn_Date = POSE_NO;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Ass_DataPre(void)
{
UINT32 i = 0;
UINT32 j = 0;
UINT32 k = 0;
UINT32 iMax = 0;
UINT8 tempAssUseState[ASS_NUM]= {0,0,0,0,0,0};; //<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־
TYPE_CAL tmpSumMax = 0.0;
TYPE_CAL tmpAss_data_SUM[ASS_NUM] = {0.0,0.0,0.0,0.0,0.0,0.0};
TYPE_CAL tmpU[4] = {0.0,0.0,0.0,0.0};
TYPE_CAL tmpRbs[9]= {0.0};
TYPE_CAL tmpSm[3]= {0.0};
TYPE_CAL tmpXs = 0.0;
TYPE_CAL tmpYs = 0.0;
TYPE_CAL tmpX = 1.0;
TYPE_CAL tmpY = 1.0;
TYPE_CAL tmpa=5.0;//<2F><><EFBFBD><EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>
TYPE_CAL tmpH=1.95;//<2F><>դ<EFBFBD><D5A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TYPE_CAL tmpTheta = 0.0;
TYPE_CAL tmpGamma = 0.0;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
AssPara_t *pAss = NULL;
AttIMPTPara_t *pImpt = NULL;
sAttModPara_t *pWMod = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
{
return;
}
pAss = &tmpAtt->sPerPara.AssPara;
pImpt=&tmpAtt->sIMPTPara;
pWMod=&tmpAtt->sModePara;
if (POSE_OK == pAss->ADIn_Date)
{
for (i= 0; i < 4; i++)
{
for (j= 0; j < 4; j++)
{
tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
}
if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[0] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[0]))
{
pAss->AssIn_AttVld[i] = POSE_OK;
}
else
{
pAss->AssIn_AttVld[i] = POSE_NO;
}
}
for (i= 4; i < ASS_NUM; i++)
{
for (j= 0; j < 4; j++)
{
tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
}
if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[1] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[1]))
{
pAss->AssIn_AttVld[i] = POSE_OK;
}
else
{
pAss->AssIn_AttVld[i] = POSE_NO;
}
}
}
if ((0x55 == pWMod->WModSadaOpen)&& (0x55 != pWMod->WModSadaOpenPre))
{
pImpt->AssUseState[4] = POSE_NO;
}
//ѡ<><D1A1><EFBFBD><EFBFBD>ǰʹ<C7B0>õ<EFBFBD>ģ<EFBFBD><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><C3A3><EFBFBD>û<EFBFBD><C3BB>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD><DABC><EFBFBD>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>Ч
for (k= 0; k < ASS_NUM -1; k++)
{
if ((POSE_OK == pAss->AssIn_AttVld[k])&& (POSE_OK == pImpt->AssUseState[k]))
{
if(tmpAss_data_SUM[k]>tmpSumMax)
{
tmpSumMax=tmpAss_data_SUM[k];
iMax=k;
}
tempAssUseState[k] = 0x55;
}
}
if((0 == tempAssUseState[0])&&(0 == tempAssUseState[1])&&(0 == tempAssUseState[2])&&(0 == tempAssUseState[3])&&(0 == tempAssUseState[4]))//̫<><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
{
pAss->Ass_SunVecVld = POSE_NO;
}
else
{
if(0 == iMax)
{
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSA[0][0]), 9 * sizeof(TYPE_CAL));
}
else if(1 == iMax)
{
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSB[0][0]), 9 * sizeof(TYPE_CAL));
}
else if(2 == iMax)
{
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSC[0][0]), 9 * sizeof(TYPE_CAL));
}
else if(3 == iMax)
{
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSD[0][0]), 9 * sizeof(TYPE_CAL));
}
else if(4 == iMax)
{
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSE[0][0]), 9 * sizeof(TYPE_CAL));
}
if ((iMax>=0)&&(iMax<4))
{
tmpH = 1.95;
}
else
{
tmpH = 4.0;
}
tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
if(tmpXs!=0)
{
tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
}
tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
if(tmpYs!=0)
{
tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>λ<EFBFBD>Ǧȡ<C7A6><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>r
tmpTheta = POSE_ATAN2F(tmpY, tmpX);
tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);
tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
tmpSm[2] = POSE_COSF(tmpGamma);
MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecb, 3, 3, 1);
pAss->Ass_SunVecVld= POSE_OK;
}
//<2F><><EFBFBD>㷫<EFBFBD><E3B7AB>ģ<EFBFBD><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>λ<EFBFBD>Ǧȡ<C7A6><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>r
if ((POSE_OK == pAss->AssIn_AttVld[5])&& (POSE_OK == pImpt->AssUseState[5]))
{
tmpH = 4.0;
memcpy(tmpU, &(pAss->AssIn_sensor[5][0]), 4*sizeof(TYPE_CAL));
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSF[0][0]), 9 * sizeof(TYPE_CAL));
tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
if(tmpXs!=0)
{
tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
}
tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
if(tmpYs!=0)
{
tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>λ<EFBFBD>Ǧȡ<C7A6><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>r
tmpTheta = POSE_ATAN2F(tmpY, tmpX);
tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);
pAss->Ass_SunATT[0] = tmpTheta;
pAss->Ass_SunATT[1] = tmpGamma;
tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
tmpSm[2] = POSE_COSF(tmpGamma);
MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecbF, 3, 3, 1);
pAss->Ass_SunOK = POSE_OK;
}
else
{
pAss->Ass_SunOK = POSE_NO;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Mag_DataGet(void)
{
TYPE_CAL tmpMag_Bb_Limit = 55000.0; //sjd20231123
INT16 tmpDtInt = 0;
INT16 *pMag16 = NULL;
UINT32 i = 0;
UINT32 j = 0;
TYPE_CAL tmpVm[MAG_NUM][3] = {{0,0,0},{0,0,0}};
UINT32 ID = 2;
ID = SV3_CQJA_X -1 ;
sDevInfo * tmpDevInfo = NULL;
sAttPriData *tmpAtt= NULL;
AttCtrlConst_t *tmpConst= NULL;
MagPara_t *pMag= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
pMag = &tmpAtt->sPerPara.MagPara;
//<2F><>ȡAD<41>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
pMag16 = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
//ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD>ƵIJ<C6B5><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (INFO_SIG_CHANGE == tmpDevInfo -> cDevYCBuffer[0])
{
for (i = 0; i < MAG_NUM; ++i)
{
//<2F><><EFBFBD><EFBFBD>Vm
for (j = 0; j< 3; j++)
{
tmpDtInt = *(pMag16 + ID +j + 3*i);
tmpVm[i][j] = tmpConst->AttCmdFlashPara.MAG_LK[i][j] * (((float)(tmpDtInt))*10/4095 - 5 ) + tmpConst->AttCmdFlashPara.MAG_LB[i][j]; //MAG_LK 24000
}
}
if ((POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit))
{
if (POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 1, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
if (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 2, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
if (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 3, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
pMag->MagIn_Valid[0] = POSE_NO;
}
else
{
memcpy(pMag->Mag_Bc_MeaAB[0], tmpVm[0], 3*sizeof(TYPE_CAL));
pMag->MagIn_Valid[0] = POSE_OK;
}
if ((POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit) || (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)||(POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit))
{
if (POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 4, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
if (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 5, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
if (POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit)
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 6, 1 ); //<2F><>ǿ<EFBFBD><C7BF>A<EFBFBD><41><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
}
pMag->MagIn_Valid[1] = POSE_NO;
}
else
{
memcpy(pMag->Mag_Bc_MeaAB[1], tmpVm[1], 3*sizeof(TYPE_CAL));
pMag->MagIn_Valid[1] = POSE_OK;
}
}
else
{
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 0, 1 ); //<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD>
pMag->MagIn_Valid[0] = POSE_NO;
pMag->MagIn_Valid[1] = POSE_NO;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Mag_DataPre(void)
{
UINT8 i = 0;
TYPE_CAL tmKm[3][3] = {{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0}};
TYPE_CAL tmpMag_Bc_MeaAB[3]= {0.0,0.0,0.0};
UINT8 tmpComTHUse[3] = {0,0,0};
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
MagPara_t *pMag = NULL;
AttIMPTPara_t *pIMU = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{
return;
}
pMag = &tmpAtt->sPerPara.MagPara;
pIMU=&tmpAtt->sIMPTPara;
//<2F><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ǿ<EFBFBD><C7BF>A<EFBFBD><41>ʸ<EFBFBD><CAB8>
if (POSE_OK == pMag->MagIn_Valid[0])
{
memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[0], 9 * sizeof(TYPE_CAL));
MatrixPlusVectorN( tmKm[0], pMag->Mag_Bc_MeaAB[0], pMag->Mag_Bb_MeaAB[0], 3, 3 );
}
if (POSE_OK ==pMag->MagIn_Valid[1])
{
memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[1], 9 * sizeof(TYPE_CAL));
MatrixPlusVectorN( tmKm[0], pMag->Mag_Bc_MeaAB[1], pMag->Mag_Bb_MeaAB[1], 3, 3 );
}
if ((POSE_NO == pIMU->MagUseState[0])&&(POSE_NO == pIMU->MagUseState[1]))
{
for (i = 0; i < 3; ++i)
{
if (0x11 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
{
tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[0][0][i];
tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[0][1][i];
tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[0][2][i];
tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[0][i];
tmpComTHUse[i] = 0x55;
}
else if (0x22 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
{
tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[1][0][i];
tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[1][1][i];
tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[1][2][i];
tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[1][i];
tmpComTHUse[i] = 0x55;
}
}
if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]))
{
MatrixPlusVectorN( tmKm[0], tmpMag_Bc_MeaAB, pMag->Mag_Bb_MeaZH, 3, 3 );
pMag->MagIn_Valid[2] = POSE_OK;
}
else
{
pMag->MagIn_Valid[2] = POSE_NO;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Whl_DataGet(void)
{
UINT8 sum= 0;
UINT32 i = 0;
UINT32 j = 0;
UINT16 tmpU16=0;
INT16 tmpI16=0;
UINT8 tmpWhlHEAD = 0;
UINT8 tmpWhlHEADP = 0;
UINT8 tmpWhlHEADID[4]={0};
UINT16 tmpWhlEnd = 0;
sDevInfo * tmpDevInfo = NULL;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
WhlPara_t *PWhl = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{
return;
}
PWhl = &tmpAtt->sPerPara.WhlPara;
tmpWhlHEADID[0] = tmpConst->Whl_ID[0];
tmpWhlHEADID[1] = tmpConst->Whl_ID[1];
tmpWhlHEADID[2] = tmpConst->Whl_ID[2];
tmpWhlHEADID[3] = tmpConst->Whl_ID[3];
//<2F><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>
memcpy(PWhl->Whl_RatePre,PWhl->Whl_Rate,4*sizeof(TYPE_CAL));
memcpy(PWhl->Whl_TimePRE,PWhl->Whl_Time,4*sizeof(UINT32));
for (i = 0; i < (WHEEL_NUM); ++i)
{
tmpDevInfo = GETDEVINFO(i+ DEV_NO_UART8) ;
tmpWhlHEAD = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF];
tmpWhlHEADP = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
tmpWhlEnd = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+32]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+33];
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>*/
if (INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
{
sum = 0;
for(j = 2; j < WHL_DATA_LEN -1; j++)
{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +j];}
sum = 0xFF - sum;
if((WHL_DATA_END != tmpWhlEnd)||(WHL_CMD_CODE_REQ!=tmpWhlHEADP)||(tmpWhlHEADID[i]!=tmpWhlHEAD))
{
PWhl->WhlIn_Valid[i] = POSE_NO;
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 1, 1 ); /* errNo=0,<2C><><EFBFBD>ְ<EFBFBD>ͷ|<7C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
}
else
{
if (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+ WHL_DATA_LEN -1]!=sum)
{
PWhl->WhlIn_Valid[i] = POSE_NO;
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 2, 1 ); /* errNo=0,<2C><><EFBFBD>ְ<EFBFBD>ͷ|<7C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
}
else
{
PWhl->Whl_Time[i]=DoGetInt32From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +2]));
tmpI16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+22]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+23];
tmpU16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+25];
PWhl->Whl_Rate[i] = DoGetFloatFrom8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+10]))*RADS2RPM;
PWhl->Whl_Current[i] = tmpI16;
PWhl->Whl_Tmper[i] = 0.4863 * (TYPE_CAL)(tmpU16) - 273.15 ;
PWhl->WhlIn_Valid[i] = POSE_OK;
}
}
}
else
{
PWhl->WhlIn_Valid[i] = POSE_NO;
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 0, 1 ); /* errNo=0,<2C><><EFBFBD><EFBFBD>ת<EFBFBD>ٲ<EFBFBD><D9B2><EFBFBD><EFBFBD><EFBFBD> */
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void Whl_DataPre(void)
{
UINT8 i = 0;
sAttPriData *tmpAtt = NULL;
WhlPara_t *PWhl = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{
return;
}
PWhl = &tmpAtt->sPerPara.WhlPara;
for (i = 0; i < WHEEL_NUM; ++i)
{
if (POSE_OK == PWhl->WhlIn_Valid[i])
{
if (POSE_ABSF( PWhl->Whl_Rate[i]- PWhl->Whl_RatePre[i] ) > 100*RADS2RPM) //200t230
{
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 4, 1 );
PWhl->WhlIn_Valid[i] = POSE_NO;
}
else
{
PWhl->WhlIn_Valid[i] = POSE_OK;
}
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void PPU_DataPre(void)
{
UINT16 tmpPPUHEAD = 0;
UINT8 tmpPPSUM = 0;
UINT8 sum = 0;
UINT8 i = 0;
UINT32 TempSatTime[2] ={0,0};
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sDevInfo * tmpDevInfo = NULL;
PPUPara_t *PPPU = NULL;
sAttOrbitCtlInfo_t *pOrb = NULL;
sAttModPara_t *pWMod= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{
return;
}
pWMod=&tmpAtt->sModePara;
PPPU = &tmpAtt->sPerPara.PPUPara;
pOrb=&tmpAtt->sOrbitInp;
tmpDevInfo = GETDEVINFO(DEV_NO_CAN0) ;
tmpPPUHEAD = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
tmpPPSUM = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF + PPU_CMD_LEN +1];
/*<2A><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ʱ*/
GetTime(TempSatTime);
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD>ж<EFBFBD>*/
if(INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
{
for(i = 0; i < PPU_CMD_LEN+1 ; i++)
{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +i];}
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD>ж<EFBFBD>*/
if((PPU_CMD_LEN == tmpPPUHEAD )&&(sum == tmpPPSUM))
{
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>з<EFBFBD><D0B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPUIn_ReCntPre = PPPU->PPUIn_ReCnt;
PPPU->PPUIn_ReCnt= tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+3];
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>£<EFBFBD><C2A3><EFBFBD>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if (PPPU->PPUIn_ReCntPre == PPPU->PPUIn_ReCnt)
{
PPPU->PPUIn_Valid = POSE_NO;
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 2, 1 );
}
else
{
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8>£<EFBFBD><C2A3><EFBFBD>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
PPPU->PPUIn_AVoltage=tmpConst->AttCmdFlashPara.PPUPVK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+15])) + tmpConst->AttCmdFlashPara.PPUPVB; //<2F>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>ѹ
PPPU->PPUIn_ACurrent=tmpConst->AttCmdFlashPara.PPUPAK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+17])) + tmpConst->AttCmdFlashPara.PPUPAB; //<2F>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD>ĵĵ<C4B5><C4B5><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0>еĵ<D0B5><C4B5><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>*/
PPPU->PPUIn_FIREOKPre = PPPU->PPUIn_FIREOK;
PPPU->PPUIn_FIREOK = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24]; //<2F><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־
PPPU->PPUIn_Valid = POSE_OK;
}
}
else
{
/*PPU<50><55><EFBFBD><EFBFBD>У<EFBFBD>鲻ͨ<E9B2BB><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
PPPU->PPUIn_Valid = POSE_NO;
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 1, 1 );
///*PPU<50><55><EFBFBD><EFBFBD>У<EFBFBD>鲻ͨ<E9B2BB><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־ Ϊ δ<><CEB4><EFBFBD><EFBFBD>*/
//PPPU->PPUIn_FIREOKPre = 0x00;
//
///*<2A><>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>鲻ͨ<E9B2BB><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ĵĵ<C4B5><C4B5><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>*/
//if(0x01 == PPPU->PPUIn_FIREOK)
//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
//
//PPPU->PPUIn_FIREOK = 0x00;
//
//return;
}
}
else
{
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
PPPU->PPUIn_Valid = POSE_NO;
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 0, 1 );
///*PPU<50><55><EFBFBD><EFBFBD>У<EFBFBD>鲻ͨ<E9B2BB><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ġ<EFBFBD><C4A1><EFBFBD>ǰ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־ Ϊ δ<><CEB4><EFBFBD><EFBFBD>*/
//PPPU->PPUIn_FIREOKPre = 0x00;
//
///*<2A><>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ĵĵ<C4B5><C4B5><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>*/
//if(0x01 == PPPU->PPUIn_FIREOK)
//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
//
//PPPU->PPUIn_FIREOK = 0x00;
//
//return;
}
/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD> <20>ܴ<EFBFBD><DCB4><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> һֱ<D2BB><D6B1>¼*/
if(0x01 == PPPU->PPUIn_FIREOK)
{
if (PPPU->PPUT_FIR_Sum< 0xFFFFFFFE)
{
PPPU->PPUT_FIR_Sum++ ;
}
else
{
PPPU->PPUT_FIR_Sum= 0;
}
}
/*PPU<50><55> <20><><EFBFBD><EFBFBD> <20><> δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>¼*/
if ((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
{
if(PPPU->PPUT_FIR_Cnt>0)
PPPU->PPUT_FIR_Cnt--;
}
/*PPU<50><55>δ<EFBFBD><CEB4><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>¼*/
if ((0x01 != PPPU->PPUIn_FIREOKPre)&&(0x01 == PPPU->PPUIn_FIREOK))
{
//if(PPPU->PPUT_FIR_Cnt>0)
//PPPU->PPUT_FIR_Cnt--;
if(PPPU->PPUT_FIR_SumCnt < 60000)
{
PPPU->PPUT_FIR_SumCnt++;
}
else
{
PPPU->PPUT_FIR_SumCnt = 0;
}
if((pOrb->CurOrbCtrlPcak_ID >= 0) &&(pOrb->CurOrbCtrlPcak_ID< 50))
{
PPPU->PPUT_FIR_Start[pOrb->CurOrbCtrlPcak_ID] = TempSatTime[0];
}
}/*PPU<50>ӵ<EFBFBD><D3B5><EFBFBD> <20><> δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>¼*/
else if((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
{
tmpConst->AttCmdFlashPara.PPUT_FIR_EndLast = TempSatTime[0] ;
pWMod->ATTFlashSwitch = POSE_OK;
if(((pOrb->CurOrbCtrlPcak_ID >= 1) &&(pOrb->CurOrbCtrlPcak_ID <= 50)))
{
PPPU->PPUT_FIR_End[pOrb->CurOrbCtrlPcak_ID - 1] = TempSatTime[0];
}
else if(0==pOrb->CurOrbCtrlPcak_ID)
{
if((PPPU->PPUT_FIR_CntS >= 1) &&(PPPU->PPUT_FIR_CntS <= 50))
{
PPPU->PPUT_FIR_End[PPPU->PPUT_FIR_CntS - 1] = TempSatTime[0];
PPPU->PPUT_FIR_CntS =0;
}
}
}
else
{;} /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>
***********************************************/
void SSABC_CC(void)
{
UINT8 i=0;
UINT8 tmpCheckCnt=0;
TYPE_CAL tmpLb[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
TYPE_CAL tmpLi[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
TYPE_CAL tmpRbs[3][9]={
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
};
TYPE_CAL tmpAab_b = 0.0;
TYPE_CAL tmpAbc_b = 0.0;
TYPE_CAL tmpAac_b = 0.0;
TYPE_CAL tmpAab_i = 0.0;
TYPE_CAL tmpAbc_i= 0.0;
TYPE_CAL tmpAac_i= 0.0;
TYPE_CAL tmpRsi[3][9]={
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
};
TYPE_CAL tmpRis[3][9]={
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
};
UINT8 Valid[3]={0};
UINT8 ssA_CCFlg=0;
UINT8 ssB_CCFlg=0;
UINT8 ssC_CCFlg=0;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
SsPara_t *ssA = NULL;
SsPara_t *ssB = NULL;
SsPara_t *ssC = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) ||( NULL==tmpConst))
{
return;
}
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
Valid[0]=ssA->ssIn_AttVld;
Valid[1]=ssB->ssIn_AttVld;
Valid[2]=ssC->ssIn_AttVld;
memcpy(tmpRbs[0], &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9 * sizeof(TYPE_CAL));
memcpy(tmpRbs[1], &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9 * sizeof(TYPE_CAL));
memcpy(tmpRbs[2], &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9 * sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
for(i=0;i<3;i++)
{
tmpLb[i][0]=tmpRbs[i][2];
tmpLb[i][1]=tmpRbs[i][5];
tmpLb[i][2]=tmpRbs[i][8];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8><EFBFBD>н<EFBFBD>
CalVecArc(tmpLb[0], tmpLb[1], &tmpAab_b);
CalVecArc(tmpLb[0], tmpLb[2], &tmpAac_b);
CalVecArc(tmpLb[1], tmpLb[2], &tmpAbc_b);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
Q2Att(ssA->ssIn_Q, tmpRsi[0]);
Q2Att(ssB->ssIn_Q, tmpRsi[1]);
Q2Att(ssC->ssIn_Q, tmpRsi[2]);
for(i=0;i<3;i++)
{
MatrixTransposeHL(tmpRsi[i], tmpRis[i],3, 3);
tmpLi[i][0]=tmpRis[i][2];
tmpLi[i][1]=tmpRis[i][5];
tmpLi[i][2]=tmpRis[i][8];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8><EFBFBD>н<EFBFBD>
CalVecArc(tmpLi[0], tmpLi[1], &tmpAab_i);
CalVecArc(tmpLi[0], tmpLi[2], &tmpAac_i);
CalVecArc(tmpLi[1], tmpLi[2], &tmpAbc_i);
if (0x55 == Valid[0])
{
ssA_CCFlg=CC_UNCERTAIN;
}
else
{
ssA_CCFlg=NO_CC;
}
if (0x55 == Valid[1])
{
ssB_CCFlg=CC_UNCERTAIN;
}
else
{
ssB_CCFlg=NO_CC;
}
if (0x55 == Valid[2])
{
ssC_CCFlg=CC_UNCERTAIN;
}
else
{
ssC_CCFlg=NO_CC;
}
if((0x55 == Valid[0])&&(0x55 == Valid[1]))
{
if(POSE_ABSF(tmpAab_i-tmpAab_b)<(tmpConst->SS_SS_CThr))
{
ssA_CCFlg=CC_CORRECT;
ssB_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
}
if((0x55 == Valid[0])&&(0x55 == Valid[2]))
{
if(POSE_ABSF(tmpAac_i-tmpAac_b)<(tmpConst->SS_SS_CThr))
{
ssA_CCFlg=CC_CORRECT;
ssC_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
}
if((0x55 == Valid[1])&&(0x55 == Valid[2]))
{
if(POSE_ABSF(tmpAbc_i-tmpAbc_b)<(tmpConst->SS_SS_CThr))
{
ssB_CCFlg=CC_CORRECT;
ssC_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
}
if(tmpCheckCnt>0)//<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
}
ssA->ssCrCheckFlg=ssA_CCFlg;
ssB->ssCrCheckFlg=ssB_CCFlg;
ssC->ssCrCheckFlg=ssC_CCFlg;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+̫<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>
***********************************************/
void StarSun_CC(void)
{
UINT8 i,tmpCheckCnt=0;
UINT8 ssA_CCFlg, ssB_CCFlg, ssC_CCFlg, Sun_CCFlg;
TYPE_CAL tmpRbi[3][9];
TYPE_CAL tmpSb[3][3];
TYPE_CAL tmpAam,tmpAbm,tmpAcm;
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
SsPara_t *ssA, *ssB, *ssC;
AssPara_t *pASS;
sAttOrbitGetPara_t *pOrbit = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
pASS=&tmpAtt->sPerPara.AssPara;
pOrbit = &tmpAtt->sOrbitPara;
//У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD>ò<EFBFBD><C3B2>ж<EFBFBD>Ҳ<EFBFBD>ɣ<EFBFBD>
if((CC_CORRECT == ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT == ssC->ssCrCheckFlg))
return;
if(0x00 == pASS->Ass_SunVecVld)
return;
if((0x00 == ssA->ssIn_AttVld)&&(0x00 == ssB->ssIn_AttVld)&&(0x00 == ssC->ssIn_AttVld))
return;
if(POSE_NO == pOrbit->MJCLv)
{
return;
}
ssA_CCFlg=CC_UNCERTAIN;
ssB_CCFlg=CC_UNCERTAIN;
ssC_CCFlg=CC_UNCERTAIN;
Sun_CCFlg=CC_UNCERTAIN;
//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>
Q2Att(ssA->ssExpose_Qbi, tmpRbi[0]);
Q2Att(ssB->ssExpose_Qbi, tmpRbi[1]);
Q2Att(ssC->ssExpose_Qbi, tmpRbi[2]);
for(i=0;i<3;i++)
MatrixProductHL(tmpRbi[i], tmpAtt->sEnvPara.SunVecI, tmpSb[i], 3, 3, 1);
//<2F><><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD>̫<EFBFBD><CCAB>̫<EFBFBD><CCAB>ʸ<EFBFBD><CAB8><EFBFBD>н<EFBFBD>
CalVecArc(tmpSb[0], pASS->Ass_SunVecb, &tmpAam);
CalVecArc(tmpSb[1], pASS->Ass_SunVecb, &tmpAbm);
CalVecArc(tmpSb[2], pASS->Ass_SunVecb, &tmpAcm);
if((0x55 == ssA->ssIn_AttVld)&&(tmpAam<tmpConst->SS_ASS_CCThr))
{
ssA_CCFlg=CC_CORRECT;
Sun_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
if((0x55 == ssB->ssIn_AttVld)&&(tmpAbm<tmpConst->SS_ASS_CCThr))
{
ssB_CCFlg=CC_CORRECT;
Sun_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
if((0x55 == ssC->ssIn_AttVld)&&(tmpAcm<tmpConst->SS_ASS_CCThr))
{
ssC_CCFlg=CC_CORRECT;
Sun_CCFlg=CC_CORRECT;
tmpCheckCnt++;
}
if(tmpCheckCnt>0)
{
if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
}
else
Sun_CCFlg=CC_UNCERTAIN;
ssA->ssCrCheckFlg=ssA_CCFlg;
ssB->ssCrCheckFlg=ssB_CCFlg;
ssC->ssCrCheckFlg=ssC_CCFlg;
pASS->AssCrCheckFlg=Sun_CCFlg;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У̫<EFBFBD><EFBFBD>
***********************************************/
void StarCheckSun(void)
{
TYPE_CAL tmpQbi[4];
TYPE_CAL tmpRbi[9];
TYPE_CAL tmpSb[3];
TYPE_CAL tmpAsm;
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
SsPara_t *ssA, *ssB, *ssC;
AssPara_t *pASS;
sAttOrbitGetPara_t *pOrbit = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
pASS=&tmpAtt->sPerPara.AssPara;
pOrbit = &tmpAtt->sOrbitPara;
if((0x00 == pASS->Ass_SunVecVld)||(POSE_NO == pOrbit->MJCLv))
{
pASS->AssCrCheckFlg=NO_CC;
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>
if(CC_CORRECT == ssA->ssCrCheckFlg)
{
memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else if(CC_CORRECT == ssB->ssCrCheckFlg)
{
memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else if(CC_CORRECT == ssC->ssCrCheckFlg)
{
memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else
{
return;
}
Q2Att(tmpQbi, tmpRbi);
MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, tmpSb, 3, 3, 1);
//<2F>жϼн<CFBC>
CalVecArc(tmpSb, pASS->Ass_SunVecb, &tmpAsm);
if(tmpAsm<tmpConst->SS_ASS_CThr)
pASS->AssCrCheckFlg=CC_CORRECT;
else
pASS->AssCrCheckFlg=CC_ERROR;
}
/***********************************************
˵<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>DZ<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>Ϊ1<EFBFBD>ҽ<EFBFBD><EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>ٽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
***********************************************/
void StarCheckGyro(void)
{
UINT8 i, j;
TYPE_CAL deltaA[GYRO_NUM][3];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
SsPara_t *ssA, *ssB, *ssC;
GyroPara_t *pGyro[3];
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
pGyro[0]=&tmpAtt->sPerPara.GyroPara[0];
pGyro[1]=&tmpAtt->sPerPara.GyroPara[1];
pGyro[2]=&tmpAtt->sPerPara.GyroPara[2];
//У<><D0A3><EFBFBD><EFBFBD>
if(CC_CORRECT ==ssA->ssCrCheckFlg)
{
for(i=0;i<GYRO_NUM;i++)
{
for (j=0;j<3;j++)
{
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssA->ssExpose_Wbi[j]);
}
}
}
else if(CC_CORRECT ==ssB->ssCrCheckFlg)
{
for(i=0;i<GYRO_NUM;i++)
{
for (j=0;j<3;j++)
{
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssB->ssExpose_Wbi[j]);
}
}
}
else if(CC_CORRECT ==ssC->ssCrCheckFlg)
{
for(i=0;i<GYRO_NUM;i++)
{
for (j=0;j<3;j++)
{
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssC->ssExpose_Wbi[j]);
}
}
}
else
{
return;
}
for(i=0;i<GYRO_NUM; i++)
{
if (POSE_NO == pGyro[i]->GyroIn_AttVld)
pGyro[i]->GyroCrCheckFlg=NO_CC;
else if((deltaA[i][0]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][1]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][2]<tmpConst->SS_GYRO_CThr))
pGyro[i]->GyroCrCheckFlg=CC_CORRECT;
else
pGyro[i]->GyroCrCheckFlg=CC_ERROR;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD>
***********************************************/
void StarCheckMag(void)
{
UINT8 i;
TYPE_CAL deltaA[MAG_NUM];
TYPE_CAL tmpQbi[4];
TYPE_CAL tmpRbi[9];
TYPE_CAL tmpSb[3];
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
SsPara_t *ssA, *ssB, *ssC;
MagPara_t *pMag;
sAttEnvPara_t *pEnv;
sAttOrbitGetPara_t *pObtIn;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
pObtIn = &tmpAtt->sOrbitPara;
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
pMag=&tmpAtt->sPerPara.MagPara;
pEnv=&tmpAtt->sEnvPara;
if(0x00 == pObtIn->OrbitVld)
{
pMag->MagCrCheckFlg[0]=NO_CC;
pMag->MagCrCheckFlg[1]=NO_CC;
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㱾<EFBFBD><E3B1BE>ϵ̫<CFB5><CCAB>ʸ<EFBFBD><CAB8>
if(CC_CORRECT == ssA->ssCrCheckFlg)
{
memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else if(CC_CORRECT == ssB->ssCrCheckFlg)
{
memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else if(CC_CORRECT == ssC->ssCrCheckFlg)
{
memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
}
else
{
return;
}
Q2Att(tmpQbi, tmpRbi);
MatrixProductHL(tmpRbi, pEnv->fvBi, tmpSb, 3, 3, 1);
//У<><D0A3>ǿ<EFBFBD><C7BF>
for(i=0;i<MAG_NUM;i++)
{
if(POSE_OK == pMag->MagIn_Valid[i])
{
CalVecArc(tmpSb, pMag->Mag_Bb_MeaAB[i], &deltaA[i]);
if(deltaA[i]<tmpConst->AttCmdFlashPara.SS_MAG_CThr)
{
pMag->MagCrCheckFlg[i]=CC_CORRECT;
}
else
{
pMag->MagCrCheckFlg[i]=CC_ERROR;
}
}
else
{
pMag->MagCrCheckFlg[i]=NO_CC;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݻ<EFBFBD>У<EFBFBD><EFBFBD>
***********************************************/
void Gyro_CC(void)
{
UINT8 i=0;
UINT8 Valid[3];
UINT8 tmpCnt = 0;
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
GyroPara_t *pGyro[3];
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];
for(i=0;i<GYRO_NUM;i++)
{
pGyro[i]->GyroCrCheckFlg=NO_CC;
Valid[i]=pGyro[i]->GyroIn_AttVld;
if (0x55 == Valid[i])
{
pGyro[i]->GyroCrCheckFlg=CC_UNCERTAIN;
}
}
if((0x55 == Valid[0])&&(0x55 ==Valid[1]))
{
if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[1]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[1]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[1]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
)
{
pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
tmpCnt++;
}
else
{
;/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
}
if((0x55 == Valid[0])&&(0x55 == Valid[2]))
{
if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
)
{
pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
tmpCnt++;
}
else
{
;/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
}
if((0x55 == Valid[1])&&(0x55 == Valid[2]))
{
if((POSE_ABSF(pGyro[1]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[1]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
&&(POSE_ABSF(pGyro[1]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
)
{
pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
tmpCnt++;
}
else
{
;/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
}
for(i=0; i<3; i++)
{
if((NO_CC == pGyro[i]->GyroCrCheckFlg)
||(CC_CORRECT == pGyro[i]->GyroCrCheckFlg)
)
continue;
if(tmpCnt > 0)
pGyro[i]->GyroCrCheckFlg = CC_ERROR;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǿ<EFBFBD>ƻ<EFBFBD>У<EFBFBD><EFBFBD>
***********************************************/
void Mag_CC(void)
{
UINT8 i;
TYPE_CAL deltaA[3];
sAttPriData *tmpAtt;
AttCtrlConst_t *tmpConst;
MagPara_t *pMag;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL==tmpAtt || NULL==tmpConst)
return;
pMag=&tmpAtt->sPerPara.MagPara;
if((POSE_OK == pMag->MagIn_Valid[0])&&(POSE_OK ==pMag->MagIn_Valid[1]))
{
for(i=0;i<3;i++)
{
deltaA[i]=POSE_ABSF(pMag->Mag_Bb_MeaAB[0][i]-pMag->Mag_Bb_MeaAB[1][i]);
}
if((deltaA[0]<tmpConst->MAG_MAG_CThr)&&(deltaA[1]<tmpConst->MAG_MAG_CThr)&&(deltaA[2]<tmpConst->MAG_MAG_CThr))
{
pMag->MagCrCheckFlg[0]=CC_CORRECT;
pMag->MagCrCheckFlg[1]=CC_CORRECT;
}
else
{
pMag->MagCrCheckFlg[0]=CC_UNCERTAIN;
pMag->MagCrCheckFlg[1]=CC_UNCERTAIN;
}
}
else
{
pMag->MagCrCheckFlg[0]=NO_CC;
pMag->MagCrCheckFlg[1]=NO_CC;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD>
***********************************************/
void Att_CrossCheck(void)
{
UINT8 tmpValidSum;
sAttPriData *tmpAtt;
SsPara_t *ssA, *ssB, *ssC;
AssPara_t *pASS;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL ==tmpAtt)
return;
ssA=&tmpAtt->sPerPara.SsPara[0];
ssB=&tmpAtt->sPerPara.SsPara[1];
ssC=&tmpAtt->sPerPara.SsPara[2];
pASS=&tmpAtt->sPerPara.AssPara;
tmpValidSum=ssA->ssIn_AttVld+ssB->ssIn_AttVld+ssC->ssIn_AttVld;
if((0xAA ==tmpValidSum)||(0xFF ==tmpValidSum))//2<><32>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
{
SSABC_CC();//2<><32>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>以У<E4BBA5><D0A3>
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
StarCheckSun();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У̫<D0A3><CCAB>
StarCheckGyro();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
StarCheckMag();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>ǿ<EFBFBD><C7BF>
}
else
{
if(0x55 == pASS->Ass_SunVecVld)//̫<><CCAB><EFBFBD><EFBFBD>Ч
{
StarSun_CC();//<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
StarCheckGyro();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
StarCheckMag();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>ǿ<EFBFBD><C7BF>
}
else
{
Gyro_CC();
Mag_CC();
}
}
else
{
Gyro_CC();
Mag_CC();
}
}
}
else if(0x55 ==tmpValidSum)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
{
if(0x55 ==pASS->Ass_SunVecVld)//̫<><CCAB><EFBFBD><EFBFBD>Ч
{
StarSun_CC();//<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
StarCheckGyro();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
StarCheckMag();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>ǿ<EFBFBD><C7BF>
}
else
{
Gyro_CC();
Mag_CC();
}
}
else
{
Gyro_CC();
Mag_CC();
}
}
else if(0 ==tmpValidSum)//û<><C3BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
{
Gyro_CC();
Mag_CC();
}
}
/************************************************
˵<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 ZKDPAction(void)
{
AttCtrlConst_t *tmpConst;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL == tmpConst)
return;
//<2F><>ʼ<EFBFBD><CABC>
ZKDPInit();
//<2F><><EFBFBD><EFBFBD>
Ss_DataGet();
Ss_DataPre();
//<2F><><EFBFBD><EFBFBD>
Gyro_DataGet();
Gyro_DataPre();
//ģ<><C4A3>̫<EFBFBD><CCAB>
Ass_DataGet();
Ass_DataPre();
//<2F><>ǿ<EFBFBD><C7BF>
Mag_DataGet();
Mag_DataPre();
//<2F><><EFBFBD><EFBFBD>
Whl_DataGet();
Whl_DataPre();
#ifndef MINMODULE_TYPE
//<2F><><EFBFBD><EFBFBD>
PPU_DataPre();
//<2F><>У<EFBFBD><D0A3>
if (tmpConst->Allow_Sensor_CThr == 0x55 )
{
Att_CrossCheck();
}
#endif
}
Reference in New Issue Copy Permalink