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/AttRECheck.c

2062 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"
/***********************************************
˵<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 ZKREInit(void)
{
UINT8 i =0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
AttIMPTPara_t *pIMU =NULL;
sAttErrCtlPara_t *pREcheck =NULL;
//WhlPara_t *PWhl;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pIMU=&tmpAtt->sIMPTPara;
pREcheck=&tmpAtt->sErrCtlPara;
//PWhl = &tmpAtt->sPerPara.WhlPara;
//<2F><><EFBFBD><EFBFBD>
for(i=0;i<GYRO_NUM;i++)
{
pREcheck->GyroIn_PowerOn[i]=POSE_OK;
pREcheck->Gyro_PowerOnCnt[i]=0;
//tmpConst->AllowGyroDiagFlg[i]=POSE_OK;
//tmpConst->AllowGyroDiagFlg[i]=0;
tmpConst->GyroDiagThr = 100;
pREcheck->GyroErrCnt[i] =0;
pREcheck->Gyro_Err_Cnt1[i] =0;
pREcheck->Gyro_Err_Cnt2[i] =0;
pREcheck->Gyro_Err_Cnt3[i] =0;
pREcheck->Gyro_Diag[i] =POSE_OK;
pREcheck->Gyro_err[i] =POSE_OK;
tmpConst->AllowGyroONOFF[i] = POSE_OK;//POSE_NO; 20240630
pIMU->GyroUseState[i] = POSE_OK; //ʹ<><CAB9>״̬<D7B4><CCAC>0
//tmpConst->GyroUsePrior[i] = PriorHig;
//tmpConst->Gyro_ComRESW[i]=0;
}
tmpConst->GyroUsePrior[0] = PriorHig;
tmpConst->GyroUsePrior[1] = PriorMid;
tmpConst->GyroUsePrior[2] = PriorLow;
tmpConst->AttCmdFlashPara.Gyro_ComRESW[0]=0;
tmpConst->AttCmdFlashPara.Gyro_ComRESW[1]=0;
tmpConst->AttCmdFlashPara.Gyro_ComRESW[2]=0;
tmpConst->Gyro_Delay_CntLimt =6; //<2F><><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_Err_CntLimt =8; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_OK_CntLimt =8; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_ReSet_CntLimt =5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Gyro_ReSetGAP_CntLimt =10; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>
for(i=0;i<STAR_NUM;i++)
{
pREcheck->ssIn_PowerOn[i]=POSE_OK;
pREcheck->ss_PowerOnCnt[i]=0;
//tmpConst->AllowssDiagFlg[i]=POSE_OK;
tmpConst->AllowssONOFF[i]= POSE_OK;//POSE_NO;20240630
tmpConst->ssDiagThr = 1200;
pREcheck->ssErrCnt[i] =0;
pREcheck->ss_Err_Cnt1[i] =0;
pREcheck->ss_Err_Cnt2[i] =0;
pREcheck->ss_Err_Cnt3[i] =0;
pREcheck->ss_Diag[i] =POSE_OK;
pREcheck->SS_err[i] =POSE_OK;
pIMU->SSUseState[i] = POSE_OK; //ʹ<><CAB9>״̬<D7B4><CCAC>0
}
tmpConst->SSUsePrior[0] = PriorHig;
tmpConst->SSUsePrior[1] = PriorMid;
tmpConst->SSUsePrior[2] = PriorLow;
//tmpConst->Ss_DiagCnt_Limt=10;
tmpConst->Ss_Delay_CntLimt =12; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
//tmpConst->Ss_Err_CntLimt =14; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_Err_CntLimt =30; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_OK_CntLimt =8; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_ReSet_CntLimt =5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Ss_ReSetGAP_CntLimt =10; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ǿ<EFBFBD><C7BF>
for(i=0;i<MAG_NUM;i++)
{
pREcheck->MagIn_PowerOn[i]=POSE_OK;
pREcheck->Mag_PowerOnCnt[i] =0; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD>Ƽӵ<C6BC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
pREcheck->MagErrCnt[i]=0; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>,<2C><><EFBFBD><EFBFBD>60000
pREcheck->MagNoErrCnt[i]=0;
pREcheck->Mag_Err_Cnt1[i]=0; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
pREcheck->Mag_Err_Cnt2[i]=0; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->Mag_Err_Cnt3[i]=0; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->Mag_Diag[i]=POSE_OK; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD>ƹ<EFBFBD><C6B9>ϱ<EFBFBD>־
pREcheck->Mag_err[i]=POSE_OK;
//tmpConst->AllowMagDiagFlg[i]=POSE_OK; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>0x55<35><35><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>Ϊ0x55
tmpConst->AllowMagONOFF[i]= POSE_OK;//POSE_NO; 20240630 //<2F><>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>0x55<35><35><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>Ϊ0x55
}
tmpConst->MagDiagThr= 100; //<2F>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ,<2C><>λs,<2C><><EFBFBD><EFBFBD>ע
tmpConst->Mag_Delay_CntLimt =12; //<2F><>ǿ<EFBFBD>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_Err_CntLimt =14; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_OK_CntLimt =8; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_ReSet_CntLimt = 5; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Mag_ReSetGAP_CntLimt =10; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>
for(i=0;i<WHEEL_NUM;i++)
{
pREcheck->WhlIn_PowerOn[i]=POSE_OK;
pREcheck->Whl_PowerOnCnt[i]=0;
//tmpConst->AllowWhlDiagFlg[i] = POSE_OK;
tmpConst->AllowWhlONOFF[i] = POSE_OK;//POSE_NO;
//tmpConst->AllowWhlONOFF = 0;
pREcheck->WhlOutErrCnt[i]= 0;
pREcheck->WhlTemErrCnt[i]= 0;
pREcheck->Whl_Err_Cnt1[i]= 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
pREcheck->Whl_Err_Cnt2[i]= 0; //<2F><><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->Whl_Err_Cnt3[i]= 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->Whl_Err[i]= POSE_OK;
pREcheck->Whl_Diag[i]=POSE_OK;
}
tmpConst->WhlTemperDiagThr = 90;
tmpConst->WhlDiag_TimeThr = 10;
tmpConst->WhlDiag_ErrThr=15;
tmpConst->Whl_Delay_CntLimt =6; //<2F><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
//tmpConst->Whl_Err_CntLimt= 8; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_Err_CntLimt= 16; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
//tmpConst->Whl_OK_CntLimt= 8; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_OK_CntLimt= 16; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_ReSet_CntLimt=5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Whl_ReSetGAP_CntLimt=16; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//PPU
pREcheck->PPU_Err=0;
//pREcheck->PPUTXT_Err=POSE_OK;
pREcheck->PPU_Diag=POSE_OK; //PPU<50><55><EFBFBD>ϱ<EFBFBD>־
pREcheck->PPUOutErrCnt=0; //PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->PPUIn_ACurCnt=0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->PPUIn_VCurCnt=0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>
pREcheck->PPUOUT_FIRECnt=0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->PPUOUT_TemperECnt=0; //<2F><>ƿ<EFBFBD><C6BF>·<EFBFBD><C2B7>·<EFBFBD><C2B7><EFBFBD>µ<EFBFBD><C2B5><EFBFBD>
pREcheck->PPU_Err_Cnt1=0; //PPU<50><55><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
pREcheck->PPU_Err_Cnt2=0; //PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pREcheck->PPU_Err_Cnt3=0; //PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//PPU
//tmpConst->AllowPPUDiagFlg= POSE_OK; //PPU<50>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>0x55<35><35><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>Ϊ0x55
//tmpConst->AllowPPUDiagFlg= 0; //PPU<50>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>0x55<35><35><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>Ϊ0x55
tmpConst->AllowPPUONOFF= POSE_OK;//POSE_NO; 20240630 //PPU<50>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>0x55<35><35><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>Ϊ0x00
//tmpConst->SafePPUONOFF=POSE_OK;
tmpConst->PPU_Delay_CntLimt =6; //<2F><>ǿ<EFBFBD>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_Err_CntLimt =8; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_OK_CntLimt =8; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_ReSet_CntLimt =5; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->PPU_ReSetGAP_CntLimt=10; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpConst->PPUTXTimeLimit=15; //<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD>Ź<EFBFBD><C5B9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ15s
tmpConst->PPUTATimeLimit=30; //<2F><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ30s
tmpConst->AttCmdFlashPara.PPUTALimit=22; //<2F><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ2.2
tmpConst->PPUTVTimeLimit=30; //<2F><><EFBFBD>ƹ<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ30s
tmpConst->AttCmdFlashPara.PPUTVLimit=260; //<2F><><EFBFBD>ƹ<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ260
tmpConst->AttCmdFlashPara.PPUTFireLimit=420; //<2F><><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ420S
tmpConst->PPUTemperTimeLimit=5; //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ 5s
tmpConst->AttCmdFlashPara.PPUHTemperLimit=30; //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ30
//tmpConst->PPULTemperLimit=10; //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ10
tmpConst->AttCmdFlashPara.PPULTemperLimit=5; //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ10
////<2F><>̬<EFBFBD><CCAC>׼<EFBFBD><D7BC>ʧ
tmpConst->AllowAttLoseDiagFlg = 0x55;//0x00;20240630
tmpConst->TimeThr_AttLoseDiag[0] =30; //30s
tmpConst->TimeThr_AttLoseDiag[1] = 60*60; //60min
tmpConst->TimeThr_AttLoseDiag[2] = 60*60; //60min
tmpConst->TimeThr_AttLoseDiag[3] = 30; //30s
tmpConst->TimeThr_AttLoseDiag[4] = 30; //30s
tmpConst->TimeThr_AttLoseDiag[5] = 30; //30s
tmpConst->TimeThr_AttLoseDiag[6] = 10; //10s
tmpConst->TimeThr_AttLoseDiag[7] = 30; //30s
tmpConst->TimeThr_AttLoseDiag[8] = 30; //30s
pREcheck->AttLoseCnt=0;
//<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
tmpConst->AllowAttExDiagFlg= 0x55;//0x00; 20240630 //<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־,Ĭ<><C4AC>Ϊ0x55
pREcheck->AttExFlg[0]=0x00;
pREcheck->AttExFlg[1]=0x00;
pREcheck->AttExFlg[2]=0x00;
//tmpConst->TimeThr_AttEx[0]=30; //30s
tmpConst->TimeThr_AttEx[0]=900; //900s
tmpConst->TimeThr_AttEx[1]=30; //30s
tmpConst->TimeThr_AttEx[2]=500; //500s
tmpConst->TimeThr_AttEx[3]=30; //30s
//tmpConst->TimeThr_AttEx[4]=30; //30s
tmpConst->TimeThr_AttEx[4]=2400; //2400s
tmpConst->TimeThr_AttEx[5]=30; //30s
tmpConst->TimeThr_AttEx[6]=20; //20s
tmpConst->TimeThr_AttEx[7]=30; //30s
tmpConst->TimeThr_AttEx[8]=30; //30s
// tmpConst->YawCntLimit=1200; //<2F>Ե<EFBFBD>ƫ<EFBFBD><C6AB>/<2F><>Ŀ<EFBFBD><C4BF>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpConst->YawCntLimit=2400; //<2F>Ե<EFBFBD>ƫ<EFBFBD><C6AB>/<2F><>Ŀ<EFBFBD><C4BF>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpConst-> AngThr_AttEx[0] =0; //<2F><><EFBFBD>ж<EFBFBD>
tmpConst-> AngThr_AttEx[1] =0; //<2F><><EFBFBD>ж<EFBFBD>
tmpConst-> AngThr_AttEx[2] = 0*ANG2RADIAN; //30<33><30>
tmpConst-> AngThr_AttEx[3] = 0*ANG2RADIAN; //30<33><30>
tmpConst-> AngThr_AttEx[4] = 10*ANG2RADIAN; //10<31><30>
tmpConst-> AngThr_AttEx[5] = 0*ANG2RADIAN; //10<31><30>
tmpConst-> AngThr_AttEx[6] = 10*ANG2RADIAN; //10<31><30>
tmpConst-> AngThr_AttEx[7] = 0*ANG2RADIAN; //10<31><30>
tmpConst-> AngThr_AttEx[8] = 10*ANG2RADIAN; //10<31><30>
tmpConst->RateThr_AttEx[0]=-0.002f*ANG2RADIAN; //-0.002<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttEx[1]=1*ANG2RADIAN; //-0.002<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttEx[2]=4*ANG2RADIAN; //1<><31>/s
tmpConst->RateThr_AttEx[3]=1*ANG2RADIAN; //1<><31>/s
tmpConst->RateThr_AttEx[4]=0.5f*ANG2RADIAN; //0.5<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttEx[5]=1*ANG2RADIAN; //0.5<EFBFBD><EFBFBD>/s
tmpConst-> RateThr_AttEx[6] = 0.5f*ANG2RADIAN; //10<31><30>
tmpConst-> RateThr_AttEx[7] = 1*ANG2RADIAN; //10<31><30>
tmpConst-> RateThr_AttEx[8] = 0.5f*ANG2RADIAN; //10<31><30>
memset(pREcheck->AngRateMod_RATEDMP,0,61*sizeof(TYPE_CAL));
pREcheck->YawCnt=0;
pREcheck->AttExCnt=0; //<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
////<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
tmpConst->AllowAttStaDiagFlg = 0x55;//0x00; 20240630
pREcheck->AttUnSta[0] = 0x00;
pREcheck->AttUnSta[1] = 0x00;
pREcheck->AttUnSta[2] = 0x00;
tmpConst->TimeThr_AttUnSta[0] = 10000; //10000s
//tmpConst->TimeThr_AttUnSta[1] = 1200; //1200s
tmpConst->TimeThr_AttUnSta[1] = 1800; //1800s
tmpConst->TimeThr_AttUnSta[2] = 15000; //15000s
tmpConst->TimeThr_AttUnSta[3] = 3600; //3600s
tmpConst->TimeThr_AttUnSta[4] = 10; //10s
tmpConst->TimeThr_AttUnSta[5] = 10; //10s
tmpConst->TimeMod_AttUnSta[0] = 0; //0
tmpConst->TimeMod_AttUnSta[1] = 0; //0
tmpConst->TimeMod_AttUnSta[2] = 0; //0
tmpConst->TimeMod_AttUnSta[3] = 0; //0
//tmpConst->TimeMod_AttUnSta[4] = 1500; //1500s
tmpConst->TimeMod_AttUnSta[4] = 1800; //1800s
tmpConst->TimeMod_AttUnSta[5] = 1500; //1500s
tmpConst->AngThr_AttUnSta[0] = 0*ANG2RADIAN; //20<32><30>
tmpConst->AngThr_AttUnSta[1] = 20.0*ANG2RADIAN; //20<32><30>
tmpConst->AngThr_AttUnSta[2] = 30.0*ANG2RADIAN; //30<33><30>
tmpConst->AngThr_AttUnSta[3] = 0*ANG2RADIAN; //20<32><30>
tmpConst->AngThr_AttUnSta[4] = 2.0*ANG2RADIAN; //20<32><30>
tmpConst->AngThr_AttUnSta[5] = 2.0*ANG2RADIAN; //20<32><30>
tmpConst->RateThr_AttUnSta[0] = 0*ANG2RADIAN; //1<><31>/s
tmpConst->RateThr_AttUnSta[1] = 1.0*ANG2RADIAN; //1<><31>/s
tmpConst->RateThr_AttUnSta[2] = 1.0*ANG2RADIAN; //0.1<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttUnSta[3] = 0*ANG2RADIAN; //0.1<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttUnSta[4] = 0.5*ANG2RADIAN; //0.5<EFBFBD><EFBFBD>/s
tmpConst->RateThr_AttUnSta[5] = 0.1*ANG2RADIAN; //0.1<EFBFBD><EFBFBD>/s
pREcheck->AttUnStaCnt=0;
pREcheck->AttModCnt=0;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\<EFBFBD>ϵ紦<EFBFBD><EFBFBD>
Chl :<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Cnt1:<EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Cnt2:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Cnt3:<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
err : <EFBFBD>쳣<EFBFBD><EFBFBD>
state :<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ImuPowerOnOff( UINT8 ChlON,UINT8 ChlOFF, UINT8 ONOFF,
UINT32 *Cnt1 , UINT32 *Cnt2, UINT32 *Cnt3,
UINT32 Cnt1Limt , UINT32 Cnt2Limt , UINT32 Cnt3Limt ,UINT32 Cnt4Limt ,
UINT8 err, UINT8 *state)
{
UINT8 PulT = 16; //160MS
UINT8 Type = 0;
if (POSE_NO == *state)
{
//state=POSE_NO;
*Cnt1 = 0;
//*Cnt2 = 0;
*Cnt3 = 0;
return;
}
if (POSE_NO == err)
{
if(*Cnt1<60000)
{(*Cnt1) ++;}
*Cnt3 = 0;
}
else
{
if(*Cnt3<60000)
{(*Cnt3)++;}
*Cnt1=0;
//Cnt2 = 0;
}
if ((*Cnt1) >= Cnt1Limt )
{
if (*Cnt2 >= Cnt2Limt)
{
*state = POSE_NO;
*Cnt1 = 0;
//*Cnt2 = 0;
*Cnt3 = 0;
return;
}
else
{
if ((*Cnt1 >= (Cnt1Limt + Cnt4Limt) ) && (*Cnt1 < (Cnt1Limt + Cnt4Limt +1) ))
{
*Cnt1 = 0;
*Cnt3 = 0;
if(*Cnt2<60000)
{(*Cnt2) ++;}
if (*Cnt2 >= Cnt2Limt)
{
if ( POWER_OFF == ONOFF)
{
//<2F><><EFBFBD>ڹص<DAB9><D8B5><EFBFBD>
OCOut(ChlOFF, Type, PulT);
}
else if (POWER_ON == ONOFF)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
OCOut(ChlON, Type, PulT);
}
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
OCOut(ChlON, Type, PulT);
}
}
}
if ((*Cnt1 < (Cnt1Limt +1) )&&(*Cnt1 >= Cnt1Limt ))
{
//<2F>ص<EFBFBD><D8B5><EFBFBD>
OCOut(ChlOFF, Type, PulT);
*state = POSE_IN;
}
}
else
{
if (*Cnt3 >= Cnt3Limt)
{
*state = POSE_OK;
*Cnt2 = 0; //zmx20231011
}
else
{
*state = POSE_IN;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void GyroImuErrJudge(void)
{
UINT8 i =0;
UINT8 ONOFF = POWER_ON;
UINT8 ChlON[GYRO_NUM] = {OC66_GYROA_ON,OC69_GYROB_ON,OC71_GYROC_ON};
UINT8 ChlOFF[GYRO_NUM] = {OC67_GYROA_OFF,OC70_GYROB_OFF,OC72_GYROC_OFF};
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
GyroPara_t *PGyro[GYRO_NUM];
AttIMPTPara_t *pIMU =NULL;
sAttErrCtlPara_t *pREcheck =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];
pIMU=&tmpAtt->sIMPTPara;
pREcheck=&tmpAtt->sErrCtlPara;
for(i=0;i<GYRO_NUM;i++)
{
if (pREcheck->Gyro_PowerOnCnt[i] >= tmpConst->Gyro_Delay_CntLimt)
{
if((POSE_NO == PGyro[i]->GyroIn_AttVld)||(CC_ERROR == PGyro[i]->GyroCrCheckFlg))
{
if(pREcheck->GyroErrCnt[i]<60000)
{pREcheck->GyroErrCnt[i]++;}
}
else
{
pREcheck->GyroErrCnt[i]=0;
}
if(pREcheck->GyroErrCnt[i] >= tmpConst->GyroDiagThr*2)
{
pREcheck->Gyro_err[i] = POSE_NO;
}
else
{
pREcheck->Gyro_err[i] = POSE_OK;
}
}
if (POSE_OK == tmpConst->AllowGyroONOFF[i])
{
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD>Ӷϵ<D3B6>
ImuPowerOnOff( ChlON[i], ChlOFF[i], ONOFF,
&pREcheck->Gyro_Err_Cnt1[i] , &pREcheck->Gyro_Err_Cnt2[i], &pREcheck->Gyro_Err_Cnt3[i],
tmpConst->Gyro_Err_CntLimt , tmpConst->Gyro_ReSet_CntLimt , tmpConst->Gyro_OK_CntLimt , tmpConst->Gyro_ReSetGAP_CntLimt ,
pREcheck->Gyro_err[i], &pREcheck->Gyro_Diag[i] );
if (POSE_NO == pREcheck->Gyro_Diag[i])
{
;//pIMU->GyroUseState[i] = POSE_NO;//ʹ<><CAB9>״̬<D7B4><CCAC>0
}
}
else
{
pREcheck->Gyro_Err_Cnt1[i] = 0;
pREcheck->Gyro_Err_Cnt2[i] = 0;
pREcheck->Gyro_Err_Cnt3[i] = 0;
pREcheck->Gyro_Diag[i] =POSE_OK;
pREcheck->Gyro_err[i] = POSE_OK;
pREcheck->GyroErrCnt[i]=0;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void SsImuErrJudge(void)
{
UINT8 i =0;
UINT8 ONOFF = POWER_ON;
UINT8 ChlON = 0;
UINT8 ChlOFF = 0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
SsPara_t *Pss[STAR_NUM];
AttIMPTPara_t *pIMU;
sAttErrCtlPara_t *pREcheck;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
Pss[0] = &tmpAtt->sPerPara.SsPara[0];
Pss[1] = &tmpAtt->sPerPara.SsPara[1];
Pss[2] = &tmpAtt->sPerPara.SsPara[2];
pIMU=&tmpAtt->sIMPTPara;
pREcheck=&tmpAtt->sErrCtlPara;
for(i=0;i<STAR_NUM;i++)
{
if (pREcheck->ss_PowerOnCnt[i] >= tmpConst->Ss_Delay_CntLimt)
{
if((POSE_NO == Pss[i]->ssIn_AttVld)||(CC_ERROR == Pss[i]->ssCrCheckFlg))
{
if(pREcheck->ssErrCnt[i]<60000)
{pREcheck->ssErrCnt[i]++;}
}
else
{
pREcheck->ssErrCnt[i]=0;
}
if(pREcheck->ssErrCnt[i] >= tmpConst->ssDiagThr*2)
{
pREcheck->SS_err[i] = POSE_NO;
}
else
{
pREcheck->SS_err[i] = POSE_OK;
}
}
if (POSE_OK == tmpConst->AllowssONOFF[i])
{
ChlON = OC74_STA_ON + i*2 ;
ChlOFF = OC74_STA_ON + i*2 + 1;
if (i == 2)
{
ChlOFF = OC83_STC_OFF;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷϵ<D3B6>
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
&pREcheck->ss_Err_Cnt1[i] , &pREcheck->ss_Err_Cnt2[i], &pREcheck->ss_Err_Cnt3[i],
tmpConst->Ss_Err_CntLimt , tmpConst->Ss_ReSet_CntLimt , tmpConst->Ss_OK_CntLimt , tmpConst->Ss_ReSetGAP_CntLimt ,
pREcheck->SS_err[i], &pREcheck->ss_Diag[i] );
if ((POSE_NO == pREcheck->ss_Diag[i]) && (POSE_OK == tmpConst->AllowssONOFF[i]))
{
;//pIMU->SSUseState[i] = POSE_NO;//ʹ<><CAB9>״̬<D7B4><CCAC>0
}
}
else
{
pREcheck->ss_Err_Cnt1[i] = 0;
pREcheck->ss_Err_Cnt2[i] = 0;
pREcheck->ss_Err_Cnt3[i] = 0;
pREcheck->ss_Diag[i] =POSE_OK;
pREcheck->ssErrCnt[i]=0;
pREcheck->SS_err[i] = POSE_OK;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǿ<EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void MagImuErrJudge(void)
{
UINT8 i =0;
UINT8 ONOFF = POWER_ON;
UINT8 ChlON = 0;
UINT8 ChlOFF = 0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
MagPara_t *PMag =NULL;
AttIMPTPara_t *pIMU =NULL;
sAttErrCtlPara_t *pREcheck =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;
pREcheck=&tmpAtt->sErrCtlPara;
for(i=0;i<MAG_NUM;i++)
{
if (pREcheck->Mag_PowerOnCnt[i] >= tmpConst->Mag_Delay_CntLimt)
{
if((POSE_NO == PMag->MagIn_Valid[i])||(CC_ERROR == PMag->MagCrCheckFlg[i]))
{
if(pREcheck->MagErrCnt[i]<60000)
{pREcheck->MagErrCnt[i]++;}
pREcheck->MagNoErrCnt[i]=0;
}
else
{
if(pREcheck->MagNoErrCnt[i]<60000)
{pREcheck->MagNoErrCnt[i]++;}
pREcheck->MagErrCnt[i]=0;
}
if(pREcheck->MagErrCnt[i] >= tmpConst->MagDiagThr*2)
{
pREcheck->Mag_err[i] = POSE_NO;
}
else if (pREcheck->MagNoErrCnt[i] > 2)
{
pREcheck->Mag_err[i] = POSE_OK;
}
}
if (POSE_OK == tmpConst->AllowMagONOFF[i])
{
ChlON = OC94_CQJA_ON + i*2 ;
ChlOFF = OC94_CQJA_ON + i*2 + 1;
//<2F><>ǿ<EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD>Ӷϵ<D3B6>
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
&pREcheck->Mag_Err_Cnt1[i] , &pREcheck->Mag_Err_Cnt2[i], &pREcheck->Mag_Err_Cnt3[i],
tmpConst->Mag_Err_CntLimt , tmpConst->Mag_ReSet_CntLimt , tmpConst->Mag_OK_CntLimt , tmpConst->Mag_ReSetGAP_CntLimt ,
pREcheck->Mag_err[i], &pREcheck->Mag_Diag[i] );
if ((POSE_NO == pREcheck->Mag_Diag[i]) && (POSE_OK == tmpConst->AllowMagONOFF[i]))
{
;//pIMU->MagUseState[i] = POSE_NO;
}
}
else
{
pREcheck->Mag_Err_Cnt2[i] = 0;
pREcheck->Mag_Diag[i] =POSE_OK;
pREcheck->Mag_Err_Cnt1[i]= 0;
pREcheck->Mag_Err_Cnt2[i]= 0;
pREcheck->Mag_Err_Cnt3[i]= 0;
pREcheck->MagErrCnt[i]=0;
pREcheck->Mag_err[i] = POSE_OK;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void WhlImuErrJudge(void)
{
UINT8 i = 0;
UINT8 ChlON = 0;
UINT8 ChlOFF = 0;
UINT8 ONOFF = POWER_OFF;
UINT32 tmp_cnt = 0;
UINT8 cmdDataBuff[50] = {0};
sTaskInfo* tmpTaskInfo = NULL;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttErrCtlPara_t *pREcheck =NULL;
WhlPara_t *PWhl =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;}
pREcheck=&tmpAtt->sErrCtlPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
pImpt=&tmpAtt->sIMPTPara;
pWMod =&tmpAtt->sModePara;
for (i = 0; i < WHEEL_NUM; ++i)
{
if (POSE_OK != tmpConst->AllowWhlONOFF[i])
{
pREcheck->Whl_Err_Cnt1[i] = 0;
pREcheck->Whl_Err_Cnt2[i] = 0;
pREcheck->Whl_Err_Cnt3[i] = 0;
pREcheck->WhlOutErrCnt[i]=0;
pREcheck->WhlTemErrCnt[i] = 0;
pREcheck->Whl_Diag[i] =POSE_OK;
pREcheck->Whl_Err[i] = POSE_OK;
}
}
switch (pWMod->WorkMode)
{
case ATTMOD_UNCTL:
case ATTMOD_RATEDMP:
case ATTMOD_MagTOSUN: /*<2A><><EFBFBD><EFBFBD><EFBFBD>з<EFBFBD><D0B7>ּ<EFBFBD><D6BC><EFBFBD>*/
for (i = 0; i < WHEEL_NUM; ++i)
{
if (pImpt->Whl_Use[i] == POSE_OK)
{
tmp_cnt++;
}
}
PWhl->Whl_UseCnt = tmp_cnt;
break;
case ATTMOD_WhlTOSUN:
case ATTMOD_ONLTOEARTH:
case ATTMOD_WHEELTOEARTH:
case ATTMOD_ATTAJUSTPRE:
case ATTMOD_ORBITCTL:
case ATTMOD_ATTAJUST:
case ATTMOD_ONLTOTAR:
//case ATTMOD_NOCOLL:
for (i = 0; i < WHEEL_NUM; ++i)
{
if (pREcheck->Whl_PowerOnCnt[i] >= tmpConst->Whl_Delay_CntLimt)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
if(PWhl->WhlIn_Valid[i]==0x55)
{pREcheck->WhlOutErrCnt[i]=0;}
else
{
if(pREcheck->WhlOutErrCnt[i]<60000)
{pREcheck->WhlOutErrCnt[i]++;}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
if(PWhl->Whl_Tmper[i]<(TYPE_CAL)tmpConst->WhlTemperDiagThr)
{pREcheck->WhlTemErrCnt[i]=0;}
else
{
if(pREcheck->WhlTemErrCnt[i]<60000)
{pREcheck->WhlTemErrCnt[i]++;}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣
if((pREcheck->WhlOutErrCnt[i]>=tmpConst->WhlDiag_ErrThr*2)||(pREcheck->WhlTemErrCnt[i]>=tmpConst->WhlDiag_TimeThr*2))
{
pREcheck->Whl_Err[i] = POSE_NO;
}
else
{
pREcheck->Whl_Err[i] = POSE_OK;
}
}
if (POSE_OK == tmpConst->AllowWhlONOFF[i])
{
ChlON = OC58_WHLX_ON + i*2;
ChlOFF = OC58_WHLX_ON + i*2 + 1;
//<2F><><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>Ӷϵ<D3B6>
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
&pREcheck->Whl_Err_Cnt1[i] , &pREcheck->Whl_Err_Cnt2[i], &pREcheck->Whl_Err_Cnt3[i],
tmpConst->Whl_Err_CntLimt , tmpConst->Whl_ReSet_CntLimt , tmpConst->Whl_OK_CntLimt , tmpConst->Whl_ReSetGAP_CntLimt ,
pREcheck->Whl_Err[i], &pREcheck->Whl_Diag[i] );
if (POSE_NO == pREcheck->Whl_Diag[i])
{
pImpt->Whl_Use[i] = POSE_NO;
}
}
else
{
pREcheck->Whl_Diag[i] =POSE_OK;
}
if (pImpt->Whl_Use[i] == POSE_OK)
{
tmp_cnt++;
}
}
PWhl->Whl_UseCnt = tmp_cnt;
if ((POSE_OK == tmpConst->AllowWhlONOFF[0])&&(POSE_OK == tmpConst->AllowWhlONOFF[1])&&(POSE_OK == tmpConst->AllowWhlONOFF[2])&&(POSE_OK == tmpConst->AllowWhlONOFF[3]))
{
if (PWhl->Whl_UseCnt >= 3)
{
//PWhl->Whl_OK = POSE_OK;
;
}
else
{
//PWhl->Whl_OK = POSE_NO;
//<2F>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3>󣬽<EFBFBD><F3A3ACBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memset(cmdDataBuff, 0x00, 50);
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
}
break;
default:
for (i = 0; i < WHEEL_NUM; ++i)
{
pREcheck->WhlOutErrCnt[i] =0;
pREcheck->WhlTemErrCnt[i] =0;
}
break;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD>쳣<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void PPUImuErrJudge(void)
{
// UINT8 PPUOn[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
//UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
INT16 TemH[4] ={0};
TYPE_CAL TemR[4] ={0.0},Temper[4] ={0.0},TempBAR[4] ={0.0};
TYPE_CAL TemA =-6.01188f;
TYPE_CAL TemB =4622.53337f;
TYPE_CAL TemC =-86421.72414f;
TYPE_CAL TemD =273.15f;
TYPE_CAL TemEXP =2.718281828f;
INT16 *pTemper =NULL;
UINT8 ChlON = 0;
UINT8 ChlOFF = 0;
UINT8 TmpPPU_Err = POSE_OK;
UINT8 i = 0;
UINT8 ONOFF = POWER_ON;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttErrCtlPara_t *pREcheck;
AttIMPTPara_t *pImpt;
PPUPara_t *PPPU;
sAttModPara_t *pWMod;
sDevInfo * tmpDevInfo;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pREcheck=&tmpAtt->sErrCtlPara;
pImpt=&tmpAtt->sIMPTPara;
PPPU = &tmpAtt->sPerPara.PPUPara;
pWMod =&tmpAtt->sModePara;
//<2F><>ȡAD<41>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
pTemper = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
////<2F><>ȡPPU<50><EFBFBD>
TemH[0] = *(pTemper + SW22_BOTTLE1-1); //<2F><>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD>1
TemH[1] = *(pTemper + SW23_BOTTLE2-1); //<2F><>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD>1
TemH[2]= *(pTemper + SW24_PIPE1-1); //<2F><>·<EFBFBD><C2B7><EFBFBD><EFBFBD>1
TemH[3]= *(pTemper + SW50_PIPE2-1); //<2F><>·<EFBFBD><C2B7><EFBFBD><EFBFBD>2
for(i=0;i<4;i++)
{
if (((3849.3-TemH[i])<1801.8)&&((3849.3-TemH[i]) > 0))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
if(POSE_ABSF(3849.3-TemH[i])<POSE_ZERO)
{
continue;
}
TemR[i] = 10000*(1801.8/(3849.3-TemH[i]) - 1);
//<2F><><EFBFBD><EFBFBD><C2B6>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>
TempBAR[i] =TemB*TemB - 4*TemC*(TemA -log(TemR[i])/log(TemEXP));
if (TempBAR[i] > POSE_ZERO)
{
if (POSE_ABSF(POSE_SQRTF(TempBAR[i]) - TemB)>POSE_ZERO)
{
Temper[i] =2*(TemC)/(POSE_SQRTF(TempBAR[i]) - TemB) - TemD;
}
}
}
}
PPPU->PPUT_BOTTLE_Tmper1 = Temper[0];
PPPU->PPUT_BOTTLE_Tmper2 = Temper[1];
PPPU->PPUT_PIPE_Tmper1 = Temper[2];
PPPU->PPUT_PIPE_Tmper2 = Temper[3];
if (pREcheck->PPU_PowerOnCnt >= tmpConst->PPU_Delay_CntLimt)
{
//ppu<70><75><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
if(PPPU->PPUIn_Valid!=POSE_OK)/*PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч*/
{
if(pREcheck->PPUOutErrCnt<60000)
{pREcheck->PPUOutErrCnt++;}
}
else
{
pREcheck->PPUOutErrCnt=0;
}
if(0x01 == PPPU->PPUIn_FIREOK)/*<2A><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־Ϊ1*/
{
if(PPPU->PPUIn_ACurrent < (((TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTALimit)/10.0)) //20231228
{pREcheck->PPUIn_ACurCnt=0;}
else
{
if(pREcheck->PPUIn_ACurCnt<60000)
{pREcheck->PPUIn_ACurCnt++;}
}
if(PPPU->PPUIn_AVoltage < (TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTVLimit)
{pREcheck->PPUIn_VCurCnt=0;}
else
{
if(pREcheck->PPUIn_VCurCnt<60000)
{pREcheck->PPUIn_VCurCnt++;}
}
}
else
{
pREcheck->PPUIn_ACurCnt=0;
pREcheck->PPUIn_VCurCnt=0;
}
if ((POSE_NO == PPPU->PPUOUT_Sta )||(POSE_IN == PPPU->PPUOUT_Sta))
{pREcheck->PPUOUT_FIRECnt=0;}
else
{
if(pREcheck->PPUOUT_FIRECnt<60000)
{pREcheck->PPUOUT_FIRECnt++;}
}
if ((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode))
{
if (((Temper[0]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[0]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[1]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[1]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))
&&((Temper[2]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[2]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[3]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[3]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit))))
{
if(pREcheck->PPUOUT_TemperECnt<60000)
{pREcheck->PPUOUT_TemperECnt++; }
}
else
{
pREcheck->PPUOUT_TemperECnt=0;
}
}
else
{
pREcheck->PPUOUT_TemperECnt=0;
}
//if(((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode)))&&(POSE_OK == tmpConst->AllowPPUONOFF))
if((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
{
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x03 ;
//pImpt->PPUUseStatus = POSE_NO;
//if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
{
pImpt->PPUUseStatus = POSE_NO;
}
}
else
{
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xFC ;
}
if(((0x01 == PPPU->PPUIn_FIREOK)&&((pREcheck->PPUIn_ACurCnt>= (tmpConst->PPUTATimeLimit*2))||(pREcheck->PPUIn_VCurCnt>=(tmpConst->PPUTVTimeLimit*2))))&&(POSE_OK == tmpConst->AllowPPUONOFF))
{
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x0C ;
//pImpt->PPUUseStatus = POSE_NO;
if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
{
pImpt->PPUUseStatus = POSE_NO;
}
}
else
{
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xF3 ;
}
if(((pREcheck->PPUOUT_FIRECnt>=tmpConst->AttCmdFlashPara.PPUTFireLimit*2)&&(PPPU->PPUIn_FIREOK!=0x01))&&(POSE_OK == tmpConst->AllowPPUONOFF))
{
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x30 ;
if ((POSE_NO==tmpConst->AttCmdFlashPara.WorkModeChangeAuto) ||(POSE_OK == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
{
//pImpt->PPUUseStatus = POSE_NO;
if((ATTMOD_ORBITCTL == pWMod->WorkMode)||(ATTMOD_ATTAJUSTPRE == pWMod->WorkMode))/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
{
pImpt->PPUUseStatus = POSE_NO;
}
}
else if ((POSE_OK==tmpConst->AttCmdFlashPara.WorkModeChangeAuto)&&(POSE_NO == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_IN;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_IN;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
}
else
{
;
}
}
else
{
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
}
if((pREcheck->PPUOUT_TemperECnt>=tmpConst->PPUTemperTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
{
pREcheck->PPU_Err = pREcheck->PPU_Err | 0xC0;
//pImpt->PPUUseStatus = POSE_NO;
if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
{
pImpt->PPUUseStatus = POSE_NO;
}
}
else
{
pREcheck->PPU_Err = pREcheck->PPU_Err & 0x3F;
}
//if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
//{
//pREcheck->PPU_Err_Cnt1 = 0;
//pREcheck->PPU_Err_Cnt2 = 0;
//pREcheck->PPU_Err_Cnt3 = 0;
//return;
//}
//if (ATTMOD_ORBITCTL ==pWMod->WorkMode)
if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
{
pREcheck->PPU_Err_Cnt1 = 0;
pREcheck->PPU_Err_Cnt2 = 0;
pREcheck->PPU_Err_Cnt3 = 0;
if (POSE_OK == tmpConst->AllowPPUONOFF)
{
;
}
else
{
pREcheck->PPU_Diag =POSE_OK;
pREcheck->PPU_Err = 0;
pREcheck->PPUOutErrCnt=0;
pREcheck->PPUIn_ACurCnt=0;
pREcheck->PPUIn_VCurCnt=0;
pREcheck->PPUOUT_FIRECnt=0;
pREcheck->PPUOUT_TemperECnt=0;
}
return;
}
}
if (0x03 == (pREcheck->PPU_Err & 0x03))
{
TmpPPU_Err = POSE_NO;
}
if (POSE_OK == tmpConst->AllowPPUONOFF)
{
ChlON = OC68_PPU_ON ;
ChlOFF = OC73_PPU_OFF;
//PPU<50><55><EFBFBD><EFBFBD><EFBFBD>Ӷϵ<D3B6>
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
&pREcheck->PPU_Err_Cnt1, &pREcheck->PPU_Err_Cnt2, &pREcheck->PPU_Err_Cnt3,
tmpConst->PPU_Err_CntLimt , tmpConst->PPU_ReSet_CntLimt , tmpConst->PPU_OK_CntLimt , tmpConst->PPU_ReSetGAP_CntLimt ,
TmpPPU_Err, &pREcheck->PPU_Diag );
if (POSE_NO == pREcheck->PPU_Diag)
{
pImpt->PPUUseStatus = POSE_NO;
}
}
else
{
pREcheck->PPU_Err_Cnt1 = 0;
pREcheck->PPU_Err_Cnt2 = 0;
pREcheck->PPU_Err_Cnt3 = 0;
pREcheck->PPU_Diag =POSE_OK;
pREcheck->PPU_Err = 0;
pREcheck->PPUOutErrCnt=0;
pREcheck->PPUIn_ACurCnt=0;
pREcheck->PPUIn_VCurCnt=0;
pREcheck->PPUOUT_FIRECnt=0;
pREcheck->PPUOUT_TemperECnt=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>0x55<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>̬δ<EFBFBD><EFBFBD>ʧ
*******************************************************************************************/
UINT8 AttLose(UINT8 i)
{
UINT8 result=0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttErrCtlPara_t *pREcheck =NULL;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return 0;}
pREcheck=&tmpAtt->sErrCtlPara;
if(POSE_OK == tmpAtt->sDeterPara.CLPAttValid)
{
pREcheck->AttLoseCnt=0;
return 0;
}
else
{
if(pREcheck->AttLoseCnt<60000)
{pREcheck->AttLoseCnt++;}
}
if(pREcheck->AttLoseCnt>=tmpConst->TimeThr_AttLoseDiag[i]*2)
{result=0x55;}
return result;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>ʧ<EFBFBD>ж<EFBFBD>
***********************************************/
void AttLoseJudge(void)
{
UINT8 cmdDataBuff[50] = {0};
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
sTaskInfo* tmpTaskInfo = NULL;
UINT8 tmpNewMod = 0x00;
UINT16 tmpNewModID = 0x00;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
sAttModPara_t *pWMod;
sAttOrbitCtlInfo_t *pOrb;
sAttTARCCtlInfo_t *pTARC;
PPUPara_t *PPPU = NULL;
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
{return;}
pWMod=&tmpAtt->sModePara;
pOrb=&tmpAtt->sOrbitInp;
pTARC=&tmpAtt->sTARCInp;
PPPU = &tmpAtt->sPerPara.PPUPara;
tmpNewMod = pWMod->WorkMode;
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP:
if(POSE_OK == AttLose(0))
{
tmpNewMod = ATTMOD_WhlTOSUN;
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
}
break;
case ATTMOD_WhlTOSUN:
if(POSE_OK == AttLose(1))
{
tmpNewMod = ATTMOD_MagTOSUN; //gg20240522js
tmpNewModID = ATT_CMDMODE_MagTOSUN;
}
break;
case ATTMOD_MagTOSUN:
if(POSE_OK == AttLose(2))
{
tmpNewMod = ATTMOD_UNCTL;
tmpNewModID = ATT_CMDMODE_UNCTL;
}
break;
case ATTMOD_ONLTOEARTH:
if(POSE_OK == AttLose(3))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
}
break;
case ATTMOD_WHEELTOEARTH:
if(POSE_OK == AttLose(4))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
}
break;
case ATTMOD_ATTAJUSTPRE:
if(POSE_OK == AttLose(5))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
pOrb->OrbDataIn_OK = POSE_NO;
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
}
break;
case ATTMOD_ORBITCTL:
if(POSE_OK == AttLose(6))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
pOrb->OrbDataIn_OK = POSE_NO;
/*<2A><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
}
break;
case ATTMOD_ATTAJUST:
if(POSE_OK == AttLose(7))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
}
break;
case ATTMOD_ONLTOTAR:
if(POSE_OK == AttLose(8))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
}
break;
default:
break;
}
if(tmpNewMod != pWMod->WorkMode)
{
memset(cmdDataBuff, 0x00, 50);
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
DoSetInt32To8(tmpNewModID, cmdDataBuff);
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
***********************************************/
void AttExJudge(void)
{
UINT8 cmdDataBuff[50] = {0};
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
sTaskInfo* tmpTaskInfo = NULL;
UINT8 tmpNewMod = 0x00;
UINT16 tmpNewModID = 0x00;
UINT8 i =0;
UINT8 tmpAttNOK[3] = {POSE_OK,POSE_OK,POSE_OK};
UINT8 tmpWhlOK = POSE_OK;
TYPE_CAL tmpMod =0.0;
TYPE_CAL tmpAngE[3] ={0.0};//<2F><>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
TYPE_CAL tmpAngRateE[3] ={0.0};//<2F><>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
WhlPara_t *PWhl =NULL;
sAttModPara_t *pWMod =NULL;
sAttErrCtlPara_t *pREcheck =NULL;
WhlPara_t *pWhl =NULL;
AttIMPTPara_t *pIMU =NULL;
PPUPara_t *PPPU = NULL;
sAttOrbitCtlInfo_t *pOrb =NULL;
sAttTARCCtlInfo_t *pTARC =NULL;
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
return;
PWhl = &tmpAtt->sPerPara.WhlPara;
pWMod=&tmpAtt->sModePara;
pREcheck=&tmpAtt->sErrCtlPara;
pWhl=&tmpAtt->sPerPara.WhlPara;
pIMU=&tmpAtt->sIMPTPara;
pOrb=&tmpAtt->sOrbitInp;
pTARC=&tmpAtt->sTARCInp;
PPPU = &tmpAtt->sPerPara.PPUPara;
tmpNewMod = pWMod->WorkMode;
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP:
{
CalNormal(tmpAtt->sDeterPara.CLPAttAngRat, 3, &tmpMod);
//ÿ30s<30><73>¼һ<C2BC>αջ<CEB1><D5BB><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ʸ<EFBFBD><CAB8>ģֵ
for(i=0;i<60;i++)
{pREcheck->AngRateMod_RATEDMP[i]=pREcheck->AngRateMod_RATEDMP[i+1];}
pREcheck->AngRateMod_RATEDMP[60]=tmpMod;
//<2F><><EFBFBD>ٶ<EFBFBD>ʸ<EFBFBD><CAB8>ģֵ<C4A3><D6B5>30sǰ<73><C7B0>ֵ<EFBFBD>ж<EFBFBD>
if ((pREcheck->AngRateMod_RATEDMP[60]-pREcheck->AngRateMod_RATEDMP[0])<(tmpConst->RateThr_AttEx[0]))
{
pREcheck->AttExCnt=0;
memset(pREcheck->AttExFlg,0,3*sizeof(UINT8));
}
else
{
memset(pREcheck->AttExFlg,0x55,3*sizeof(UINT8));
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[0]*2))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>״̬
if (PWhl->Whl_UseCnt >= 3)
{
tmpNewMod = ATTMOD_WhlTOSUN;
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
}
else
{
tmpNewMod = ATTMOD_MagTOSUN;
tmpNewModID = ATT_CMDMODE_MagTOSUN;
}
////<2F><><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䱸<EFBFBD>ݣ<EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD>ã<EFBFBD>
for(i=0;i<GYRO_NUM;i++)
{
if(POSE_OK == pREcheck->GyroIn_PowerOn[i])
{
pIMU->GyroUseState[i] = POSE_NO;//ʹ<><CAB9>״̬<D7B4><CCAC>0
}
}
}
break;
}
case ATTMOD_WhlTOSUN:
{
for(i=0;i<3;i++)
{
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[1])
{
tmpAttNOK[i] = POSE_OK;
}
else
{
tmpAttNOK[i] = POSE_NO;
}
}
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
{
pREcheck->AttExCnt=0;
}
else
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
for(i=0;i<3;i++)
{
pREcheck->AttExFlg[i]=tmpAttNOK[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[1]*2))
{
//<2F>дſض<C5BF><D8B6><EFBFBD>
tmpNewMod = ATTMOD_MagTOSUN;
tmpNewModID = ATT_CMDMODE_MagTOSUN;
pREcheck->AttExCnt = 0;
}
break;
}
case ATTMOD_MagTOSUN:
{
for(i=0;i<3;i++)
{
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[2])
{
tmpAttNOK[i] = POSE_OK;
}
else
{
tmpAttNOK[i] = POSE_NO;
}
}
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
{
pREcheck->AttExCnt=0;
}
else
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
for(i=0;i<3;i++)
{
pREcheck->AttExFlg[i]=tmpAttNOK[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[2]*2))
{
//<2F><><EFBFBD>ڴ<EFBFBD>ȷ<EFBFBD><C8B7>
tmpNewMod = ATTMOD_UNCTL;
tmpNewModID = ATT_CMDMODE_UNCTL;
}
break;
}
case ATTMOD_ONLTOEARTH :
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[3])
{
tmpAttNOK[i] = POSE_OK;
}
else
{
tmpAttNOK[i] = POSE_NO;
}
}
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
{
pREcheck->AttExCnt=0;
}
else
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
for(i=0;i<3;i++)
{
pREcheck->AttExFlg[i]=tmpAttNOK[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[3]*2))
{
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
}
break;
}
case ATTMOD_WHEELTOEARTH:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
if((POSE_ABSF(tmpAtt->sDeterPara.CLPAttAng[i])>tmpConst->AngThr_AttEx[4])||(POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i])>tmpConst->RateThr_AttEx[4]))
{pREcheck->AttExFlg[i]=0x55;}
else
{pREcheck->AttExFlg[i]=0;}
}
//XY<58><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if((POSE_OK == pREcheck->AttExFlg[0])||(POSE_OK == pREcheck->AttExFlg[1]))
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
else
{pREcheck->AttExCnt=0;}
//Z<><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(POSE_OK == pREcheck->AttExFlg[2])
{
if(pREcheck->YawCnt<60000)
{pREcheck->YawCnt++;}
}
else
{pREcheck->YawCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if((pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[4]*2))||
(pREcheck->YawCnt>=(tmpConst->YawCntLimit*2)))
{
pREcheck->YawCnt=0;
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
}
break;
}
case ATTMOD_ATTAJUSTPRE:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[5])
{
tmpAttNOK[i] = POSE_OK;
}
else
{
tmpAttNOK[i] = POSE_NO;
}
}
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
{
pREcheck->AttExCnt=0;
}
else
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
for(i=0;i<3;i++)
{
pREcheck->AttExFlg[i]=tmpAttNOK[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[5]*2))
{
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
}
break;
}
case ATTMOD_ORBITCTL:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[6])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[6]))
{pREcheck->AttExFlg[i]=0x55;}
else
{pREcheck->AttExFlg[i]=0;}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD>6000rpm
for(i=0;i<4;i++)
{
//if (pWhl->Whl_Rate[i] > 6000*RPM2RADS)
if (POSE_ABSF(pWhl->Whl_Rate[i]) > (628.32 * RADS2RPM))
{
tmpWhlOK = POSE_NO;
break;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 == pREcheck->AttExFlg[2]) || (POSE_NO == tmpWhlOK))
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
else
{pREcheck->AttExCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[6]*2))
{
//<2F><><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Եض<D4B5><D8B6><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
/*<2A><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EEA3AC>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
}
break;
}
case ATTMOD_ATTAJUST:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[7])
{
tmpAttNOK[i] = POSE_OK;
}
else
{
tmpAttNOK[i] = POSE_NO;
}
}
if((POSE_NO ==tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
{
pREcheck->AttExCnt=0;
}
else
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
for(i=0;i<3;i++)
{
pREcheck->AttExFlg[i]=tmpAttNOK[i];
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[7]*2))
{
//<2F><><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Եض<D4B5><D8B6><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
}
break;
}
case ATTMOD_ONLTOTAR:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[8])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[8]))
{pREcheck->AttExFlg[i]=0x55;}
else
{pREcheck->AttExFlg[i]=0;}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 ==pREcheck->AttExFlg[2]) )
{
if(pREcheck->AttExCnt<60000)
{pREcheck->AttExCnt++;}
}
else
{pREcheck->AttExCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[8]*2))
{
//<2F><><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Եض<D4B5><D8B6><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
}
break;
}
default:
break;
}
if(tmpNewMod != pWMod->WorkMode)
{
memset(cmdDataBuff, 0x00, 50);
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
DoSetInt32To8(tmpNewModID, cmdDataBuff);
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> */
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><EFBFBD>ж<EFBFBD>
***********************************************/
void AttUnStaJudge(void)
{
UINT8 cmdDataBuff[50] = {0};
sTaskInfo* tmpTaskInfo = NULL;
UINT8 i =0;
UINT8 tmpNewMod = 0x00;
UINT16 tmpNewModID = 0x00;
TYPE_CAL tmpAngE[3]= {0.0f};//<2F><>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
TYPE_CAL tmpAngRateE[3] ={0.0f};//<2F><>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
sAttErrCtlPara_t *pREcheck =NULL;
WhlPara_t *PWhl =NULL;
sAttOrbitCtlInfo_t *pOrb =NULL;
sAttTARCCtlInfo_t *pTARC =NULL;
sAttDeterPara_t *pDeter =NULL;
PPUPara_t *PPPU = NULL;
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
{return;}
pWMod=&tmpAtt->sModePara;
pREcheck=&tmpAtt->sErrCtlPara;
//pCtrl=&tmpAtt->sCtlPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
tmpNewMod = pWMod->WorkMode;
pOrb=&tmpAtt->sOrbitInp;
pTARC=&tmpAtt->sTARCInp;
pDeter=&tmpAtt->sDeterPara;
PPPU = &tmpAtt->sPerPara.PPUPara;
switch(pWMod->WorkMode)
{
//<2F><><EFBFBD><EFBFBD>
case ATTMOD_RATEDMP:
{
if(pREcheck->AttUnStaCnt<60000)
{pREcheck->AttUnStaCnt++;}
if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[0]*2))
{
//<2F><><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>ָ<EFBFBD><D6B8>
if (PWhl->Whl_UseCnt >= 3)
{
tmpNewMod = ATTMOD_WhlTOSUN;
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
}
else
{
tmpNewMod = ATTMOD_MagTOSUN;
tmpNewModID = ATT_CMDMODE_MagTOSUN;
}
}
break;
}
case ATTMOD_WhlTOSUN:
{
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[1])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[1]))
{pREcheck->AttUnSta[i]=0;}
else
{pREcheck->AttUnSta[i]=0x55;}
}
if((POSE_OK == pREcheck->AttUnSta[0])||(POSE_OK == pREcheck->AttUnSta[1])||(POSE_OK == pREcheck->AttUnSta[2]))
{
if(pREcheck->AttUnStaCnt<60000)
{pREcheck->AttUnStaCnt++;}
}
else
{pREcheck->AttUnStaCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[1]*2))
{
//<2F>дſض<C5BF><D8B6><EFBFBD>
tmpNewMod = ATTMOD_MagTOSUN;
tmpNewModID = ATT_CMDMODE_MagTOSUN;
}
break;
}
case ATTMOD_MagTOSUN:
{
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
//tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
tmpAngE[i]=pDeter->AngToSun[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[2])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[2]))
{pREcheck->AttUnSta[i]=0;}
else
{pREcheck->AttUnSta[i]=0x55;}
}
if((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))
{
if(pREcheck->AttUnStaCnt<60000)
{pREcheck->AttUnStaCnt++;}
}
else
{pREcheck->AttUnStaCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[2]*2))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>޿<EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_UNCTL;
tmpNewModID = ATT_CMDMODE_UNCTL;
}
break;
}
case ATTMOD_ONLTOEARTH:
{
if(pREcheck->AttUnStaCnt<60000)
{
pREcheck->AttUnStaCnt++;
}
if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[3]*2))
{
tmpNewMod = ATTMOD_RATEDMP;
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
}
break;
}
case ATTMOD_ATTAJUSTPRE:
{
if(pREcheck->AttModCnt<60000)
{pREcheck->AttModCnt++;}
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[4])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[4]))
{pREcheck->AttUnSta[i]=0;}
else
{pREcheck->AttUnSta[i]=0x55;}
}
if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[4]*2)))
{
if(pREcheck->AttUnStaCnt<60000)
{pREcheck->AttUnStaCnt++;}
}
else
{pREcheck->AttUnStaCnt=0;}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
if(pREcheck->AttUnStaCnt>= (tmpConst->TimeThr_AttUnSta[4]*2))
{
//<2F><><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Եض<D4B5><D8B6><EFBFBD>ָ<EFBFBD><D6B8>
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
}
break;
}
case ATTMOD_ATTAJUST:
{
if(pREcheck->AttModCnt<60000)
{pREcheck->AttModCnt++;}
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
for(i=0;i<3;i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƫ<EFBFBD><C6AB>
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
DeviAngleLimit(&tmpAngE[i]);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[5])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[5]))
{pREcheck->AttUnSta[i]=0;}
else
{pREcheck->AttUnSta[i]=0x55;}
}
if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[5]*2)))
{
if(pREcheck->AttUnStaCnt<60000)
{pREcheck->AttUnStaCnt++;}
}
else
{pREcheck->AttUnStaCnt=0;}
if(pREcheck->AttUnStaCnt > (tmpConst->TimeThr_AttUnSta[5]*2))
{
tmpNewMod = ATTMOD_ONLTOEARTH;
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
}
break;
}
default:
break;
}
if (tmpNewMod != pWMod->WorkMode)
{
memset(cmdDataBuff, 0x00, 50);
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
DoSetInt32To8(tmpNewModID, cmdDataBuff);
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
}
/************************************************
˵<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 ZKREAction(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttErrCtlPara_t *pREcheck =NULL;
sAttDeterPara_t *pDeter =NULL;
PPUPara_t *PPPU = NULL;
UINT8 PPUOn2[8] = {0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
if((NULL==tmpAtt) || (NULL==tmpConst))
return;
pREcheck=&tmpAtt->sErrCtlPara;
pDeter=&tmpAtt->sDeterPara;
PPPU = &tmpAtt->sPerPara.PPUPara;
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
GyroImuErrJudge();
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
SsImuErrJudge();
//ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
MagImuErrJudge();
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
WhlImuErrJudge();
//PPU<50>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
PPUImuErrJudge();
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD>PPU<50><55>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>˳<EFBFBD><CBB3><EFBFBD>PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȿ<EFBFBD>*/
if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_Err_Flag)/*<2A><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD>˳<EFBFBD>*/
{
if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt <= 250)/*<2A>Լ<EFBFBD><D4BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt++;
if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt >= 5)/*2.5<EFBFBD><EFBFBD><EFBFBD>󣬷<EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD>ƹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>*/
{
OCOut(OC73_PPU_OFF, 0, 16);
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_NO;
}
}
else if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*<2A><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD>*/
{
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*<2A>Լ<EFBFBD><D4BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 5)/*5<><35><EFBFBD>󣬷<EFBFBD><F3A3ACB7>͵<EFBFBD><CDB5>ƹ<EFBFBD><C6B9>翪ָ<E7BFAA><D6B8>*/
{
OCOut(OC68_PPU_ON, 0, 16);
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
}
}
else if(POSE_IN == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*<2A><><EFBFBD>ε<EFBFBD><CEB5><EFBFBD>*/
{
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*<2A>Լ<EFBFBD><D4BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt == 5)/*2.5<EFBFBD><EFBFBD><EFBFBD>󣬷<EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD>ƹ<EFBFBD><EFBFBD>翪ָ<EFBFBD><EFBFBD>*/
{
OCOut(OC68_PPU_ON, 0, 16);
}
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 65)/*30<33><30><EFBFBD>󣬷<EFBFBD><F3A3ACB7>͵<EFBFBD><CDB5>ƹ<EFBFBD><C6B9>翪ָ<E7BFAA><D6B8>*/
{
//PPU<50><55><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
dev_can_write(DEV_NO_CAN0,PPUOn2,8);
PPPU->PPUOUT_Sta = POSE_OK;
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
pREcheck->PPUOUT_FIRECnt=0;
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
}
}
else
{;}/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
//<2F><>̬<EFBFBD><CCAC>׼<EFBFBD><D7BC>ʧ<EFBFBD>ж<EFBFBD>
if(0x55 == tmpConst->AllowAttLoseDiagFlg)
{
AttLoseJudge();
}
else
{
pREcheck->AttLoseCnt=0;
}
//<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if((0x55 == tmpConst->AllowAttExDiagFlg)&&(0x55 == pDeter->CLPAttValid))
{AttExJudge();}
else
{
pREcheck->AttExFlg[0]=0x00;
pREcheck->AttExFlg[1]=0x00;
pREcheck->AttExFlg[2]=0x00;
pREcheck->AttExCnt=0;
}
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6>ж<EFBFBD>
if((0x55 == tmpConst->AllowAttStaDiagFlg)&&(0x55 == pDeter->CLPAttValid))
{AttUnStaJudge(); }
else
{
pREcheck->AttUnSta[0] = 0;
pREcheck->AttUnSta[1] = 0;
pREcheck->AttUnSta[2] = 0;
pREcheck->AttUnStaCnt=0;
}
}
Reference in New Issue Copy Permalink