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

461 lines
13 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\BlockRing.h"
#include "..\PrjCommon\DataStorDefine.h"
#include "..\PrjCommon\DevDefine.h"
//#include "..\PrjCommon\DevDrvs.h"
//#include "..\PrjPowCtrlMng\PowCtlMng.h"
#include "AttMath.h"
#include "AttCtrlMain.h"
/********************************************************************/
//extern sAttPriData *m_pZKPriInfo;
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿ع<EFBFBD><EFBFBD><EFBFBD>ģʽѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>,<EFBFBD><EFBFBD>ģʽ<EFBFBD>л<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>ֲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
ע<EFBFBD><EFBFBD>: ģʽ<EFBFBD>л<EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
***********************************************/
void ZKModChgInit(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCmdDsp_t *pCmdDsp= NULL;
if(NULL == tmpAtt)
{return;}
pCmdDsp = &tmpAtt->sCmdDspPara;
//<2F>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3>󣬽<EFBFBD><F3A3ACBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#ifdef MINMODULE_TYPE
tmpAtt->sModePara.WModCnt2[0]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sModePara.WModCnt2[1]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#else
//pAttCtlPara->sWModePara.WModCnt1=0; //1<><31><EFBFBD><EFBFBD>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>ģʽ
tmpAtt->sModePara.WModCnt2[0]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sModePara.WModCnt2[1]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sModePara.WModCnt3=0; //3<><33><EFBFBD>ֿض<D6BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>
tmpAtt->sModePara.WModCnt4=0; //4<><34><EFBFBD>ſض<C5BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>
tmpAtt->sModePara.WModCnt5=0; //5<><35><EFBFBD>Եض<D4B5><D8B6><EFBFBD>
tmpAtt->sModePara.WModCnt6=0; //6<><36><EFBFBD><EFBFBD>̬<EFBFBD>Ե<EFBFBD>
tmpAtt->sModePara.WModCnt7=0; //7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
tmpAtt->sModePara.WModCnt8=0; //8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sModePara.WModCntA=0; //11<31><31><EFBFBD><EFBFBD>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>
pCmdDsp->Wheel_Cmd_Cnt = 0; /*<2A><><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ָ<EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>*/
tmpAtt->sCtlPara.PD2PIDCnt=0; //PDתPID<49><44><EFBFBD><EFBFBD>
tmpAtt->sCtlPara.CtrlPD2PID=0; //
memset(tmpAtt->sCtlPara.SumAerrPre, 0.0, 3*sizeof(TYPE_CAL)); //PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sErrCtlPara.AttLoseCnt=0;//ģʽ<C4A3>л<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sErrCtlPara.AttExCnt=0;
tmpAtt->sErrCtlPara.AttExFlg[0]=0x00;
tmpAtt->sErrCtlPara.AttExFlg[1]=0x00;
tmpAtt->sErrCtlPara.AttExFlg[2]=0x00;
tmpAtt->sErrCtlPara.AttUnStaCnt=0;
tmpAtt->sErrCtlPara.AttModCnt=0;
tmpAtt->sErrCtlPara.AttUnSta[0] = 0;
tmpAtt->sErrCtlPara.AttUnSta[1] = 0;
tmpAtt->sErrCtlPara.AttUnSta[2] = 0;
tmpAtt->sErrCtlPara.YawCnt = 0;
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ǰģʽ
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>
ע<EFBFBD><EFBFBD>:
***********************************************/
void ZKModDoSet(UINT8 newWMode)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
if (newWMode == tmpAtt->sModePara.WorkMode)
{return;}
tmpAtt->sModePara.WorkModePre = tmpAtt->sModePara.WorkMode;
tmpAtt->sModePara.WorkMode = newWMode;
tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;
}
/***********************************************
˵<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 ZKModInit(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
AttCmdDsp_t *pCmdDsp= NULL;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod =&tmpAtt->sModePara;
pCmdDsp = &tmpAtt->sCmdDspPara;
//<2F><><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>ʼ<EFBFBD><CABC>Ϊ<EFBFBD><CEAA>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>
#ifdef MINMODULE_TYPE
pWMod->WorkMode = ATTMOD_RATEDMP;
#else
pWMod->WorkMode = ATTMOD_NOCOLL;
#endif
pWMod->ReDataBK = POSE_NO;
pWMod->ATTFlashSwitch= POSE_NO;
pWMod->Mod2UnctlFirst=0x55;
pWMod->WorkModePre = ATTMOD_NOCOLL;
tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;
pWMod->ZKPowOnFirst =0; //<2F>״η<D7B4><CEB7><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pWMod->ZKPowOnStep =0; //<2F>״η<D7B4><CEB7><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD>
//pWMod->ModeTRATEDMP =0;
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO; //<2F><><EFBFBD>ε<EFBFBD><CEB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>Ĭ<EFBFBD>Ͻ<EFBFBD>ֹ
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λΪs<CEAA><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
//tmpConst->judge_time1 =200;
tmpConst->judge_time1 =180; //ZMX20240115
tmpConst->judge_time2 = 60;
tmpConst->judge_time3=30;
tmpConst->AttCmdFlashPara.judgeWait_time=6000;
tmpConst->judge_time4 = 0;
tmpConst->judge_time5 = 30;
tmpConst->judge_time6 = 0;
tmpConst->judge_time7 = 0;
tmpConst->judge_time8 = 0;
tmpConst->judge_time9 = 30;
tmpConst->judge_timeA = 0;
tmpConst->judge_timeB = 120;
//<2F>ж<EFBFBD><D0B6><EFBFBD>ֵ<EFBFBD><D6B5>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
tmpConst->RateThr_2T3 = 0.3f * ANG2RADIAN; //0.3<EFBFBD><EFBFBD>/s
tmpConst->HWHLThr_2T3 = 0.5f;
tmpConst->AngThr_3TB = 2.0f * ANG2RADIAN; //2.0<EFBFBD><EFBFBD> --<2D><><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>ת<EFBFBD>޿<EFBFBD>ģʽ<C4A3>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ
tmpConst->RateThr_3TB = 0.1f * ANG2RADIAN; //0.1<EFBFBD><EFBFBD>/s--<2D><><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>ת<EFBFBD>޿<EFBFBD>ģʽ<C4A3><CABD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>ֵ
tmpConst->AngThr_5T6 = 2.0f * ANG2RADIAN; //<2F>Եض<D4B5><D8B6><EFBFBD>ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>ģʽ<C4A3>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ
tmpConst->RateThr_5T6 = 0.1f * ANG2RADIAN; //<2F>Եض<D4B5><D8B6><EFBFBD>ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>ģʽ<C4A3><CABD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>ֵ
tmpConst->AngThr_9TA = 2.0f * ANG2RADIAN;
tmpConst->RateThr_9TA = 0.1f * ANG2RADIAN;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
pWMod->WModCnt2[0]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pWMod->WModCnt2[1]=0; //2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pWMod->WModCnt3=0; //3<><33><EFBFBD>ֿض<D6BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>
pWMod->WModCnt4=0; //4<><34><EFBFBD>ſض<C5BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>
pWMod->WModCnt5=0; //5<><35><EFBFBD>Եض<D4B5><D8B6><EFBFBD>
pWMod->WModCnt6=0; //6<><36><EFBFBD><EFBFBD>̬<EFBFBD>Ե<EFBFBD>
pWMod->WModCnt7=0; //7<><37><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
pWMod->WModCnt8=0; //8<><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pWMod->WModCntA=0; //11<31><31><EFBFBD><EFBFBD>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>
pWMod->WModCntB= 0;
pWMod->WModCnt1 = 0;
pCmdDsp->Wheel_Cmd_Cnt = 0;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿ع<EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
ע<EFBFBD><EFBFBD>: ÿ0.5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
***********************************************/
void ZKDoModCal(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
sAttDeterPara_t *pDeter =NULL;
WhlPara_t *PWhl =NULL;
//AttIMPTPara_t *pImpt;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod=&tmpAtt->sModePara;
pDeter=&tmpAtt->sDeterPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
//pImpt=&tmpAtt->sIMPTPara;
#ifdef MINMODULE_TYPE
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP://<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>10s<30><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0.3<EFBFBD><EFBFBD>/s&&<26>ջ<EFBFBD><D5BB><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
if(
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
(pDeter->CLPAttValid==0x55))
{
if(pWMod->WModCnt2[0]<60000)
{pWMod->WModCnt2[0]++;}
}
else
{
pWMod->WModCnt2[0]=0;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ؽǶ<D8BD><C7B6><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>0.5Nms
if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
{
if(pWMod->WModCnt2[1]<60000)
{pWMod->WModCnt2[1]++;}
}
else
{
pWMod->WModCnt2[1]=0;
}
break;
}
default:
{
break;
}
}
#else
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP://<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
{
//<2F><><EFBFBD><EFBFBD>60s<30><73><EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־Ϊ<D6BE><CEAA>Ч<EFBFBD><D0A7><EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>ڡ<EFBFBD>0.3<EFBFBD><EFBFBD>/s<><73>
if(
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
(0x55 == pDeter->CLPAttValid))
{
if(pWMod->WModCnt2[0]<60000)
{pWMod->WModCnt2[0]++;}
}
else
{
pWMod->WModCnt2[0]=0;
}
/*<2A><><EFBFBD><EFBFBD>60s<30><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־<EFBFBD><D6BE>Ч<EFBFBD>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>ģС<C4A3><D0A1>0.5Nms*/
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ؽǶ<D8BD><C7B6><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>0.5Nms
if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
{
if(pWMod->WModCnt2[1]<60000)
{pWMod->WModCnt2[1]++;}
}
else
{
pWMod->WModCnt2[1]=0;
}
break;
}
case ATTMOD_WhlTOSUN://<2F><><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>ת<EFBFBD>޿<EFBFBD>ģʽ
//case ATTMOD_MagTOSUN://<2F><><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>ת<EFBFBD>޿<EFBFBD>ģʽ
{
if((0x55 == pDeter->CLPAttValid) && (0x00 == pWMod->WModSadaOpen) && //<2F><><EFBFBD><EFBFBD>չ<EFBFBD><D5B9><EFBFBD><EFBFBD>־Ϊδչ<CEB4><D5B9>״̬ ע<>ⷫ<EFBFBD><E2B7AB>δչ<CEB4><D5B9>δ00<30><30>չ<EFBFBD><D5B9>Ϊ55
//(POSE_ABSF(pDeter->CLPAttAng[1])<=tmpConst->AngThr_3TB) &&
//(POSE_ABSF(pDeter->CLPAttAng[2])<=tmpConst->AngThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_3TB) && //20230926
(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_3TB))
{
if(pWMod->WModCnt3<60000)
{pWMod->WModCnt3++;}
}
else {pWMod->WModCnt3=0;}
break;
}
case ATTMOD_UNCTL://<2F>޿<EFBFBD>ģʽת<CABD><D7AA><EFBFBD>ն<EFBFBD><D5B6><EFBFBD>
{
if (0x55 == pWMod->WModSadaOpen)
{
if(pWMod->WModCntB<60000) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>WModCntB<74><42>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
{pWMod->WModCntB++;}
}
break;
}
case ATTMOD_ONLTOEARTH://<2F>Եض<D4B5><D8B6><EFBFBD>ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>ģʽ<C4A3><CABD>
{
if((0x55 == pDeter->CLPAttValid) &&
(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_5T6)
//(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
)
{
if(pWMod->WModCnt5<60000)
{pWMod->WModCnt5++;}
}
else {pWMod->WModCnt5=0;}
break;
}
//case ATTMOD_ATTAJUST://<2F>Եض<D4B5><D8B6><EFBFBD>ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>ģʽ<C4A3><CABD>
//{
//if((0x55 == pDeter->CLPAttValid) &&
//(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_9TA)
////(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
//)
//{
//if(pWMod->WModCnt9<60000)
//{pWMod->WModCnt9++;}
//}
//else {pWMod->WModCnt9=0;}
//break;
//}
default:
{
break;
}
}
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿ع<EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><EFBFBD>жϣ<EFBFBD><EFBFBD>ж<EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD>ģʽָ<EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD>ջ<EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD>
ע<EFBFBD><EFBFBD>:
***********************************************/
void ZKModeSel_Auto(void)
{
UINT8 tmpNewMod =0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
WhlPara_t *PWhl =NULL;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod=&tmpAtt->sModePara;
PWhl = &tmpAtt->sPerPara.WhlPara;
tmpNewMod=pWMod->WorkMode;
#ifdef MINMODULE_TYPE
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP:
{
if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
{
tmpNewMod = ATTMOD_MagTOSUN;
pWMod->WModCnt2[0]=0;
pWMod->WModCnt2[1]=0;
}
break;
}
default:
{
break;
}
}
#else
switch(pWMod->WorkMode)
{
case ATTMOD_NOCOLL://<2F><>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>ģʽ
{
if (pWMod->WModCnt1>= (tmpConst->judge_time1*2) || (POSE_OK == pWMod->WModSadaOpen))
{
tmpNewMod = ATTMOD_RATEDMP;
}
break;
}
case ATTMOD_RATEDMP://<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
{
if (PWhl->Whl_UseCnt >= 3)
{
tmpNewMod = ATTMOD_WhlTOSUN;
}
else
{
tmpNewMod = ATTMOD_MagTOSUN;
}
}
break;
}
case ATTMOD_WhlTOSUN:
{
if((pWMod->WModCnt3>=(tmpConst->judge_time3 * 2))&&(pWMod->WModCnt1>=(tmpConst->AttCmdFlashPara.judgeWait_time * 2)))
{
//pWMod->Mod2UnctlFirst=0x55;
tmpNewMod = ATTMOD_UNCTL;
}
break;
}
case ATTMOD_UNCTL:
{
if (0x55 == pWMod->Mod2UnctlFirst)
{
if(pWMod->WModCntB>= (tmpConst->judge_timeB* 2)) //<2F><><EFBFBD><EFBFBD>120s
{
tmpNewMod = ATTMOD_RATEDMP;
pWMod->Mod2UnctlFirst=0x00;
}
}
break;
}
case ATTMOD_ONLTOEARTH:
{
if(pWMod->WModCnt5>=(tmpConst->judge_time5* 2)) //<2F><><EFBFBD><EFBFBD>30s
{
tmpNewMod = ATTMOD_WHEELTOEARTH;
}
break;
}
default:
{
break;
}
}
#endif
tmpAtt->sModePara.ModeSwitchFlg = POSE_NO;
ZKModDoSet(tmpNewMod);
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿ع<EFBFBD><EFBFBD><EFBFBD>ģʽѡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ע<EFBFBD><EFBFBD>: ÿ0.5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
***********************************************/
void ZKDoModeAct(void)
{
//AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
//
//if(NULL==tmpConst)
//{
//return;
//}
ZKDoModCal();
ZKModeSel_Auto();
}
Reference in New Issue Copy Permalink