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

4828 lines
138 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 "AttCtrlMain.h"
#include "..\PrjTelmMng\TelmMng.h" /* <20>غ<EFBFBD><D8BA>첽<EFBFBD><ECB2BD>ָ֪ͨ<D6AA><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
/***********************************************************************/
sTaskInfo *m_pZKTaskInfo; /*<2A><><EFBFBD><EFBFBD><EFBFBD>ṹָ<E1B9B9><D6B8>,<2C>Ѿ<EFBFBD><D1BE><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊδ<CEAA><CEB4>*/
//<2F><>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
static const TYPE_CAL M_SSA[3][3] = {{0.3420F,-0.2114F,-0.9156F},{-0.9397F,-0.0769F,-0.3333F},{-0.0000F,0.9744F,-0.2250F}};
static const TYPE_CAL M_SSB[3][3] = {{1.0000F,0.0F, 0.0F},{0.0F,-0.2250F,-0.9744F},{0.0F,0.9744F,-0.2250F}};
//static const TYPE_CAL M_SSC[3][3] ={{0.3214F,-0.3420F,0.8830F},{-0.1170F,-0.9397F,-0.3214F},{0.9397F, 0.0000F,-0.3420F}};
static const TYPE_CAL M_SSC[3][3] ={{0.2962F,-0.5F,0.8138F},{-0.1710F,-0.8660F,-0.4698F},{0.9397F, 0.0000F,-0.3420F}};
//static const TYPE_CAL M_GYROA[3][3] = {{-0.0000F,1.0000F,-0.0000F} , {1.0000F,0.0000F,0.0000F},{0.0000F,0.0F,-1.0000F}};
//static const TYPE_CAL M_GYROB[3][3] = {{-0.0000F,1.0000F, -0.0000F},{1.0000F,0.0000F,0.0000F},{0.0000F,0.0F,-1.0000F}};
static const TYPE_CAL M_GYROA[3][3] = {{-1.0000F,0.0000F,0.0000F}, {0.0000F,1.0000F,0.0000F},{0.0000F,0.0F,-1.0000F}};
static const TYPE_CAL M_GYROB[3][3] = {{-1.0000F,0.0000F,0.0000F}, {0.0000F,1.0000F,0.0000F},{0.0000F,0.0F,-1.0000F}};
static const TYPE_CAL M_GYROC[3][3] = {{0.0000F,-1.0000F,0.0F},{1.0000F,0.0000F,0.0F},{0.0F,0.0F,1.0000F}};
static const TYPE_CAL M_aSSA[3][3] = { { 0.0F, -0.6427876F, 0.7660444F },{ 0.0F, -0.7660444F ,-0.6427876F },{ 1.0F, 0.0F ,0.0F } }; //ģ<><C4A3>̫<EFBFBD><CCAB>A
static const TYPE_CAL M_aSSB[3][3] = { { 0.0F, -0.6427876F, -0.7660444F },{ 0.0F, 0.7660444F ,-0.6427876F },{ 1.0F, 0.0F ,0.0F } }; //ģ<><C4A3>̫<EFBFBD><CCAB>B
static const TYPE_CAL M_aSSC[3][3] = { {-0.8660254F ,0.0F, 0.5F },{ 0.5F, 0.0F ,0.8660254F },{ 0.0F ,1.0F ,0.0F } }; //ģ<><C4A3>̫<EFBFBD><CCAB>C
static const TYPE_CAL M_aSSD[3][3] = { {0.0F,0.8660254F, -0.5F },{ 0.0F, 0.5F ,0.8660254F },{ 1.0F ,0.0F ,0.0F } }; //ģ<><C4A3>̫<EFBFBD><CCAB>D
static const TYPE_CAL M_aSSE[3][3] = { { 0.0F,1.0F, 0.0F },{ 1.0F,0.0F,0.0F },{ 0.0F, 0.0F ,-1.0F } }; //ģ<><C4A3>̫<EFBFBD><CCAB>E
static const TYPE_CAL M_aSSF[3][3] = { { -1.0F,0.0F,0.0F },{ 0.0F,0.0F,1.0F},{ 0.0F,1.0F,0.0F } };
static const TYPE_CAL M_MAG1[3][3] = { { 1.0F,0.0F,0.0F },{ 0.0F,-1.0F,0.0F },{ 0.0F,0.0F,-1.0F } }; //<2F><>ǿ<EFBFBD>ư<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
static const TYPE_CAL M_MAG2[3][3] = { { 1.0F,0.0F,0.0F },{ 0.0F,-1.0F,0.0F },{ 0.0F,0.0F,-1.0F } }; //<2F><>ǿ<EFBFBD>ư<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
static const TYPE_CAL M_MAGCtrl[3][4] = { { 1.0F,0.0F,0.0F,0.0F },{ 0.0F,1.0F,0.0F,0.0F},{ 0.0F,0.0F,1.0F,1.0F} }; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
static const TYPE_CAL M_Wheel[3][4] = { { -1.0F,0.0F,0.0F,0.4912F },{ 0.0F,-1.0F,0.0F,-0.4912F},{ 0.0F,0.0F,-1.0F,-0.7193F} };
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><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 ZKDptPowerOnPrc(void)
{
sAttPriData *tmpAtt = NULL;
sTaskMngDataRecord *tmpTaskMngDataRcd = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
if((NULL == tmpAtt) || (NULL == tmpTaskMngDataRcd))
{
return;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ磬<D3B5><E7A3AC><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־5s<35><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־Ϊ<D6BE><CEAA><EFBFBD><EFBFBD>0x55<35><35> <20>˴<EFBFBD><CBB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD>5s*/
if(0x55 != tmpTaskMngDataRcd->bSHDepart)
{
//ZKModDoSet(ATTMOD_BREAKPRE);
tmpAtt->sModePara.ZKPowOnFirst = 0x55;
tmpAtt->sModePara.WModCnt1 = 0;
}
else
{
if(tmpAtt->sModePara.WModCnt1 <= 60000)
tmpAtt->sModePara.WModCnt1++;
}
/*<2A><><EFBFBD>Ƿ<EFBFBD><C7B7>룬<EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A1/A2/B<><42><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF>A/B<><42><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A/B/C<><43>GNSS<53><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>X/Y/Z/S<><53>SADA<44><41><EFBFBD>ݡ<EFBFBD><DDA1><EFBFBD><EFBFBD><EFBFBD>*/
if((0x55 == tmpTaskMngDataRcd->bSHDepart) /*<2A><><EFBFBD><EFBFBD> */
&& (tmpAtt->sModePara.WModCnt1 >= 5) /*<2A><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD>5s<35><73>Ч*/
&& (0x55 == tmpAtt->sModePara.ZKPowOnFirst) /*<2A><> <20><>δ<EFBFBD><CEB4><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD> */
)
{
if (0x00 == tmpAtt->sModePara.ZKPowOnStep)
{
//<2F><><EFBFBD><EFBFBD>A/B/C<>ӵ<EFBFBD>
OCOut(OC74_STA_ON, 0, 16);
OCOut(OC76_STB_ON, 0, 16);
OCOut(OC78_STC_ON, 0, 16);
//<2F><><EFBFBD><EFBFBD>
OCOut(OC66_GYROA_ON, 0, 16);
OCOut(OC69_GYROB_ON, 0, 16);
OCOut(OC71_GYROC_ON, 0, 16);
//<2F><>ǿ<EFBFBD><C7BF>
OCOut(OC94_CQJA_ON, 0, 16);
OCOut(OC96_CQJB_ON, 0, 16);
//GNS
//OCOut(OC79_GNSSA_ON, 0, 16);
//ZKModDoSet(ATTMOD_NOCOLL);
tmpAtt->sModePara.ZKPowOnStep = 0x11;
}
else if (0x11 == tmpAtt->sModePara.ZKPowOnStep)
{
//<2F><><EFBFBD><EFBFBD>A<EFBFBD>ӵ<EFBFBD>
OCOut(OC58_WHLX_ON, 0, 16);
tmpAtt->sModePara.ZKPowOnStep = 0x22;
}
else if (0x22 == tmpAtt->sModePara.ZKPowOnStep)
{
//<2F><><EFBFBD><EFBFBD>B<EFBFBD>ӵ<EFBFBD>
OCOut(OC60_WHLY_ON, 0, 16);
tmpAtt->sModePara.ZKPowOnStep = 0x33;
}
else if (0x33 == tmpAtt->sModePara.ZKPowOnStep)
{
//<2F><><EFBFBD><EFBFBD>C<EFBFBD>ӵ<EFBFBD>
OCOut(OC62_WHLZ_ON, 0, 16);
tmpAtt->sModePara.ZKPowOnStep = 0x44;
}
else if (0x44 == tmpAtt->sModePara.ZKPowOnStep)
{
//<2F><><EFBFBD><EFBFBD>D<EFBFBD>ӵ<EFBFBD>
OCOut(OC64_WHLS_ON, 0, 16);
tmpAtt->sModePara.ZKPowOnStep = 0x55;
tmpAtt->sModePara.ZKPowOnFirst = 0x00;
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>TRUE / FALSE
<EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ע<EFBFBD>⣺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲɼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
BOOL AttCtrlInit(UINT8 taskID)
{
UINT8 i = 0x00;
UINT8 j = 0x00;
UINT8 k = 0x00;
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
BOOL result = TRUE;
#ifndef MINMODULE_TYPE
m_pZKTaskInfo = (sTaskInfo *)GetTaskInfoBT(taskID);
if(NULL == m_pZKTaskInfo)
{
return FALSE;
}
#endif
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst= NULL;
SsPara_t *PStar[STAR_NUM]= {NULL};
GyroPara_t *PGyro[GYRO_NUM]= {NULL};
MagPara_t *PMag = NULL;
WhlPara_t *PWhl= NULL;
MtPara_t *pMt= NULL;
sAttErrCtlPara_t *pREcheck= NULL;
sAttCtlPara_t *pCtrl= NULL;
sAttDeterPara_t *pDeter= NULL;
sAttOrbitGetPara_t *pOrbitIn= NULL;
AttIMPTPara_t *pImpt= NULL;
sAttModPara_t *pWMod= NULL;
sAttGuidLawPara_t *pGuide= NULL;
sAttOrbitCtlInfo_t *pOrb= NULL;
sAttOrbitCtlData_t *tmpOrbCmd= NULL;
sAttTARCCtlData_t *tmpTARCmd= NULL;
sAttTARCCtlInfo_t *pTAR= NULL;
PPUPara_t *PPPU= NULL;
AttCmdDsp_t *pCmdDsp= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
tmpTARCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
if(NULL == tmpAtt || NULL ==tmpConst || NULL ==tmpOrbCmd || NULL ==tmpTARCmd)
return FALSE;
memset(tmpAtt, 0, sizeof(sAttPriData));
memset(tmpOrbCmd, 0, sizeof(sAttOrbitCtlData_t));
memset(tmpTARCmd, 0, sizeof(sAttTARCCtlData_t));
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
PMag = &tmpAtt->sPerPara.MagPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
pREcheck = &tmpAtt->sErrCtlPara;
pCtrl = &tmpAtt->sCtlPara;
pMt = &tmpAtt->sPerPara.MtPara;
pDeter = &tmpAtt->sDeterPara;
pOrbitIn = &tmpAtt->sOrbitPara;
pImpt = &tmpAtt->sIMPTPara;
pWMod = &tmpAtt->sModePara;
pGuide = &tmpAtt->sGuidLawPara;
pOrb = &tmpAtt->sOrbitInp;
pTAR = &tmpAtt->sTARCInp;
PPPU = &tmpAtt->sPerPara.PPUPara;
pCmdDsp = &tmpAtt->sCmdDspPara;
/*<2A><><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>*/
ZKModInit();
ZKEnvInit();
ZKDPInit();
ZKREInit();
ZKDYInit();
ZKPosCtlInit();
memcpy(&(tmpConst->AttCmdFlashPara.M_SSA[0][0]), &M_SSA[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_SSB[0][0]), &M_SSB[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_SSC[0][0]), &M_SSC[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_GYRO[0][0][0]), &M_GYROA[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_GYRO[1][0][0]), &M_GYROB[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_GYRO[2][0][0]), &M_GYROC[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSA[0][0]), &M_aSSA[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSB[0][0]), &M_aSSB[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSC[0][0]), &M_aSSC[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSD[0][0]), &M_aSSD[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSE[0][0]), &M_aSSE[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_aSSF[0][0]), &M_aSSF[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_MAG[0]), &M_MAG1[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_MAG[1]), &M_MAG2[0][0], 9*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_Wheel[0][0]), &M_Wheel[0][0], 12*sizeof(TYPE_CAL));
memcpy(&(tmpConst->AttCmdFlashPara.M_MAGCtrl[0][0]), &M_MAGCtrl[0][0], 12*sizeof(TYPE_CAL));
//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD>ֵ(<28><><EFBFBD><EFBFBD>ע<EFBFBD>޸<EFBFBD>)
tmpConst->Allow_Sensor_CThr = 0x55;//0;20240630
tmpConst->SS_SS_CThr = 2.0*ANG2RADIAN;
tmpConst->SS_ASS_CThr = 10.0*ANG2RADIAN;
tmpConst->SS_ASS_CCThr = 10.0*ANG2RADIAN;
tmpConst->SS_GYRO_CThr = 0.2*ANG2RADIAN;
//tmpConst->SS_MAG_CThr = 3*ANG2RADIAN;
tmpConst->AttCmdFlashPara.SS_MAG_CThr = 30*ANG2RADIAN;
//tmpConst->MAG_MAG_CThr = 1000;
tmpConst->MAG_MAG_CThr = 3000; //200t230
tmpConst->GYRO_GYRO_CThr = 0.1*ANG2RADIAN;
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpConst->SAT_J[0][0]=497.35;
tmpConst->SAT_J[0][1]=0.0;
tmpConst->SAT_J[0][2]=0.0;
tmpConst->SAT_J[1][0]=0.0;
tmpConst->SAT_J[1][1]=585.45;
tmpConst->SAT_J[1][2]=0.0;
tmpConst->SAT_J[2][0]=0.0;
tmpConst->SAT_J[2][1]=0.0;
tmpConst->SAT_J[2][2]=154.56;
//ģ<><C4A3>̫<EFBFBD><CCAB>D:
for (i = 0; i < ASS_NUM; ++i)
{
pImpt->AssUseState[i]= POSE_OK; //<2F><><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־
}
tmpConst->Ass_MinFour_limt[0] = 3.5; //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[0] = 12.0; //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MinFour_limt[1] = 3.5; //EFƬģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[1] = 7.0; //EFƬģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
//<2F><><EFBFBD><EFBFBD>
for (i = 0; i < GYRO_NUM; i++)
{
PGyro[i]->Gyro_AvailableFlg=POSE_OK;
for (k = 0; k < 3; k++)
{
PGyro[i]->Gyro_ComCnt[k] = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱȽ<DDB1><C8BD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[i][k] =0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
for (j = 0; j < 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[i][k][j] =0;
}
tmpConst->AttCmdFlashPara.Gyro_Comd_K[i][k][k] =1;
}
memset(PGyro[i]->GyroIn_SpeedPre, 0, 3*sizeof(TYPE_CAL));
memset(PGyro[i]->GyroIn_Speed, 0, 3*sizeof(TYPE_CAL));
}
//<2F><><EFBFBD><EFBFBD>
for (i = 0; i < 3; i++)
{
PStar[i]->ssAvailableFlg=POSE_OK; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD>־
PStar[i]->ssExpose_cnt=0;
memset(PStar[i]->ssIn_Qpre, 0.0, 4*sizeof(TYPE_CAL));
memset(PStar[i]->ssIn_Q, 0.0, 4*sizeof(TYPE_CAL));
}
//<2F><>ǿ<EFBFBD><C7BF>
PMag->Mag_Use = POSE_OK;
PMag->Mag_UsePre = POSE_OK;
pImpt->MagUseState[0] = POSE_OK;
pImpt->MagUseState[1] = POSE_OK;
memset(PMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
memset(PMag->Mag_BbPre,0.0,3*sizeof(TYPE_CAL));
for (i = 0; i < MAG_NUM; i++)
{
//PMag->MagIn_Valid[i]= POSE_OK; //ģ<><C4A3><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF>A/B<><42>Ч<EFBFBD><D0A7>־
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memset(PMag->Mag_Bc_MeaAB[i],0.0,3*sizeof(TYPE_CAL));
memset(PMag->Mag_Bb_MeaAB[i],0.0,3*sizeof(TYPE_CAL));
}
PMag->MagIn_DataSta= 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
for (i = 0; i < WHEEL_NUM; ++i)
{
tmpConst->Whl_J[i] = 0.0064;
pImpt->Whl_Use[i]=POSE_OK;
}
tmpConst->Whl_HCent[0]=0.5 * 0.7;
tmpConst->Whl_HCent[1]=-0.5 * 0.7;
tmpConst->Whl_HCent[2]=-0.732 * 0.7;
tmpConst->Whl_HCent[3]=1.018 * 0.7;
memset(PWhl->Whl_Rate,0,4*sizeof(TYPE_CAL));
memset(PWhl->Whl_RatePre,0,4*sizeof(TYPE_CAL));
memset(PWhl->Whl_Time,0,4*sizeof(UINT32));
memset(PWhl->Whl_TimePRE,0,4*sizeof(UINT32));
pCmdDsp->Wheel_Cmd_Cnt = 0;
pREcheck->WhlUseState= 0x0f;
PWhl->Whl_UseCnt=4;
tmpConst->WheelD_NomSpeed = 2000; //rpm
tmpConst->WheelD_SpeedErrThr = 100; //20230104
tmpConst->WheelD_Default = 0.03;
tmpConst->WheelD_AllDefault = 0.04;
memset(PWhl->Whl_WorkMode, WHL_CMD_CODE_RATE, 4*sizeof(UINT8));
tmpConst->Whl_ID[0]=0x01;
tmpConst->Whl_ID[1]=0x01;
tmpConst->Whl_ID[2]=0x01;
tmpConst->Whl_ID[3]=0x01;
#ifndef MINMODULE_TYPE
//PPU
//tmpAtt->sErrCtlPara.AttPPUOnReq = POSE_NO;
//tmpAtt->sErrCtlPara.AttPPUOffReq = POSE_NO;
pImpt->PPUUseStatus=POSE_OK;
pImpt->PPUSafeStatus=POSE_OK;
PPPU->PPUOUT_Sta = POSE_IN; //PPU <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чԭ<D0A7><D4AD>
PPPU->PPUIn_ReCnt= 0; //PPUң<55><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
PPPU->PPUNoUse_Cnt= 0;
PPPU->PPUIn_ReCntPre= 0; //PPU<50><55>һ<EFBFBD><D2BB>ң<EFBFBD><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (i = 0; i < 50; ++i)
{
PPPU->PPUT_FIR_Start[i] =i;
PPPU->PPUT_FIR_End[i] = i;
}
tmpConst->AttCmdFlashPara.PPUT_FIR_EndLast = 0;
PPPU->PPUT_FIR_Sum = 0;
PPPU->PPUT_FIR_Cnt = 0;
PPPU->PPUT_FIR_CntS = 0;
PPPU->PPUT_FIR_SumCnt = 0;
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO; //<2F><><EFBFBD>ε绰<CEB5><E7BBB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>׼
//tmpConst->AttCmdFlashPara.PPUPVK = 0.1065;
//tmpConst->AttCmdFlashPara.PPUPAK = 0.000605;
//tmpConst->AttCmdFlashPara.PPUPVB = -1.4477;
//tmpConst->AttCmdFlashPara.PPUPAB = -0.0107;
tmpConst->AttCmdFlashPara.PPUPVK = 0.1;
tmpConst->AttCmdFlashPara.PPUPAK = 0.001;
tmpConst->AttCmdFlashPara.PPUPVB = 0.0;
tmpConst->AttCmdFlashPara.PPUPAB = 0.0;
PPPU->PPUIn_FIREOK = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־
PPPU->PPUIn_FIREOKPre = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD>־
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD>־*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_NO; /*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>˳<EFBFBD><CBB3><EFBFBD>־*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>*/
#endif
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpConst->Mag_Bb_CtrlMax=100.0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ž<EFBFBD> 50
tmpConst->Mag_Kp[0]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_Kp[1]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_Kp[2]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_Kd[0]=2.4;
tmpConst->Mag_Kd[1]=2.4;
tmpConst->Mag_Kd[2]=2.4;
tmpConst->Mag_KpFBZK[0]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_KpFBZK[1]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_KpFBZK[2]=0.001; //<2F><><EFBFBD><EFBFBD>΢<EFBFBD><CEA2>ϵ<EFBFBD><CFB5>Ϊ
tmpConst->Mag_KdFBZK[0]=2.4;
tmpConst->Mag_KdFBZK[1]=2.4;
tmpConst->Mag_KdFBZK[2]=2.4;
tmpConst->Mag_WTOSUN =-0.5*ANG2RADIAN;
tmpConst->Mag_FBZKWTOSUN =0.3*ANG2RADIAN;
tmpConst->Mag_ATTTOSUN =30.0 * ANG2RADIAN;
tmpConst->Mag_DampBdotMin=10.0; //<2F>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 10
for (i = 0; i < MAG_NUM; ++i)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.MAG_LK[i][j]=28000.0;
tmpConst->AttCmdFlashPara.MAG_LB[i][j]=-70000.0;
}
}
pMt->MTCtrlTime = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pMt->MTStatus = 0;
tmpConst->MTCtrlTIME=8;
tmpConst->AttCmdFlashPara.Mag_ComRESW[0]=0x00; //<2F><>ǿ<EFBFBD>Ʊ<EFBFBD>ͷ<EFBFBD>ع<EFBFBD>״̬<D7B4><CCAC>
tmpConst->AttCmdFlashPara.Mag_ComRESW[1]=0x00; //<2F><>ǿ<EFBFBD>Ʊ<EFBFBD>ͷ<EFBFBD>ع<EFBFBD>״̬<D7B4><CCAC>
tmpConst->AttCmdFlashPara.Mag_ComRESW[2]=0x00; //<2F><>ǿ<EFBFBD>Ʊ<EFBFBD>ͷ<EFBFBD>ع<EFBFBD>״̬<D7B4><CCAC>
pImpt->MTUseStatus[0]=POSE_OK;
pImpt->MTUseStatus[1]=POSE_OK;
pImpt->MTUseStatus[2]=POSE_OK;
pImpt->MTUseStatus[3]=POSE_OK;
tmpConst->MTOutXYZLimit[0] = 100.0;
tmpConst->MTOutXYZLimit[1] = 100.0;
tmpConst->MTOutXYZLimit[2] = 50.0;
tmpConst->MTOutXYZLimit[3] = 50.0;
//<2F><><EFBFBD><EFBFBD>
pWMod->WModSadaOpen=POSE_NO;
pWMod->MiniDataSet =POSE_OK;
pWMod->WModSadaOpenPre=POSE_NO;
pWMod->ZKSadaOpen=POSE_NO;
//<2F><>ж<EFBFBD><D0B6>
tmpConst->k1_dump = 0.5;
tmpConst->k2_dump = 0.3;
//tmpConst->ku_dump = 400000.0;
tmpConst->ku_dump = 500000.0;
tmpConst->MTAtt_dump = 45.0 *ANG2RADIAN ;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pOrbitIn->OrbitVld=POSE_OK;
memset(pOrbitIn->JPos,0,3*sizeof(TYPE_CAL));
memset(pOrbitIn->JVel,0,3*sizeof(TYPE_CAL));
pOrbitIn->MJC=0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pOrbitIn->MJCLv=POSE_NO;
pOrbitIn->GAST=0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD><CEBA><EFBFBD>ʱ<EFBFBD><CAB1>
//PD<50><44><EFBFBD><EFBFBD>
memset(pDeter->CLPAttAngRat,0,3*sizeof(TYPE_CAL));
memset(pDeter->CLPAttAngRatPre,0,3*sizeof(TYPE_CAL));
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><D8B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memset(tmpAtt->sCtlPara.SumAerrPre, 0, 3*sizeof(TYPE_CAL)); //PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memset(tmpAtt->sCtlPara.WheelCtrlResultPre, 0, 3*sizeof(TYPE_CAL)); //ǰ<>ο<EFBFBD><CEBF><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmpAtt->sCtlPara.PD2PIDCnt = 0;
tmpAtt->sCtlPara.CtrlPD2PID =0;
#ifndef MINMODULE_TYPE
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pOrb->CurOrbCtrlPcak_ID = 0x00;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbDataIn_OK = POSE_NO;
pOrb->OrbDataIn_OKCnt = 0x00;
for (i = 0; i < 50; ++i)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpOrbCmd->OrbCtrlPack[i],0,sizeof(AttOrbCtrlPara_t));
}
memset(&pOrb->OrbCtrlPackCur,0,sizeof(AttOrbCtrlPara_t));
//Ŀ<>궨<EFBFBD><EAB6A8>
pTAR->CurTARCtrlPcak_ID = 0;
pTAR->CurTARCtrlPcak_PreID = 0xFF;
pTAR->TARDataIn_OK = POSE_NO;
pTAR->TARDataIn_OKCnt = 0;
for (i = 0; i < 30; ++i)
{
pTAR->TARCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpTARCmd->TARCtrlPack[i],0,sizeof(sAttTARCCtlPara_t));
}
#endif
//<2F><><EFBFBD>˳<EFBFBD>ʼ<EFBFBD><CABC>
memset(pDeter->Qi,0,4*sizeof(TYPE_CAL));
memset(pDeter->Wi,0,3*sizeof(TYPE_CAL));
memset(pDeter->QiPre,0,4*sizeof(TYPE_CAL));
memset(pDeter->WiPre,0,3*sizeof(TYPE_CAL));
pDeter->Qi_Vld = POSE_NO; //<2F><><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־
pDeter->QiPre_Vld = POSE_NO;
pDeter->PPUVld = POSE_NO;
pDeter->Wi_Vld = POSE_OK; //<2F><><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>Ч<EFBFBD><D0A7>־
tmpConst->GyroINT_TLimt = 2400;
pDeter->GyroINT_Time= 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ɳ<EFBFBD>ʼ<EFBFBD><CABC>
pGuide->CaptureSunCnt=0;
//<2F>˿س<CBBF>ʼ<EFBFBD><CABC>
tmpConst->AccThru_Up[0]= 0;
tmpConst->AccThru_Up[1]= 0;
tmpConst->AccThru_Up[2]= -8.0E-5;
//<2F><>̬ȷ<CCAC><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
tmpAtt->sCtlPara.IsDump = POSE_NO;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pCtrl->MagCtrlResult[0] =0.0;
pCtrl->MagCtrlResult[1] =0.0;
pCtrl->MagCtrlResult[2] =0.0;
pCtrl->MagCtrlXYZZ[0] =0.0;
pCtrl->MagCtrlXYZZ[1] =0.0;
pCtrl->MagCtrlXYZZ[2] =0.0;
pCtrl->MagCtrlXYZZ[3] =0.0;
//tmpAtt->AttData3T2Para.AttCtlBKFlag = POSE_NO;
//<2F><><EFBFBD><EFBFBD>flash<73><68><EFBFBD>ݳ<EFBFBD>ʼ<EFBFBD><CABC>
tmpAtt->ATSFlashPara.SHDepartCntlimt1 =0;
/*<2A><>ȡflash<73><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ATTRestoreFromFlash();
/*<2A><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ATTImportDataBackup();
/*<2A><><EFBFBD><EFBFBD>/<2F>л<EFBFBD>/<2F><>λ <20><><EFBFBD><EFBFBD><EFBFBD>쳣PPU<50>ض<EFBFBD><D8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>Դֻ<D4B4>ص<EFBFBD><D8B5><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;
///*<2A><><EFBFBD><EFBFBD>/<2F>л<EFBFBD>/<2F><>λ <20><><EFBFBD><EFBFBD>ת<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>0*/
//SetWheel(1,0,WHL_CMD_CODE_CURR);
//SetWheel(2,0,WHL_CMD_CODE_CURR);
//SetWheel(3,0,WHL_CMD_CODE_CURR);
//SetWheel(4,0,WHL_CMD_CODE_CURR);
/*<2A><><EFBFBD><EFBFBD>/<2F>л<EFBFBD>/<2F><>λ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>ر<EFBFBD>Ϊ0*/
magnetic_out(0x11, 0, 0, 0);
magnetic_out(0x22, 0, 0, 0);
magnetic_out(0x33, 0, 0, 0);
magnetic_out(0x44, 0, 0, 0);
return result;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿<EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKCmdResolve(void)
{
UINT8 i= 0x00;
UINT8 j= 0x00;
UINT8 k= 0x00;
UINT8 tmpCmd[MOD_REQ_MAXLEN]= {0x00};
UINT8 tmpWhlCmd[MOD_REQ_MAXLEN]= {0x00};
UINT16 tmpCmdCode = 0; //ָ<><D6B8><EFBFBD><EFBFBD>
UINT8 tmpOffset = 0x02; //ָ<><D6B8><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
UINT8 *tmpData= 0x00;
UINT8 tmpDevNO = 0x00;
UINT32 tmpiDataLength= 0x00; /*<2A><><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>*/
UINT32 tmpiDataLength1= 0x00; /*<2A><><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>*/
UINT16 tmpiDataType =0; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
UINT16 tmpiDataCheck=0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
UINT16 tmpiDataCheck1=0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
TYPE_CAL tmpWlimit=0.0;
TYPE_CAL tmpAlimit=0.0;
TYPE_CAL tmpAngm=0.0;
UINT16 tmpTimeLmt=0;
UINT16 tmpTimeLmt1=0;
UINT8 tmpWorkMod= 0x00;
UINT8 tmpWorkModPre= 0x00;
UINT8 tmpCOMDCHOICE= 0x00;
UINT32 SatTime[2]= {0x00,0x00};
TYPE_CAL DETLTime1= 0.0;
TYPE_CAL DETLTime2= 0.0;
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
sTaskMngDataRecord * tmpTaskMngDataRcd = NULL;
sAttPriData *tmpAtt= NULL;
AttCtrlConst_t *tmpConst= NULL;
sAttOrbitCtlData_t *tmpOrbCmd= NULL;
sAttTARCCtlData_t *tmpTARCCmd= NULL;
sAttOrbitCtlInfo_t *pOrb= NULL;
sAttTARCCtlInfo_t *pTARC= NULL;
sAttModPara_t *pWMod= NULL;
AttIMPTPara_t *pIMU= NULL;
PPUPara_t *PPPU= NULL;
sTaskInfo *tmpCollectInfo = (sTaskInfo *)GetTaskInfoBT(TASK_COLLECT);
tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
if((NULL == tmpAtt) || (NULL ==tmpConst) || (NULL ==tmpOrbCmd) || (NULL ==tmpTARCCmd)|| (NULL ==tmpCollectInfo)|| (NULL ==tmpTaskMngDataRcd))
{
return ;
}
pOrb=&tmpAtt->sOrbitInp;
pTARC=&tmpAtt->sTARCInp;
pIMU=&tmpAtt->sIMPTPara;
pWMod=&tmpAtt->sModePara;
PPPU = &tmpAtt->sPerPara.PPUPara;
while(BlockRingGet(&(m_pZKTaskInfo->sCmdBuffer), tmpCmd, TRUE))
{
/*<2A><><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
tmpCmdCode = DoGetInt16From8(tmpCmd + tmpOffset);
m_pZKTaskInfo->iCmdLast = tmpCmdCode;
m_pZKTaskInfo->iCmdRes = ERR_CODE_NOERR;
m_pZKTaskInfo->iCmdCount++;
//<2F><>ȡע<C8A1><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
tmpData=(UINT8*)(tmpCmd + tmpOffset +2);
switch(tmpCmdCode)
{
/****************<2A><>ȫģʽ<C4A3><CABD>ָ<EFBFBD><D6B8>*************************/
case ATT_SAFEMODE_SAFE1://<2F><><EFBFBD>ǰ<EFBFBD>ȫģʽ1
{
pOrb->CurOrbCtrlPcak_ID = 0x00;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
for (i = 0; i < 50; ++i)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpOrbCmd->OrbCtrlPack[i],0,sizeof(AttOrbCtrlPara_t));
}
memset(&pOrb->OrbCtrlPackCur,0,sizeof(AttOrbCtrlPara_t));
pIMU->PPUUseStatus=POSE_NO;
pIMU->PPUSafeStatus=POSE_NO;
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
if ((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode))
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_RATEDMP);
}
break;
}
case ATT_SAFEMODE_SAFE2://<2F><><EFBFBD>ǰ<EFBFBD>ȫģʽ2
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_RATEDMP);
pOrb->CurOrbCtrlPcak_ID = 0x00;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
for (i = 0; i < 50; ++i)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpOrbCmd->OrbCtrlPack[i],0,sizeof(AttOrbCtrlPara_t));
}
memset(&pOrb->OrbCtrlPackCur,0,sizeof(AttOrbCtrlPara_t));
pIMU->PPUUseStatus=POSE_NO;
pIMU->PPUSafeStatus=POSE_NO;
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
break;
}
case ATT_SAFEMODE_SAFE3://<2F><><EFBFBD>ǰ<EFBFBD>ȫģʽ3
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20>ſض<C5BF><D8B6><EFBFBD>*/
ZKModDoSet(ATTMOD_MagTOSUN);
pOrb->CurOrbCtrlPcak_ID = 0x00;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
for (i = 0; i < 50; ++i)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpOrbCmd->OrbCtrlPack[i],0,sizeof(AttOrbCtrlPara_t));
}
memset(&pOrb->OrbCtrlPackCur,0,sizeof(AttOrbCtrlPara_t));
pIMU->PPUUseStatus=POSE_NO;
pIMU->PPUSafeStatus=POSE_NO;
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
break;
}
/**********************************ģʽ<C4A3><CABD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>*********************************/
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0X22
case ATT_CMDMODE_MODSUNCAP:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_RATEDMP);
if(0xAA55 == DoGetInt16From8(tmpData))
{
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><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><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;
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
pTARC->TARDataIn_OK = POSE_NO;
}
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F>ֿض<D6BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>0X33
case ATT_CMDMODE_WhlTOSUN:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20>ֿض<D6BF><D8B6><EFBFBD>*/
ZKModDoSet(ATTMOD_WhlTOSUN);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F>ſض<C5BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>0X44
case ATT_CMDMODE_MagTOSUN:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20>ſض<C5BF><D8B6><EFBFBD>*/
ZKModDoSet(ATTMOD_MagTOSUN);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F>Եض<D4B5><D8B6><EFBFBD>0X55
case ATT_CMDMODE_ONLTOEARTH:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
if((ATTMOD_RATEDMP == pWMod->WorkMode)
||(ATTMOD_WhlTOSUN == pWMod->WorkMode)
||(ATTMOD_MagTOSUN == pWMod->WorkMode)
||(ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)
||(ATTMOD_ORBITCTL == pWMod->WorkMode)
||(ATTMOD_ATTAJUST == pWMod->WorkMode)
||(ATTMOD_ONLTOTAR == pWMod->WorkMode)
)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20>Եض<D4B5><D8B6><EFBFBD>*/
ZKModDoSet(ATTMOD_ONLTOEARTH);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F><>̬<EFBFBD>Ե<EFBFBD>0X66
case ATT_CMDMODE_WHEELTOEARTH:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
if((ATTMOD_ONLTOEARTH == pWMod->WorkMode)
||(ATTMOD_ORBITCTL == pWMod->WorkMode)
||(ATTMOD_ONLTOTAR == pWMod->WorkMode)
)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><>̬<EFBFBD>Ե<EFBFBD>*/
ZKModDoSet(ATTMOD_WHEELTOEARTH);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F>޿<EFBFBD>ģʽ0XBB
case ATT_CMDMODE_UNCTL:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
if((ATTMOD_WhlTOSUN == pWMod->WorkMode)
||(ATTMOD_MagTOSUN == pWMod->WorkMode)
||(ATTMOD_RATEDMP == pWMod->WorkMode)
)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20>޿<EFBFBD>*/
ZKModDoSet(ATTMOD_UNCTL);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_WORKMODE_ORBITCTLPRE: //<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>(0x3606
if((ATTMOD_WhlTOSUN == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD>ֿض<D6BF><D8B6><EFBFBD>*/
||(ATTMOD_WHEELTOEARTH == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>*/
||(ATTMOD_ORBITCTL == pWMod->WorkMode)
)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_ATTAJUSTPRE);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
case ATT_WORKMODE_ORBITCTL : //<2F><><EFBFBD><EFBFBD>(0x3607)
if(ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_ORBITCTL);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
case ATT_WORKMODE_ATTAJUST : /*(0x3611) <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if((ATTMOD_WhlTOSUN == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD>ֿض<D6BF><D8B6><EFBFBD>*/
||(ATTMOD_WHEELTOEARTH == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD>ȶ<EFBFBD><C8B6>Ե<EFBFBD>*/
||(ATTMOD_ONLTOTAR == pWMod->WorkMode)
)
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_ATTAJUST);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
case ATT_WORKMODE_ONLTOTAR : /*(0x3612) Ŀ<>궨<EFBFBD><EAB6A8>*/
if(ATTMOD_ATTAJUST == pWMod->WorkMode)/*<2A><>ǰģʽΪ<CABD><CEAA><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>*/
{
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>*/
ZKModDoSet(ATTMOD_ONLTOTAR);
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>ע
case ATT_CMDMODE_ORBITCTL:
{
tmpData=(UINT8*)(ATTCLT_DATA5_ADDR);
GetTime(SatTime);
memcpy(&(tmpiDataLength), tmpData, 4);
memcpy(&(tmpiDataType), tmpData+ 4, 2);
memcpy(&(tmpiDataCheck), tmpData+ 6, 2);
for (i = 0; i < 50; ++i)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpOrbCmd->OrbCtrlPack[i],0,sizeof(AttOrbCtrlPara_t));
}
tmpiDataLength1 = 1200;
if ( tmpiDataLength1 != tmpiDataLength)
{
pOrb->OrbDataIn_OK = 0x01;
PPPU->PPUT_FIR_Cnt = 0;
return ;
}
tmpiDataCheck1 =DoGetCheckSum(tmpData + sizeof(sSaveDataHead), tmpiDataLength);
if ((0x26==tmpiDataType)&&(tmpiDataCheck == tmpiDataCheck1))
{
pOrb->CurOrbCtrlPcak_ID = 0x00;
pOrb->CurOrbCtrlPcak_PreID = 0xFF ;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = 0x55;
for (i = 0; i < 50; ++i)
{
PPPU->PPUT_FIR_Start[i] =0;
PPPU->PPUT_FIR_End[i] = 0;
}
tmpConst->AttCmdFlashPara.PPUT_FIR_EndLast = 0;
pWMod->ATTFlashSwitch = POSE_OK;
PPPU->PPUT_FIR_Cnt = 0;
PPPU->PPUT_FIR_CntS = 0;
for (i = 0; i < 50; i++)
{
memcpy(&(tmpDevNO), tmpData + i * 24 + sizeof(sSaveDataHead) , sizeof(UINT8));
if ((tmpDevNO>=1)&&(tmpDevNO<=50)&&(i +1==tmpDevNO))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(&(tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrlPcak_ID), (UINT8*)(tmpData + i * 24+ sizeof(sSaveDataHead)), sizeof(UINT8));
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbAttAjustTime=DoGetInt32From8(tmpData + i * 24 + sizeof(sSaveDataHead) +1);
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrAng[0]= DoGetFloatFrom8(tmpData + i * 24 + sizeof(sSaveDataHead) +5);
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrAng[1]= DoGetFloatFrom8(tmpData + i * 24 + sizeof(sSaveDataHead) +9);
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrAng[2]= DoGetFloatFrom8(tmpData + i * 24 + sizeof(sSaveDataHead) +13);
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrl_StartTime=DoGetInt32From8(tmpData + i * 24 + sizeof(sSaveDataHead) +17);
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrl_TimeLength=DoGetInt16From8(tmpData + i * 24 + sizeof(sSaveDataHead) +21);
memcpy(&(tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrl_Mod), (UINT8*)(tmpData + i * 24 + sizeof(sSaveDataHead) +23), sizeof(UINT8));
tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbDateSum =DoGetCheckSum(&(tmpOrbCmd->OrbCtrlPack[tmpDevNO-1].OrbCtrlPcak_ID), 24);
pOrb->OrbCtrlPcak_ReceiveFlg[tmpDevNO-1]=0x55 ;
}
}
if ((tmpOrbCmd->OrbCtrlPack[0].OrbAttAjustTime > SatTime[0])&&(tmpOrbCmd->OrbCtrlPack[0].OrbCtrl_StartTime > SatTime[0]))
{
PPPU->PPUT_FIR_Cnt = 1;
for (i = 1; i < 50; i++)
{
if (0x55 == pOrb->OrbCtrlPcak_ReceiveFlg[i])
{
PPPU->PPUT_FIR_Cnt = i +1;
DETLTime1 = (TYPE_CAL)tmpOrbCmd->OrbCtrlPack[i].OrbAttAjustTime - (TYPE_CAL)tmpOrbCmd->OrbCtrlPack[i -1].OrbCtrl_StartTime - (TYPE_CAL)tmpOrbCmd->OrbCtrlPack[i -1].OrbCtrl_TimeLength;
DETLTime2 = (TYPE_CAL)tmpOrbCmd->OrbCtrlPack[i].OrbCtrl_StartTime - (TYPE_CAL)tmpOrbCmd->OrbCtrlPack[i].OrbAttAjustTime;
if ((DETLTime1 < 0.0)|| (DETLTime2 < 0.0))
{
pOrb->OrbDataIn_OK = 0x02;
PPPU->PPUT_FIR_Cnt = 0;
PPPU->PPUT_FIR_CntS = 0;
break;
}
}
else
{
break;
}
}
}
else
{
PPPU->PPUT_FIR_Cnt = 0;
PPPU->PPUT_FIR_CntS = 0;
pOrb->OrbDataIn_OK = 0x02;
}
if ((PPPU->PPUT_FIR_Cnt >= 1)&&(PPPU->PPUT_FIR_Cnt <= 50)&&(0x55 == pOrb->OrbDataIn_OK))
{
if ((tmpOrbCmd->OrbCtrlPack[PPPU->PPUT_FIR_Cnt-1].OrbCtrl_Mod == 0x55) ||(tmpOrbCmd->OrbCtrlPack[PPPU->PPUT_FIR_Cnt-1].OrbCtrl_Mod == 0x44))
{
pOrb->OrbDataIn_OK = 0x55;
PPPU->PPUT_FIR_CntS =PPPU->PPUT_FIR_Cnt;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
//PPU<50>ػ<EFBFBD>ָ<EFBFBD><D6B8>
dev_can_write(DEV_NO_CAN0, PPUOff, 8);/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹػ<C6B9><D8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ5<D2AA><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD>*/
}
else
{
PPPU->PPUT_FIR_Cnt = 0;
PPPU->PPUT_FIR_CntS = 0;
pOrb->OrbDataIn_OK = 0x04;
}
}
}
else
{
for (i = 0; i < 50; i++)
{
pOrb->OrbCtrlPcak_ReceiveFlg[i]=0x00 ;
}
pOrb->OrbDataIn_OK = 0x01;
PPPU->PPUT_FIR_Cnt = 0;
}
if (POSE_OK == pOrb->OrbDataIn_OK )
{
if(pOrb->OrbDataIn_OKCnt<60000)
{
pOrb->OrbDataIn_OKCnt++;
}
else
{
pOrb->OrbDataIn_OKCnt = 0;
}
}
break;
}
//<2F><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>ע
case ATT_CMDMODE_ONLTOTAR:
{
tmpData=(UINT8*)(ATTCLT_DATA4_ADDR);
GetTime(SatTime);
memcpy(&(tmpiDataLength), tmpData, 4);
memcpy(&(tmpiDataType), tmpData+ 4, 2);
memcpy(&(tmpiDataCheck), tmpData+ 6, 2);
tmpiDataCheck1 =DoGetCheckSum(tmpData + sizeof(sSaveDataHead), tmpiDataLength);
pTARC->TARDataIn_OK = POSE_OK;
for (i = 0; i < 30; ++i)
{
pTARC->TARCtrlPcak_ReceiveFlg[i]= 0x00;
memset(&tmpTARCCmd->TARCtrlPack[i],0,sizeof(sAttTARCCtlPara_t));
}
tmpiDataLength1 = 690;
if ( tmpiDataLength1 != tmpiDataLength)
{
pTARC->TARDataIn_OK = 0x02 ;
return ;
}
if ((0x25==tmpiDataType)&&(tmpiDataCheck == tmpiDataCheck1))
{
pTARC->CurTARCtrlPcak_ID = 0;
pTARC->CurTARCtrlPcak_PreID = 0xFF ;
for (i = 0; i < 30; i++)
{
memcpy(&(tmpDevNO), tmpData + i * 23 + sizeof(sSaveDataHead) , sizeof(UINT8));
if ((tmpDevNO>0)&&(tmpDevNO<=30)&&(i +1==tmpDevNO))
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(&(tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrlPcak_ID), (UINT8*)(tmpData + i * 23+ sizeof(sSaveDataHead)), sizeof(UINT8));
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrl_JustTime= DoGetInt32From8(tmpData + i * 23 + sizeof(sSaveDataHead) +1);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrAng[0]= DoGetFloatFrom8(tmpData + i * 23 + sizeof(sSaveDataHead) +5);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrAng[1]= DoGetFloatFrom8(tmpData + i * 23 + sizeof(sSaveDataHead) +9);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrAng[2]= DoGetFloatFrom8(tmpData + i * 23 + sizeof(sSaveDataHead) +13);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrl_StartTime= DoGetInt32From8(tmpData + i * 23 + sizeof(sSaveDataHead) +17);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrl_TimeLength= DoGetInt16From8(tmpData + i * 23 + sizeof(sSaveDataHead) +21);
tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrlDateSum=DoGetCheckSum(&(tmpTARCCmd->TARCtrlPack[tmpDevNO-1].TARCtrlPcak_ID), 23);
pTARC->TARCtrlPcak_ReceiveFlg[tmpDevNO-1]=0x55 ;
}
}
if ((tmpTARCCmd->TARCtrlPack[0].TARCtrl_JustTime > SatTime[0])&&(tmpTARCCmd->TARCtrlPack[0].TARCtrl_StartTime > SatTime[0]))
{
for (i = 1; i < 30; i++)
{
if (pTARC->TARCtrlPcak_ReceiveFlg[i])
{
DETLTime1 = (TYPE_CAL)tmpTARCCmd->TARCtrlPack[i].TARCtrl_JustTime - (TYPE_CAL)tmpTARCCmd->TARCtrlPack[i -1].TARCtrl_JustTime - (TYPE_CAL)tmpTARCCmd->TARCtrlPack[i -1].TARCtrl_TimeLength;
DETLTime2 = (TYPE_CAL)tmpTARCCmd->TARCtrlPack[i].TARCtrl_StartTime - (TYPE_CAL)tmpTARCCmd->TARCtrlPack[i].TARCtrl_JustTime;
if ((DETLTime1 < 0.0)|| (DETLTime2 < 0.0))
{
pTARC->TARDataIn_OK = 0x01;
break;
}
}
else
{
break;
}
}
}
else
{
pTARC->TARDataIn_OK = 0x01;
}
}
else
{
for (i = 0; i < 30; i++)
{
pTARC->TARCtrlPcak_ReceiveFlg[i]=0x00 ;
}
pTARC->TARDataIn_OK = 0x02 ;
}
if (POSE_OK == pTARC->TARDataIn_OK )
{
if(pTARC->TARDataIn_OKCnt<60000)
{
pTARC->TARDataIn_OKCnt++;
}
else
{
pTARC->TARDataIn_OKCnt = 0;
}
}
break;
}
case ATT_CMDMODE_ORBITCTLREQ: /* <20>غ<EFBFBD><D8BA>첽<EFBFBD><ECB2BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
{
/* ң<><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><C2B7>첽<EFBFBD><ECB2BD> */
((sYaoCePackege*)GETPKGSTRUCT(TM_PKGNO_YB_32))->iCollectTime = 1;//<2F>Զ<EFBFBD><D4B6><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʶ
break;
}
//ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD>
case ATT_CMDMODE_MODECHANG:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO;
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
pWMod->ATTFlashSwitch = POSE_OK;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_CMDMODE_NOMODECHANG:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_OK;
pWMod->ATTFlashSwitch = POSE_OK;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>以У<E4BBA5><D0A3>
case ATT_CMDMODE_AllowCThr:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->Allow_Sensor_CThr=0x55;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_CMDMODE_NOAllowCThr:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->Allow_Sensor_CThr=0x00;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDOBITRESET:
{
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));
ZKModDoSet(ATTMOD_ONLTOEARTH);
pOrb->OrbDataIn_OK = POSE_NO;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><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;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
}
//<2F><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD>
case ATT_CMDATTRESET:
{
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
ZKModDoSet(ATTMOD_RATEDMP);
pTARC->TARDataIn_OK = POSE_NO;
break;
}
//<2F><>̬<EFBFBD><CCAC>׼<EFBFBD><D7BC>ʧ
case ATT_CMDMODE_ATTLOSE:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttLoseDiagFlg = 0x55;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_CMDMODE_NOATTLOSE:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttLoseDiagFlg = 0;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>
case ATT_CMDMODE_ATTEx:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttExDiagFlg = 0x55;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_CMDMODE_NOATTEx:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttExDiagFlg = 0;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD>
case ATT_CMDMODE_AttUnSta:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttStaDiagFlg = 0x55;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
case ATT_CMDMODE_NOAttUnSta:
{
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
tmpConst->AllowAttStaDiagFlg = 0;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ز<EFBFBD><D8B2><EFBFBD>
case ATT_CMDCODE_MODPARAM:
{
tmpWorkModPre = *tmpData;
tmpWorkMod = *(tmpData + 1);
tmpWlimit = DoGetFloatFrom8(tmpData+2)*ANG2RADIAN;
tmpAlimit = DoGetFloatFrom8(tmpData+6) *ANG2RADIAN;
tmpTimeLmt = DoGetInt16From8(tmpData+10);
tmpTimeLmt1 = DoGetInt16From8(tmpData+12);
tmpAngm = DoGetFloatFrom8(tmpData+14);
if((tmpWorkMod & 0x0F) > 0x0B)
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
break;
}
if(ATTMOD_NOCOLL == tmpWorkModPre) //1<><31><EFBFBD><EFBFBD>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>ģʽ
{
tmpConst->judge_time1=tmpTimeLmt;
}
else if(ATTMOD_RATEDMP == tmpWorkModPre)
{
tmpConst->judge_time2 = tmpTimeLmt;
tmpConst->RateThr_2T3 =tmpWlimit;
tmpConst->HWHLThr_2T3 = tmpAngm;
}
else if(ATTMOD_WhlTOSUN == tmpWorkModPre)
{
tmpConst->judge_time3 = tmpTimeLmt;
tmpConst->AttCmdFlashPara.judgeWait_time = tmpTimeLmt1;
tmpConst->AngThr_3TB = tmpAlimit;
tmpConst->RateThr_3TB = tmpWlimit;
pWMod->ATTFlashSwitch = POSE_OK;
}
else if(ATTMOD_MagTOSUN == tmpWorkModPre)
{
tmpConst->judge_time4 = tmpTimeLmt;
tmpConst->AngThr_3TB = tmpAlimit;
tmpConst->RateThr_3TB = tmpWlimit;
}
else if(ATTMOD_ONLTOEARTH == tmpWorkModPre)
{
tmpConst->judge_time5 = tmpTimeLmt;
tmpConst->AngThr_5T6 = tmpAlimit;
tmpConst->RateThr_5T6 = tmpWlimit;
}
else if(ATTMOD_WHEELTOEARTH == tmpWorkModPre)
{
tmpConst->judge_time6 = tmpTimeLmt;
}
else if(ATTMOD_ATTAJUSTPRE == tmpWorkModPre)
{
tmpConst->judge_time7 = tmpTimeLmt;
}
else if(ATTMOD_ORBITCTL == tmpWorkModPre)
{
tmpConst->judge_time8 = tmpTimeLmt;
}
else if(ATTMOD_ATTAJUST == tmpWorkModPre)
{
tmpConst->judge_time9 = tmpTimeLmt;
tmpConst->AngThr_9TA = tmpAlimit;
tmpConst->RateThr_9TA = tmpWlimit;
}
else if(ATTMOD_ONLTOTAR == tmpWorkModPre)
{
tmpConst->judge_timeA = tmpTimeLmt;
}
else if(ATTMOD_UNCTL == tmpWorkModPre)
{
tmpConst->judge_timeB = tmpTimeLmt;
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE; /*ָ<><D6B8>ʧ<EFBFBD><CAA7>*/
}
break;
}
//̫<><CCAB>
case ATT_CMDCODE_ASS:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
tmpConst->Ass_MinFour_limt[0] = DoGetFloatFrom8(tmpData+1 +0); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[0] = DoGetFloatFrom8(tmpData+1 +4); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
break;
}
case 0x22:
{
tmpConst->Ass_MinFour_limt[1] = DoGetFloatFrom8(tmpData+1 +0); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[1] = DoGetFloatFrom8(tmpData+1 +4); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
break;
}
default:
break;
}
break;
}
//<2F><>У<EFBFBD><D0A3><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ע
case ATT_CMDCROSSCHECKTHR:
{
tmpConst->SS_SS_CThr = DoGetFloatFrom8(tmpData)*ANG2RADIAN;
tmpConst->SS_ASS_CCThr = DoGetFloatFrom8(tmpData + 4)*ANG2RADIAN;
tmpConst->SS_ASS_CThr = DoGetFloatFrom8(tmpData + 8)*ANG2RADIAN;
tmpConst->AttCmdFlashPara.SS_MAG_CThr = DoGetFloatFrom8(tmpData + 12)*ANG2RADIAN;
tmpConst->SS_GYRO_CThr = DoGetFloatFrom8(tmpData + 16)*ANG2RADIAN;
tmpConst->GYRO_GYRO_CThr = DoGetFloatFrom8(tmpData + 20)*ANG2RADIAN;
tmpConst->MAG_MAG_CThr = DoGetFloatFrom8(tmpData + 24);
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>쳣״̬
case ATT_CMDCROSSERR:
{
tmpConst->AllowssONOFF[0] = *(tmpData );
tmpConst->AllowssONOFF[1] = *(tmpData +1);
tmpConst->AllowssONOFF[2] = *(tmpData +2);
tmpConst->AllowMagONOFF[0] = *(tmpData+3);
tmpConst->AllowMagONOFF[1] = *(tmpData +4);
tmpConst->AllowGyroONOFF[0] = *(tmpData +5);
tmpConst->AllowGyroONOFF[1] = *(tmpData +6);
tmpConst->AllowGyroONOFF[2] = *(tmpData +7);
tmpConst->AllowWhlONOFF[0] = *(tmpData +8);
tmpConst->AllowWhlONOFF[1] = *(tmpData +9);
tmpConst->AllowWhlONOFF[2] = *(tmpData +10);
tmpConst->AllowWhlONOFF[3] = *(tmpData +11);
tmpConst->AllowPPUONOFF = *(tmpData +12);
break;
}
//<2F><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>״̬
case ATT_CMDCROSSUSE:
{
pIMU->SSUseState[0] = *(tmpData);
pIMU->SSUseState[1] = *(tmpData + 1);
pIMU->SSUseState[2] = *(tmpData + 2);
pIMU->AssUseState[0] = *(tmpData + 3);
pIMU->AssUseState[1] = *(tmpData + 4);
pIMU->AssUseState[2] = *(tmpData + 5);
pIMU->AssUseState[3] = *(tmpData + 6);
pIMU->AssUseState[4] = *(tmpData + 7);
pIMU->AssUseState[5] = *(tmpData + 8);
pIMU->MagUseState[0] = *(tmpData + 9);
pIMU->MagUseState[1] = *(tmpData + 10);
pIMU->GyroUseState[0] = *(tmpData + 11);
pIMU->GyroUseState[1] = *(tmpData + 12);
pIMU->GyroUseState[2] = *(tmpData + 13);
pIMU->Whl_Use[0] = *(tmpData + 14);
pIMU->Whl_Use[1] = *(tmpData + 15);
pIMU->Whl_Use[2] = *(tmpData + 16);
pIMU->Whl_Use[3] = *(tmpData + 17);
pIMU->MTUseStatus[0] = *(tmpData + 18);
pIMU->MTUseStatus[1] = *(tmpData + 19);
pIMU->MTUseStatus[2] = *(tmpData + 20);
pIMU->MTUseStatus[3] = *(tmpData + 21);
pIMU->PPUUseStatus= *(tmpData + 22);
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ع<EFBFBD>״̬ע<CCAC><D7A2>
case ATT_CMDCROSS:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
if (*(tmpData+0) != *(tmpData +1))
{
if (*(tmpData+0) != *(tmpData +2))
{
tmpConst->SSUsePrior[0]= *(tmpData) ;
tmpConst->SSUsePrior[1]= *(tmpData +1) ;
tmpConst->SSUsePrior[2]= *(tmpData +2) ;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
if (*(tmpData+3) != *(tmpData +4))
{
if (*(tmpData+3) != *(tmpData +5))
{
tmpConst->GyroUsePrior[0]= *(tmpData +3) ;
tmpConst->GyroUsePrior[1]= *(tmpData +4) ;
tmpConst->GyroUsePrior[2]= *(tmpData +5) ;
}
}
break;
}
case ATT_CMDCREUSE:
{
tmpConst->AttCmdFlashPara.Mag_ComRESW[0] = *(tmpData);
tmpConst->AttCmdFlashPara.Mag_ComRESW[1] = *(tmpData+1);
tmpConst->AttCmdFlashPara.Mag_ComRESW[2] = *(tmpData+2);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[0] = *(tmpData+3);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[1] = *(tmpData+4);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[2] = *(tmpData+5);
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDCROSSDIG:
{
tmpConst->ssDiagThr = DoGetInt16From8(tmpData) ;
tmpConst->MagDiagThr= DoGetInt16From8(tmpData +2) ;
tmpConst->GyroDiagThr= DoGetInt16From8(tmpData +4) ;
tmpConst->WhlDiag_ErrThr= DoGetInt16From8(tmpData +6) ; //<2F>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ,<2C><>λ,<2C><><EFBFBD><EFBFBD>ע
tmpConst->WhlDiag_TimeThr= DoGetInt16From8(tmpData +8) ; //<2F>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ,<2C><>λ,<2C><><EFBFBD><EFBFBD>ע
tmpConst->WhlTemperDiagThr= DoGetInt16From8(tmpData +10) ; //<2F>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ,<2C><>λ,<2C><><EFBFBD><EFBFBD>ע
break;
}
//PPU<50><55><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDCPPUDIG:
{
tmpConst->PPUTXTimeLimit=DoGetInt16From8(tmpData ); //<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD>Ź<EFBFBD><C5B9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ30
tmpConst->PPUTATimeLimit=DoGetInt16From8(tmpData+2); //<2F><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ60
tmpConst->AttCmdFlashPara.PPUTALimit=DoGetInt16From8(tmpData+4); //<2F><><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ 1.6
tmpConst->PPUTVTimeLimit=DoGetInt16From8(tmpData+6); //<2F><><EFBFBD>ƹ<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ60
tmpConst->AttCmdFlashPara.PPUTVLimit=DoGetInt16From8(tmpData+8); //<2F><><EFBFBD>ƹ<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ260
tmpConst->AttCmdFlashPara.PPUTFireLimit=DoGetInt16From8(tmpData+10); //<2F><><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ300
tmpConst->PPUTemperTimeLimit=DoGetInt16From8(tmpData+12); //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֵ 10
tmpConst->AttCmdFlashPara.PPUHTemperLimit=DoGetInt16From8(tmpData+14); //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ 30
tmpConst->AttCmdFlashPara.PPULTemperLimit=DoGetInt16From8(tmpData+16); //<2F><><EFBFBD><EFBFBD><EFBFBD>¶ȹ<C2B6><C8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ 10
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
case ATT_CMDCDIGTIME:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
tmpConst->Ss_Delay_CntLimt =DoGetInt16From8(tmpData + 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_OK_CntLimt= DoGetInt16From8(tmpData+2+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_Err_CntLimt= DoGetInt16From8(tmpData+4+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Ss_ReSet_CntLimt=DoGetInt16From8(tmpData+6+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Ss_ReSetGAP_CntLimt=DoGetInt16From8(tmpData+8+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
case 0x22 :
{
tmpConst->Mag_Delay_CntLimt =DoGetInt16From8(tmpData+ 1) ; //<2F><>ǿ<EFBFBD>ƿ<EFBFBD><C6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_OK_CntLimt= DoGetInt16From8(tmpData+2+ 1) ; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_Err_CntLimt= DoGetInt16From8(tmpData+4+ 1) ; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Mag_ReSet_CntLimt=DoGetInt16From8(tmpData+6+ 1) ; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Mag_ReSetGAP_CntLimt=DoGetInt16From8(tmpData+8+ 1) ; //<2F><>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
case 0x33 :
{
tmpConst->Gyro_Delay_CntLimt =DoGetInt16From8(tmpData+ 1) ; //<2F><><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_OK_CntLimt= DoGetInt16From8(tmpData+2+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_Err_CntLimt= DoGetInt16From8(tmpData+4+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Gyro_ReSet_CntLimt=DoGetInt16From8(tmpData+6+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Gyro_ReSetGAP_CntLimt=DoGetInt16From8(tmpData+8+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
case 0x44 :
{
tmpConst->Whl_Delay_CntLimt =DoGetInt16From8(tmpData+ 1) ; //<2F><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_OK_CntLimt= DoGetInt16From8(tmpData+2+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_Err_CntLimt= DoGetInt16From8(tmpData+4+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->Whl_ReSet_CntLimt=DoGetInt16From8(tmpData+6+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->Whl_ReSetGAP_CntLimt=DoGetInt16From8(tmpData+8+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
case 0x55 :
{
tmpConst->PPU_Delay_CntLimt =DoGetInt16From8(tmpData+ 1) ; //<2F><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD>쳣<EFBFBD>ӳ<EFBFBD><D3B3>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_OK_CntLimt= DoGetInt16From8(tmpData+2+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_Err_CntLimt= DoGetInt16From8(tmpData+4+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD>жϼ<D0B6><CFBC><EFBFBD>
tmpConst->PPU_ReSet_CntLimt=DoGetInt16From8(tmpData+6+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><DCB4><EFBFBD>
tmpConst->PPU_ReSetGAP_CntLimt=DoGetInt16From8(tmpData+8+ 1) ; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
default:
break;
}
break;
}
//<2F><>׼<EFBFBD><D7BC>ʧ<EFBFBD><CAA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDCDIGLOSE:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case ATTMOD_RATEDMP :
{
tmpConst->TimeThr_AttLoseDiag[0] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_WhlTOSUN:
{
tmpConst->TimeThr_AttLoseDiag[1] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_MagTOSUN :
{
tmpConst->TimeThr_AttLoseDiag[2] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_ONLTOEARTH :
{
tmpConst->TimeThr_AttLoseDiag[3] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_WHEELTOEARTH :
{
tmpConst->TimeThr_AttLoseDiag[4] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_ATTAJUSTPRE :
{
tmpConst->TimeThr_AttLoseDiag[5] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_ORBITCTL :
{
tmpConst->TimeThr_AttLoseDiag[6] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_ATTAJUST :
{
tmpConst->TimeThr_AttLoseDiag[7] = DoGetInt16From8(tmpData+1 );
break;
}
case ATTMOD_ONLTOTAR :
{
tmpConst->TimeThr_AttLoseDiag[8] = DoGetInt16From8(tmpData+1 );
break;
}
default:
break;
}
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDCDIGEX:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case ATTMOD_RATEDMP :
{
tmpConst->TimeThr_AttEx[0] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[0] = DoGetFloatFrom8(tmpData+1 +2) *ANG2RADIAN;
tmpConst->AngThr_AttEx[0] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_WhlTOSUN:
{
tmpConst->TimeThr_AttEx[1] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[1] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[1] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_MagTOSUN :
{
tmpConst->TimeThr_AttEx[2] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[2] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[2] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_ONLTOEARTH :
{
tmpConst->TimeThr_AttEx[3] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[3] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[3] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_WHEELTOEARTH :
{
tmpConst->TimeThr_AttEx[4] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[4] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[4] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
tmpConst->YawCntLimit = DoGetInt16From8(tmpData+1+10 );
break;
}
case ATTMOD_ATTAJUSTPRE :
{
tmpConst->TimeThr_AttEx[5] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[5] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[5] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_ORBITCTL :
{
tmpConst->TimeThr_AttEx[6] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[6] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[6] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_ATTAJUST :
{
tmpConst->TimeThr_AttEx[7] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[7] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[7] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
case ATTMOD_ONLTOTAR :
{
tmpConst->TimeThr_AttEx[8] = DoGetInt16From8(tmpData+1 );
tmpConst->RateThr_AttEx[8] = DoGetFloatFrom8(tmpData+1 +2)*ANG2RADIAN;
tmpConst->AngThr_AttEx[8] = DoGetFloatFrom8(tmpData+1 +6)*ANG2RADIAN;
break;
}
default:
break;
}
break;
}
//<2F><>̬<EFBFBD><CCAC><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATT_CMDCDIGNOSTATE:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case ATTMOD_RATEDMP :
{
tmpConst->TimeThr_AttUnSta[0] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[0] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[0] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[0] = DoGetFloatFrom8(tmpData+1 +4 +4)*ANG2RADIAN;
break;
}
case ATTMOD_WhlTOSUN:
{
tmpConst->TimeThr_AttUnSta[1] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[1] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[1] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[1] = DoGetFloatFrom8(tmpData+1 +4 +4)*ANG2RADIAN;
break;
}
case ATTMOD_MagTOSUN :
{
tmpConst->TimeThr_AttUnSta[2] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[2] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[2] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[2] = DoGetFloatFrom8(tmpData+1 +4 +4)*ANG2RADIAN;
break;
}
case ATTMOD_ONLTOEARTH :
{
tmpConst->TimeThr_AttUnSta[3] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[3] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[3] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[3] = DoGetFloatFrom8(tmpData+1 +4 +4)*ANG2RADIAN;
break;
}
case ATTMOD_ATTAJUSTPRE :
{
tmpConst->TimeThr_AttUnSta[4] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[4] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[4] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[4] = DoGetFloatFrom8(tmpData+1 +4+4)*ANG2RADIAN;
break;
}
case ATTMOD_ATTAJUST :
{
tmpConst->TimeThr_AttUnSta[5] = DoGetInt16From8(tmpData+1 );
tmpConst->TimeMod_AttUnSta[5] = DoGetInt16From8(tmpData+1 +2 );
tmpConst->RateThr_AttUnSta[5] = DoGetFloatFrom8(tmpData+1 +2 +2)*ANG2RADIAN;
tmpConst->AngThr_AttUnSta[5] = DoGetFloatFrom8(tmpData+1 +4 +4)*ANG2RADIAN;
break;
}
default:
break;
}
break;
}
//<2F><>ƽ<EFBFBD><C6BD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
case ATT_CMDWHLSLIMT:
{
tmpConst->WheelD_NomSpeed = DoGetFloatFrom8(tmpData); //rpm
tmpConst->WheelD_SpeedErrThr = DoGetFloatFrom8(tmpData+4);
tmpConst->WheelD_Default = DoGetFloatFrom8(tmpData+8);
tmpConst->WheelD_AllDefault = DoGetFloatFrom8(tmpData+12);
break;
}
/**********************************<2A><>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>*********************************/
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_SSMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSA[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSB[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSC[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//̫<><CCAB><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_ASSMAT:
{
tmpDevNO = *tmpData; //̫<><CCAB>ѡ<EFBFBD><D1A1>
if(0x11 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSA[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x22 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSB[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x33 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSC[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x44 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSD[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x55 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSE[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x66 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSF[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><>ǿ<EFBFBD>ư<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MAGMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for(k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_MAG[0][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for(k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_MAG[1][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ݰ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_GyroMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[0][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[1][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[2][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ְ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_WHLMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][0] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][1] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][2] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x88 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][3] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MTMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][0] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x22 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][1] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x44 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][2] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x88 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][3] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_DMPPARA:
{
tmpConst->Mag_Bb_CtrlMax = DoGetFloatFrom8(tmpData);
tmpConst->Mag_DampBdotMin = DoGetFloatFrom8(tmpData+4);
tmpConst->MTCtrlTIME= DoGetInt16From8(tmpData+ 8);
break;
}
//<2F>ſض<C5BF><D8B6>տ<EFBFBD><D5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_TOSUNPARA:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x55 :
{
for (i = 0; i < 3; i++)
{
tmpConst->Mag_Kp[i] = DoGetFloatFrom8(tmpData+i*4 +1);
}
for (i = 0; i < 3; i++)
{
tmpConst->Mag_Kd[i] = DoGetFloatFrom8(tmpData+i*4+12+1);
}
tmpConst->Mag_WTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 +1) *ANG2RADIAN ;
break;
}
case 0xAA :
{
for (i = 0; i < 3; i++)
{
tmpConst->Mag_KpFBZK[i] = DoGetFloatFrom8(tmpData+i*4+1);
}
for (i = 0; i < 3; i++)
{
tmpConst->Mag_KdFBZK[i] = DoGetFloatFrom8(tmpData+i*4+ 12+1);
}
tmpConst->Mag_FBZKWTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 +1) *ANG2RADIAN ;
break;
}
default:
break;
}
tmpConst->Mag_ATTTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 + 4+1) *ANG2RADIAN ;
break;
}
//<2F><>ж<EFBFBD>ؿ<EFBFBD><D8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MTCTRLPARA:
{
tmpConst->k1_dump = DoGetFloatFrom8(tmpData);
tmpConst->k2_dump = DoGetFloatFrom8(tmpData+4);
tmpConst->MTAtt_dump = DoGetFloatFrom8(tmpData+8) *ANG2RADIAN ;
tmpConst->ku_dump = DoGetFloatFrom8(tmpData+12);
break;
}
//<2F><><EFBFBD><EFBFBD>PD<50><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_PDCTRLPARA:
{
tmpWorkMod =*tmpData;
if (tmpWorkMod == 0xAA)
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.kp_ToSun_PD[i] = DoGetFloatFrom8(tmpData+1+i*4);
tmpConst->AttCmdFlashPara.kd_ToSun_PD[i] = DoGetFloatFrom8(tmpData+13+i*4);
}
tmpConst->AttCmdFlashPara.w0max_PD = DoGetFloatFrom8(tmpData+13+12) * ANG2RADIAN;
tmpConst->AttCmdFlashPara.PD_Ts = DoGetFloatFrom8(tmpData+13+12 +4);
}
else if (tmpWorkMod == 0x55)
{
for(i=0; i<3; i++)
{
tmpConst->kp_NoToSun_PD[i] = DoGetFloatFrom8(tmpData+1+i*4);
tmpConst->kd_NoToSun_PD[i] = DoGetFloatFrom8(tmpData+13+i*4);
}
tmpConst->w0max_NoPD = DoGetFloatFrom8(tmpData+13+12) * ANG2RADIAN;
tmpConst->PDNo_Ts = DoGetFloatFrom8(tmpData+13+12 +4);
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_PIDCTRLPARA:
{
//tmpData = tmpCmd + tmpOffset;
tmpWorkMod =*tmpData;
if (tmpWorkMod == 0xAA)
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.kp_PID[i] = DoGetFloatFrom8(tmpData+i*4 +1);
tmpConst->AttCmdFlashPara.ki_PID[i] = DoGetFloatFrom8(tmpData+12+i*4+1);
tmpConst->AttCmdFlashPara.kd_PID[i] = DoGetFloatFrom8(tmpData+24+i*4+1);
}
tmpConst->AttCmdFlashPara.w0max_PID = DoGetFloatFrom8(tmpData+36+1) * ANG2RADIAN;
tmpConst->AttCmdFlashPara.PID_Ts = DoGetFloatFrom8(tmpData+40+1);
pWMod->ATTFlashSwitch = POSE_OK;
}
else if (tmpWorkMod == 0x55)
{
for(i=0; i<3; i++)
{
tmpConst->kp_NoPID[i] = DoGetFloatFrom8(tmpData+i*4 +1);
tmpConst->ki_NoPID[i] = DoGetFloatFrom8(tmpData+12+i*4+1);
tmpConst->kd_NoPID[i] = DoGetFloatFrom8(tmpData+24+i*4+1);
}
tmpConst->w0max_NoPID = DoGetFloatFrom8(tmpData+36+1) * ANG2RADIAN;
tmpConst->PIDNo_Ts = DoGetFloatFrom8(tmpData+40+1);
}
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫע<C6AB><D7A2>
case ATT_CMDCODE_GyroBIAS:
{
tmpWorkMod =*tmpData;
if (0x11==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[0][k] =DoGetFloatFrom8(tmpData+k*4 +1) * ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
else if (0x22==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[1][k] =DoGetFloatFrom8(tmpData+k*4 +1) * ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
else if (0x44==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[2][k] =DoGetFloatFrom8(tmpData+k*4 +1) * ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_GyroPH:
{
tmpWorkMod =*tmpData;
if (0x11==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[0][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
else if (0x22==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[1][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
else if (0x44==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[2][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD>Ӱ<EFBFBD>ж<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_BetaAng:
{
tmpConst->BetaAngLimit = DoGetFloatFrom8(tmpData) *ANG2RADIAN ;
break;
}
//<2F><><EFBFBD><EFBFBD>IDѡ<44><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_WhlID:
{
memset(tmpWhlCmd,0x00,5*sizeof(UINT8));
tmpWorkMod =*tmpData;
if (0x11==tmpWorkMod)
{
tmpConst->Whl_ID[0] =*(tmpData +1);
tmpWhlCmd[0] = 0x00;
tmpWhlCmd[1] = 0x03;
tmpWhlCmd[2] = tmpWorkMod ;
tmpWhlCmd[3] = tmpWorkMod ;
tmpWhlCmd[4] =tmpConst->Whl_ID[0];
}
else if (0x22==tmpWorkMod)
{
tmpConst->Whl_ID[1] =*(tmpData +1);
tmpWhlCmd[0] = 0x00;
tmpWhlCmd[1] = 0x03;
tmpWhlCmd[2] = tmpWorkMod ;
tmpWhlCmd[3] = tmpWorkMod ;
tmpWhlCmd[4] =tmpConst->Whl_ID[1];
}
else if (0x33==tmpWorkMod)
{
tmpConst->Whl_ID[2] =*(tmpData +1);
tmpWhlCmd[0] = 0x00;
tmpWhlCmd[1] = 0x03;
tmpWhlCmd[2] = tmpWorkMod ;
tmpWhlCmd[3] = tmpWorkMod ;
tmpWhlCmd[4] =tmpConst->Whl_ID[2];
}
else if (0x44==tmpWorkMod)
{
tmpConst->Whl_ID[3] =*(tmpData +1);
tmpWhlCmd[0] = 0x00;
tmpWhlCmd[1] = 0x03;
tmpWhlCmd[2] = tmpWorkMod ;
tmpWhlCmd[3] = tmpWorkMod ;
tmpWhlCmd[4] =tmpConst->Whl_ID[3];
}
BlockRingAdd(&tmpCollectInfo->sCmdBuffer,tmpWhlCmd,TRUE);
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D>ž<EFBFBD><C5BE><EFBFBD>ֵ
case ATT_CMDCODE_MTCTRL:
{
tmpConst->MTOutXYZLimit[0] = DoGetFloatFrom8(tmpData);
tmpConst->MTOutXYZLimit[1] = DoGetFloatFrom8(tmpData +4);
tmpConst->MTOutXYZLimit[2] = DoGetFloatFrom8(tmpData +8);
tmpConst->MTOutXYZLimit[3] = DoGetFloatFrom8(tmpData +12);
break;
}
//<2F><>ǿ<EFBFBD>Ʊ궨ϵ<EAB6A8><CFB5>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MTBD:
{
memset(&tmpCOMDCHOICE,0x00,sizeof(UINT8));
tmpCOMDCHOICE =*tmpData;
if (0x11==tmpCOMDCHOICE)
{
for (i = 0; i< 3; i++)
{
tmpConst->AttCmdFlashPara.MAG_LK[0][i]=DoGetFloatFrom8(tmpData+i*4+1);
tmpConst->AttCmdFlashPara.MAG_LB[0][i]=DoGetFloatFrom8(tmpData+12+i*4+1);
}
}
else if (0x22==tmpCOMDCHOICE)
{
for (i = 0; i< 3; i++)
{
tmpConst->AttCmdFlashPara.MAG_LK[1][i]=DoGetFloatFrom8(tmpData+i*4+1);
tmpConst->AttCmdFlashPara.MAG_LB[1][i]=DoGetFloatFrom8(tmpData+12+i*4+1);
}
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ٶ<EFBFBD>
case ATT_CMDMODE_PPUAB:
{
for (i = 0; i < 3; i++)
{
tmpConst->AccThru_Up[i] = DoGetFloatFrom8(tmpData+i*4) * 0.001f;
}
break;
}
case ATT_CMDCODE_PPUBD:
{
tmpConst->AttCmdFlashPara.PPUPVK = DoGetFloatFrom8(tmpData);
tmpConst->AttCmdFlashPara.PPUPVB = DoGetFloatFrom8(tmpData +4);
tmpConst->AttCmdFlashPara.PPUPAK = DoGetFloatFrom8(tmpData +8);
tmpConst->AttCmdFlashPara.PPUPAB = DoGetFloatFrom8(tmpData +12);
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><>̬ȷ<CCAC><C8B7>
case ATT_CMDCODE_GyroINTTLimt:
{
tmpConst->GyroINT_TLimt = DoGetInt16From8(tmpData);
break;
}
//<2F><>ȫģʽPPU<50><55><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>״̬
case ATT_CMDCODE_SafePPU:
{
pIMU->PPUSafeStatus= *(tmpData);
break;
}
//<2F><>ȫģʽPPU<50><55><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>״̬
case ATT_CMDCODE_WHLCENT:
{
tmpConst->Whl_HCent[0] = DoGetFloatFrom8(tmpData);
tmpConst->Whl_HCent[1] = DoGetFloatFrom8(tmpData +4);
tmpConst->Whl_HCent[2] = DoGetFloatFrom8(tmpData +8);
tmpConst->Whl_HCent[3] = DoGetFloatFrom8(tmpData +12);
break;
}
default:
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
break;
}
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿<EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKMINICmdResolve(UINT8 *tmpCmd)
{
#ifdef MINMODULE_TYPE
UINT8 i= 0x00;
UINT8 j= 0x00;
UINT8 k= 0x00;
//UINT8 tmpCmd[MOD_REQ_MAXLEN]= {0x00};
//UINT8 tmpWhlCmd[MOD_REQ_MAXLEN]= {0x00};
UINT16 tmpCmdCode = 0; //ָ<><D6B8><EFBFBD><EFBFBD>
UINT8 tmpOffset = 0x02; //ָ<><D6B8><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
UINT8 *tmpData= 0x00;
UINT8 tmpDevNO = 0x00;
UINT32 tmpiDataLength= 0x00; /*<2A><><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>*/
UINT16 tmpiDataType =0; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
UINT16 tmpiDataCheck=0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
UINT16 tmpiDataCheck1=0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
TYPE_CAL tmpWlimit=0.0;
TYPE_CAL tmpAlimit=0.0;
TYPE_CAL tmpAngm=0.0;
UINT16 tmpTimeLmt=0;
UINT16 tmpTimeLmt1=0;
UINT8 tmpWorkMod= 0x00;
UINT8 tmpWorkModPre= 0x00;
UINT8 tmpCOMDCHOICE= 0x00;
sTaskMngDataRecord * tmpTaskMngDataRcd = NULL;
sAttPriData *tmpAtt= NULL;
AttCtrlConst_t *tmpConst= NULL;
sAttOrbitCtlData_t *tmpOrbCmd= NULL;
sAttTARCCtlData_t *tmpTARCCmd= NULL;
sAttOrbitCtlInfo_t *pOrb= NULL;
sAttTARCCtlInfo_t *pTARC= NULL;
sAttModPara_t *pWMod= NULL;
sAttErrCtlPara_t *pREcheck= NULL;
AttIMPTPara_t *pIMU= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
if((NULL == tmpAtt) || (NULL ==tmpConst) || (NULL ==tmpOrbCmd) || (NULL ==tmpTARCCmd)|| (NULL ==tmpTaskMngDataRcd))
{
return ;
}
pOrb=&tmpAtt->sOrbitInp;
pTARC=&tmpAtt->sTARCInp;
pREcheck=&tmpAtt->sErrCtlPara;
pIMU=&tmpAtt->sIMPTPara;
pWMod=&tmpAtt->sModePara;
/*<2A><><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
tmpCmdCode = DoGetInt16From8(tmpCmd + tmpOffset);
m_pZKTaskInfo->iCmdLast = tmpCmdCode;
m_pZKTaskInfo->iCmdRes = ERR_CODE_NOERR;
m_pZKTaskInfo->iCmdCount++;
//<2F><>ȡע<C8A1><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
tmpData=(UINT8*)(tmpCmd + tmpOffset +2);
switch(tmpCmdCode)
{
/**********************************ģʽ<C4A3><CABD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>*********************************/
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0X22
case ATT_CMDMODE_MODSUNCAP:
{
//tmpConst->WorkModeUp = ATTMOD_RATEDMP;
//tmpConst->WorkModeChange=0x55;
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_RATEDMP);
break;
}
//<2F>ſض<C5BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>0X44
case ATT_CMDMODE_MagTOSUN:
{
//tmpConst->WorkModeUp = ATTMOD_MagTOSUN;
//tmpConst->WorkModeChange=0x55;
/*<2A><><EFBFBD>ù<EFBFBD><C3B9><EFBFBD>ģʽ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(ATTMOD_MagTOSUN);
break;
}
//ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD>
case ATT_CMDMODE_MODECHANG:
{
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO;
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
case ATT_CMDMODE_NOMODECHANG:
{
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_OK;
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//ģʽ<C4A3>л<EFBFBD><D0BB><EFBFBD><EFBFBD>ز<EFBFBD><D8B2><EFBFBD>
case ATT_CMDCODE_MODPARAM:
{
tmpWorkModPre = *tmpData;
tmpWorkMod = *(tmpData + 1);
tmpWlimit = DoGetFloatFrom8(tmpData+2) *ANG2RADIAN;
tmpAlimit = DoGetFloatFrom8(tmpData+6) *ANG2RADIAN;
tmpTimeLmt = DoGetInt16From8(tmpData+10);
tmpTimeLmt1 = DoGetInt16From8(tmpData+12);
tmpAngm = DoGetFloatFrom8(tmpData+14);
if((tmpWorkMod & 0x0F) > 0x0B)
{
m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
break;
}
if(ATTMOD_RATEDMP == tmpWorkModPre)
{
tmpConst->judge_time2 = tmpTimeLmt;
tmpConst->RateThr_2T3 =tmpWlimit;
tmpConst->HWHLThr_2T3 = tmpAngm;
}
else if(ATTMOD_MagTOSUN == tmpWorkModPre)
{
tmpConst->judge_time4 = tmpTimeLmt;
tmpConst->AngThr_3TB = tmpAlimit;
tmpConst->RateThr_3TB = tmpWlimit;
}
break;
}
//̫<><CCAB>
case ATT_CMDCODE_ASS:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
tmpConst->Ass_MinFour_limt[0] = DoGetFloatFrom8(tmpData+1 +0); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[0] = DoGetFloatFrom8(tmpData+1 +4); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
break;
}
case 0x22:
{
tmpConst->Ass_MinFour_limt[1] = DoGetFloatFrom8(tmpData+1 +0); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
tmpConst->Ass_MaxFour_limt[1] = DoGetFloatFrom8(tmpData+1 +4); //4Ƭģ<C6AC><C4A3>̫<EFBFBD><CCAB><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>ֵ
break;
}
default:
break;
}
break;
}
//<2F><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>״̬
case ATT_CMDCROSSUSE:
{
pIMU->SSUseState[0] = *(tmpData);
pIMU->SSUseState[1] = *(tmpData + 1);
pIMU->SSUseState[2] = *(tmpData + 2);
pIMU->AssUseState[0] = *(tmpData + 3);
pIMU->AssUseState[1] = *(tmpData + 4);
pIMU->AssUseState[2] = *(tmpData + 5);
pIMU->AssUseState[3] = *(tmpData + 6);
pIMU->AssUseState[4] = *(tmpData + 7);
pIMU->AssUseState[5] = *(tmpData + 8);
pIMU->MagUseState[0] = *(tmpData + 9);
pIMU->MagUseState[1] = *(tmpData + 10);
pIMU->GyroUseState[0] = *(tmpData + 11);
pIMU->GyroUseState[1] = *(tmpData + 12);
pIMU->GyroUseState[2] = *(tmpData + 13);
pIMU->Whl_Use[0] = *(tmpData + 14);
pIMU->Whl_Use[1] = *(tmpData + 15);
pIMU->Whl_Use[2] = *(tmpData + 16);
pIMU->Whl_Use[3] = *(tmpData + 17);
pIMU->MTUseStatus[0] = *(tmpData + 18);
pIMU->MTUseStatus[1] = *(tmpData + 19);
pIMU->MTUseStatus[2] = *(tmpData + 20);
pIMU->MTUseStatus[3] = *(tmpData + 21);
pIMU->PPUUseStatus= *(tmpData + 22);
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>ع<EFBFBD>״̬ע<CCAC><D7A2>
case ATT_CMDCROSS:
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
if (*(tmpData+0) != *(tmpData +1))
{
if (*(tmpData+0) != *(tmpData +2))
{
tmpConst->SSUsePrior[0]= *(tmpData) ;
tmpConst->SSUsePrior[1]= *(tmpData +1) ;
tmpConst->SSUsePrior[2]= *(tmpData +2) ;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>
if (*(tmpData+3) != *(tmpData +4))
{
if (*(tmpData+3) != *(tmpData +5))
{
tmpConst->GyroUsePrior[0]= *(tmpData +3) ;
tmpConst->GyroUsePrior[1]= *(tmpData +4) ;
tmpConst->GyroUsePrior[2]= *(tmpData +5) ;
}
}
break;
}
case ATT_CMDCREUSE:
{
tmpConst->AttCmdFlashPara.Mag_ComRESW[0] = *(tmpData);
tmpConst->AttCmdFlashPara.Mag_ComRESW[1] = *(tmpData+1);
tmpConst->AttCmdFlashPara.Mag_ComRESW[2] = *(tmpData+2);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[0] = *(tmpData+3);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[1] = *(tmpData+4);
tmpConst->AttCmdFlashPara.Gyro_ComRESW[2] = *(tmpData+5);
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
/**********************************<2A><>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>*********************************/
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_SSMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSA[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSB[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_SSC[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 + 1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//̫<><CCAB><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_ASSMAT:
{
tmpDevNO = *tmpData; //̫<><CCAB>ѡ<EFBFBD><D1A1>
if(0x11 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSA[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x22 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSB[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x33 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSC[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x44 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSD[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x55 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSE[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else if(0x66 == tmpDevNO)
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_aSSF[i][k] = DoGetFloatFrom8(tmpData+i*12+k*4+1);
}
}
}
else
{
//m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><>ǿ<EFBFBD>ư<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MAGMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for(k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_MAG[0][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for(k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_MAG[1][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ݰ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_GyroMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[0][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[1][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
for (k=0; k<3; k++)
{
tmpConst->AttCmdFlashPara.M_GYRO[2][i][k] = DoGetFloatFrom8(tmpData+i*12+k*4 +1);
}
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ְ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_WHLMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][0] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x22 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][1] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x44 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][2] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x88 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_Wheel[i][3] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MTMAT:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x11 :
{
for(i=0; i<3; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][0] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x22 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][1] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x44 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][2] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
case 0x88 :
{
for(i=0; i<4; i++)
{
tmpConst->AttCmdFlashPara.M_MAGCtrl[i][3] = DoGetFloatFrom8(tmpData+i*4 +1);
}
break;
}
default:
break;
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_DMPPARA:
{
tmpConst->Mag_Bb_CtrlMax = DoGetFloatFrom8(tmpData);
tmpConst->Mag_DampBdotMin = DoGetFloatFrom8(tmpData+4);
tmpConst->MTCtrlTIME= DoGetInt16From8(tmpData+ 8);
break;
}
//<2F>ſض<C5BF><D8B6>տ<EFBFBD><D5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_TOSUNPARA:
{
tmpCOMDCHOICE =*(tmpData) ;
switch (tmpCOMDCHOICE)
{
case 0x55 :
{
for (i = 0; i < 3; i++)
{
tmpConst->Mag_Kd[i] = DoGetFloatFrom8(tmpData+i*4 +1);
}
for (i = 0; i < 3; i++)
{
tmpConst->Mag_Kp[i] = DoGetFloatFrom8(tmpData+i*4+ 12+1);
}
tmpConst->Mag_WTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 +1) *ANG2RADIAN ;
break;
}
case 0xAA :
{
for (i = 0; i < 3; i++)
{
tmpConst->Mag_KpFBZK[i] = DoGetFloatFrom8(tmpData+i*4+1);
}
for (i = 0; i < 3; i++)
{
tmpConst->Mag_KdFBZK[i] = DoGetFloatFrom8(tmpData+i*4+ 12+1);
}
tmpConst->Mag_FBZKWTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 +1) *ANG2RADIAN ;
break;
}
default:
break;
}
tmpConst->Mag_ATTTOSUN=DoGetFloatFrom8(tmpData + 12 + 12 + 4+1) *ANG2RADIAN;
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫע<C6AB><D7A2>
case ATT_CMDCODE_GyroBIAS:
{
tmpWorkMod =*tmpData;
if (0x11==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[0][k] =DoGetFloatFrom8(tmpData+k*4 +1)* ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
else if (0x22==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[1][k] =DoGetFloatFrom8(tmpData+k*4 +1)* ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
else if (0x44==tmpWorkMod)
{
for (k = 0; k < 3; k++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[2][k] =DoGetFloatFrom8(tmpData+k*4 +1)* ANG2RADIAN; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ
}
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><D7A2>
case ATT_CMDCODE_GyroPH:
{
tmpWorkMod =*tmpData;
if (0x11==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[0][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
else if (0x22==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[1][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
else if (0x44==tmpWorkMod)
{
for (i = 0; i < 3; i++)
{
for (j = 0; j< 3; j++)
{
tmpConst->AttCmdFlashPara.Gyro_Comd_K[2][i][j] =DoGetFloatFrom8(tmpData+i*12+j*4+1); //<2F><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
}
}
pWMod->ATTFlashSwitch = POSE_OK;
break;
}
//<2F><>ǿ<EFBFBD>Ʊ궨ϵ<EAB6A8><CFB5>ע<EFBFBD><D7A2>
case ATT_CMDCODE_MTBD:
{
memset(tmpCOMDCHOICE,0x00,sizeof(UINT8));
tmpCOMDCHOICE =*tmpData;
if (0x11==tmpCOMDCHOICE)
{
for (i = 0; i< 3; i++)
{
tmpConst->AttCmdFlashPara.MAG_LK[0][i]=DoGetFloatFrom8(tmpData+i*4+1);
tmpConst->AttCmdFlashPara.MAG_LB[0][i]=DoGetFloatFrom8(tmpData+12+i*4+1);
}
}
else if (0x22==tmpCOMDCHOICE)
{
for (i = 0; i< 3; i++)
{
tmpConst->AttCmdFlashPara.MAG_LK[1][i]=DoGetFloatFrom8(tmpData+i*4+1);
tmpConst->AttCmdFlashPara.MAG_LB[1][i]=DoGetFloatFrom8(tmpData+12+i*4+1);
}
}
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D>ž<EFBFBD><C5BE><EFBFBD>ֵ
case ATT_CMDCODE_MTCTRL:
{
tmpConst->MTOutXYZLimit[0] = DoGetFloatFrom8(tmpData);
tmpConst->MTOutXYZLimit[1] = DoGetFloatFrom8(tmpData +4);
tmpConst->MTOutXYZLimit[2] = DoGetFloatFrom8(tmpData +8);
tmpConst->MTOutXYZLimit[3] = DoGetFloatFrom8(tmpData +12);
break;
}
default:
{
//m_pZKTaskInfo->iCmdRes = ERR_CODE_CMDCODE;
break;
}
}
#endif
}
//<2F>˿ص<CBBF><D8B5><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC>ȡ
void ZKPowerState(void)
{
UINT8 i= 0x00;
sAttPriData *tmpAtt = NULL;
sAttErrCtlPara_t *pREcheck= NULL;
sDevInfo *tmpDevInfo= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL ==tmpAtt)
return;
pREcheck=&tmpAtt->sErrCtlPara;
//<2F><>ʼ<EFBFBD><CABC>\<5C><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>
for(i=0; i<3; i++)
{
pREcheck->ssIn_PowerOn[i] = POSE_NO;
pREcheck->GyroIn_PowerOn[i] = POSE_NO;
}
//<2F><><EFBFBD><EFBFBD>
for(i=0; i<4; i++)
{
pREcheck->WhlIn_PowerOn[i] = POSE_NO;
}
//<2F><>ǿ<EFBFBD><C7BF>
for(i=0; i<2; i++)
{
pREcheck->MagIn_PowerOn[i] = POSE_NO;
}
//<2F><><EFBFBD><EFBFBD>
pREcheck->PPUIn_PowerOn = POSE_NO;
//<2F><><EFBFBD><EFBFBD>A
tmpDevInfo = GETDEVINFO(DEV_NO_UART15);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->ssIn_PowerOn[0] = POSE_OK;
if(pREcheck->ss_PowerOnCnt[0]<60000)
pREcheck->ss_PowerOnCnt[0]++; //<2F><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->ss_PowerOnCnt[0] = 0;
}
//<2F><><EFBFBD><EFBFBD>B
tmpDevInfo = GETDEVINFO(DEV_NO_UART16);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->ssIn_PowerOn[1] = POSE_OK;
if(pREcheck->ss_PowerOnCnt[1]<60000)
pREcheck->ss_PowerOnCnt[1]++; //<2F><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->ss_PowerOnCnt[1] = 0;
}
//<2F><><EFBFBD><EFBFBD>C
tmpDevInfo = GETDEVINFO(DEV_NO_UART4);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->ssIn_PowerOn[2] = POSE_OK;
if(pREcheck->ss_PowerOnCnt[2]<60000)
pREcheck->ss_PowerOnCnt[2]++; //<2F><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->ss_PowerOnCnt[2] = 0;
}
//<2F><><EFBFBD><EFBFBD>A
tmpDevInfo = GETDEVINFO(DEV_NO_UART5);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->GyroIn_PowerOn[0] = POSE_OK;
if(pREcheck->Gyro_PowerOnCnt[0]<60000)
pREcheck->Gyro_PowerOnCnt[0]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Gyro_PowerOnCnt[0] =0;
}
//<2F><><EFBFBD><EFBFBD>B
tmpDevInfo = GETDEVINFO(DEV_NO_UART6);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->GyroIn_PowerOn[1] = POSE_OK;
if(pREcheck->Gyro_PowerOnCnt[1]<60000)
pREcheck->Gyro_PowerOnCnt[1]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Gyro_PowerOnCnt[1] =0;
}
//<2F><><EFBFBD><EFBFBD>C
tmpDevInfo = GETDEVINFO(DEV_NO_UART7);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->GyroIn_PowerOn[2] = POSE_OK;
if(pREcheck->Gyro_PowerOnCnt[2]<60000)
pREcheck->Gyro_PowerOnCnt[2]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Gyro_PowerOnCnt[2] =0;
}
//<2F><><EFBFBD><EFBFBD>A
tmpDevInfo = GETDEVINFO(DEV_NO_UART8);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->WhlIn_PowerOn[0] = POSE_OK;
if(pREcheck->Whl_PowerOnCnt[0]<60000)
pREcheck->Whl_PowerOnCnt[0]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Whl_PowerOnCnt[0] =0;
}
//<2F><><EFBFBD><EFBFBD>B
tmpDevInfo = GETDEVINFO(DEV_NO_UART9);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->WhlIn_PowerOn[1] = POSE_OK;
if(pREcheck->Whl_PowerOnCnt[1]<60000)
pREcheck->Whl_PowerOnCnt[1]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Whl_PowerOnCnt[1] =0;
}
//<2F><><EFBFBD><EFBFBD>C
tmpDevInfo = GETDEVINFO(DEV_NO_UART10);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->WhlIn_PowerOn[2] = POSE_OK;
if(pREcheck->Whl_PowerOnCnt[2]<60000)
pREcheck->Whl_PowerOnCnt[2]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Whl_PowerOnCnt[2] =0;
}
//<2F><><EFBFBD><EFBFBD>D
tmpDevInfo = GETDEVINFO(DEV_NO_UART11);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->WhlIn_PowerOn[3] = POSE_OK;
if(pREcheck->Whl_PowerOnCnt[3]<60000)
pREcheck->Whl_PowerOnCnt[3]++; //<2F><><EFBFBD>ݼӵ<DDBC>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->Whl_PowerOnCnt[3] =0;
}
//<2F><>ǿ<EFBFBD><C7BF>A
tmpDevInfo = GETDEVINFO(DEV_NO_CQJ);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->MagIn_PowerOn[0] = POSE_OK;
if(pREcheck->Mag_PowerOnCnt[0]<60000)
pREcheck->Mag_PowerOnCnt[0]++;
}
else
{
pREcheck->Mag_PowerOnCnt[0] =0;
}
//<2F><>ǿ<EFBFBD><C7BF>B
tmpDevInfo = GETDEVINFO(DEV_NO_CQJ);
if(0x20 == (tmpDevInfo->sPowerInfo.bStart & 0x20))
{
pREcheck->MagIn_PowerOn[1] = POSE_OK;
if(pREcheck->Mag_PowerOnCnt[1]<60000)
pREcheck->Mag_PowerOnCnt[1]++;
}
else
{
pREcheck->Mag_PowerOnCnt[1] =0;
}
//<2F><><EFBFBD><EFBFBD>
tmpDevInfo = GETDEVINFO(DEV_NO_CAN0);
if(0x10 == (tmpDevInfo->sPowerInfo.bStart & 0x10))
{
pREcheck->PPUIn_PowerOn = POSE_OK;
if(pREcheck->PPU_PowerOnCnt<60000)
pREcheck->PPU_PowerOnCnt++; //<2F><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>
}
else
{
pREcheck->PPU_PowerOnCnt = 0;
}
}
/**************************************
**˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><EFBFBD><EFBFBD>ݻָ<EFBFBD>ģʽ<EFBFBD>л<EFBFBD>
**<EFBFBD><EFBFBD><EFBFBD>룺@index<EFBFBD><EFBFBD>־λ<EFBFBD><EFBFBD>
***************************************/
void ZKReModBK(void)
{
UINT8 tmpNewMod = ATTMOD_RATEDMP ;
sAttPriData *tmpAtt = NULL;
OrbitPriData_t *tmpOrbit= NULL;
tmpOrbit = (OrbitPriData_t *)ORBIT_BASE_ADDR;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if((NULL == tmpAtt) || (NULL == tmpOrbit))
{
return ;
}
tmpNewMod = tmpAtt->sModePara.WorkMode;
switch(tmpAtt->sModePara.WorkMode)
{
case ATTMOD_BREAKPRE: /*<2A>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD>ǰ 0x00*/
{
tmpNewMod = ATTMOD_NOCOLL;
break;
}
case ATTMOD_UNCTL: /*<2A>޿<EFBFBD>ģʽ 0xBB*/
{
tmpNewMod = ATTMOD_RATEDMP;
break;
}
case ATTMOD_ONLTOTAR: /*<2A><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8> 0xAA*/
case ATTMOD_ATTAJUST: /*<2A><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD> 0x99*/
case ATTMOD_ONLTOEARTH: /*<2A>Եض<D4B5><D8B6><EFBFBD> 0x55*/
case ATTMOD_WHEELTOEARTH: /*<2A><>̬<EFBFBD>Ե<EFBFBD> 0x66*/
{
if (POSE_OK == tmpOrbit->sCalOutData.ObtOutVldLast)
{
tmpNewMod = tmpAtt->sModePara.WorkMode;
}
else
{
tmpNewMod = ATTMOD_RATEDMP;
}
break;
}
case ATTMOD_ATTAJUSTPRE: /*<2A><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD> 0x77*/
case ATTMOD_ORBITCTL: /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0x88*/
{
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
tmpAtt->sPerPara.PPUPara.PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
tmpAtt->sPerPara.PPUPara.PPU_OrbitCtl_Quit_Err_Cnt = 0;
tmpAtt->sPerPara.PPUPara.PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
tmpAtt->sPerPara.PPUPara.PPU_OrbitCtl_Quit_OK_Cnt = 0;
if (POSE_OK ==tmpOrbit->sCalOutData.ObtOutVldLast)
{
tmpNewMod = ATTMOD_ONLTOEARTH;
}
else
{
tmpNewMod = ATTMOD_RATEDMP;
}
break;
}
case ATTMOD_NOCOLL: /*<2A><>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>ģʽ 0x11 */
case ATTMOD_RATEDMP: /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0x22 */
case ATTMOD_WhlTOSUN: /*<2A>ֿض<D6BF><D8B6>ն<EFBFBD><D5B6><EFBFBD> 0x33 */
case ATTMOD_MagTOSUN: /*<2A>ſض<C5BF><D8B6>ն<EFBFBD><D5B6><EFBFBD> 0x44 */
{
tmpNewMod = tmpAtt->sModePara.WorkMode;
break;
}
default: /*<2A><><EFBFBD>˿صĹ<D8B5><C4B9><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
{
tmpNewMod = ATTMOD_RATEDMP;
break;
}
}
/*<2A>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3>󣬽<EFBFBD><F3A3ACBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKModDoSet(tmpNewMod);
}
/************************************************FLASH********************************************************************/
/**************************************
**˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡFLASH<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**<EFBFBD><EFBFBD><EFBFBD>룺
***************************************/
void ZKREFlashBK(void)
{
AttCtrlConst_t *tmpConst = (AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sWheelIdArray *tmpBKStr = (sWheelIdArray *)ST_WHEEL_ID_ARRAY_ADDR;
if((NULL == tmpConst) ||(NULL == tmpBKStr))
{
return ;
}
if(tmpBKStr->WHEEL_ID_ARRAY[0][0] == 0X11)
{
tmpConst->Whl_ID[0]= tmpBKStr->WHEEL_ID_ARRAY[0][1];
}
if(tmpBKStr->WHEEL_ID_ARRAY[1][0] == 0X22)
{
tmpConst->Whl_ID[1]= tmpBKStr->WHEEL_ID_ARRAY[1][1];
}
if(tmpBKStr->WHEEL_ID_ARRAY[2][0] == 0X33)
{
tmpConst->Whl_ID[2]= tmpBKStr->WHEEL_ID_ARRAY[2][1];
}
if(tmpBKStr->WHEEL_ID_ARRAY[3][0] == 0X44)
{
tmpConst->Whl_ID[3]= tmpBKStr->WHEEL_ID_ARRAY[3][1];
}
}
/************************************************<2A><>ȡ<EFBFBD><C8A1>********************************************************************/
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺pTDATA , pSDATA
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
ע<EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD>롢<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪUINT8<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ATTGet2Base3FromBuff(UINT8 *pTDATA, UINT8 *pSDATA)
{
if((NULL == pSDATA) || (NULL == pTDATA))
{
return;
}
if(!(pSDATA[0]^pSDATA[1]))
{
*pTDATA = pSDATA[0];
}
else if(!(pSDATA[0]^pSDATA[2]))
{
*pTDATA = pSDATA[2];
}
else if(!(pSDATA[1]^pSDATA[2]))
{
*pTDATA = pSDATA[1];
}
else
{
; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD><EFBFBD>ݣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD>ڽ<EFBFBD><EFBFBD>и<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺pTDATA , pSDATA
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
ע<EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD>롢<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪUINT8<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ATTSetBase3ToBuff(UINT8 *pTDATA, UINT8 *pSDATA)
{
if((NULL == pSDATA) || (NULL == pTDATA))
{
return;
}
pSDATA[0] = *pTDATA;
pSDATA[1] = *pTDATA;
pSDATA[2] = *pTDATA;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><EFBFBD>и<EFBFBD><EFBFBD><EFBFBD>
ע<EFBFBD><EFBFBD>:<EFBFBD><EFBFBD><EFBFBD>롢<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪUINT8<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ATTGetImportDataFromBuff(void)
{
UINT8 i = 0X00;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL == tmpAtt) || (NULL == tmpConst))
{
return;
}
//if(POSE_OK == tmpAtt->AttData3T2Para.AttCtlBKFlag)
//{
//tmpAtt->AttData3T2Para.AttCtlBKFlag = POSE_NO;
ATTGet2Base3FromBuff(&tmpAtt->sModePara.WorkMode, &tmpAtt->AttData3T2Para.WorkModeBK[0]);
ATTGet2Base3FromBuff(&tmpConst->AttCmdFlashPara.WorkModeChangeAuto, &tmpAtt->AttData3T2Para.WorkModeChangeAutoBK[0]);
for (i = 0; i < STAR_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sErrCtlPara.SS_err[i], &tmpAtt->AttData3T2Para.SS_errBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sErrCtlPara.Mag_err[i], &tmpAtt->AttData3T2Para.Mag_errBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sErrCtlPara.Gyro_err[i], &tmpAtt->AttData3T2Para.Gyro_errBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sErrCtlPara.Whl_Err[i], &tmpAtt->AttData3T2Para.Whl_ErrBK[i][0]);
}
ATTGet2Base3FromBuff(&tmpAtt->sErrCtlPara.PPU_Err, &tmpAtt->AttData3T2Para.PPU_ErrBK[0]);
for (i = 0; i < STAR_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.SSUseState[i], &tmpAtt->AttData3T2Para.SSUseStateBK[i][0]);
}
for (i = 0; i < ASS_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.AssUseState[i], &tmpAtt->AttData3T2Para.AssUseStateBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.MagUseState[i], &tmpAtt->AttData3T2Para.MagUseStateBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.GyroUseState[i], &tmpAtt->AttData3T2Para.GyroUseStateBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.Whl_Use[i], &tmpAtt->AttData3T2Para.Whl_UseBK[i][0]);
}
for (i = 0; i < MT_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.MTUseStatus[i], &tmpAtt->AttData3T2Para.MTUseStatusBK[i][0]);
}
ATTGet2Base3FromBuff(&tmpAtt->sIMPTPara.PPUUseStatus, &tmpAtt->AttData3T2Para.PPUUseStatusBK[0]);
for (i = 0; i < 3; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AttCmdFlashPara.Gyro_ComRESW[i], &tmpAtt->AttData3T2Para.Gyro_ComRESWBK[i][0]);
}
for (i = 0; i < 3; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AttCmdFlashPara.Mag_ComRESW[i], &tmpAtt->AttData3T2Para.Mag_ComRESWBK[i][0]);
}
for (i = 0; i < STAR_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sPerPara.SsPara[i].ssPrior, &tmpAtt->AttData3T2Para.ssPriorBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpAtt->sPerPara.GyroPara[i].GyroPrior, &tmpAtt->AttData3T2Para.GyroPriorBK[i][0]);
}
for (i = 0; i < STAR_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AllowssONOFF[i], &tmpAtt->AttData3T2Para.AllowssONOFFBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AllowGyroONOFF[i], &tmpAtt->AttData3T2Para.AllowGyroONOFFBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AllowMagONOFF[i], &tmpAtt->AttData3T2Para.AllowMagONOFFBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTGet2Base3FromBuff(&tmpConst->AllowWhlONOFF[i], &tmpAtt->AttData3T2Para.AllowWhlONOFFBK[i][0]);
}
ATTGet2Base3FromBuff(&tmpConst->AllowPPUONOFF, &tmpAtt->AttData3T2Para.AllowPPUONOFFBK[0]);
ATTGet2Base3FromBuff(&tmpConst->Allow_Sensor_CThr, &tmpAtt->AttData3T2Para.Allow_Sensor_CThrBK[0]);
ATTGet2Base3FromBuff(&tmpConst->AllowAttLoseDiagFlg, &tmpAtt->AttData3T2Para.AllowAttLoseDiagFlgBK[0]);
ATTGet2Base3FromBuff(&tmpConst->AllowAttExDiagFlg, &tmpAtt->AttData3T2Para.AllowAttExDiagFlgBK[0]);
ATTGet2Base3FromBuff(&tmpConst->AllowAttStaDiagFlg, &tmpAtt->AttData3T2Para.AllowAttStaDiagFlgBK[0]);
//}
}
/*****************************************************************************
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
******************************************************************************/
void ATTImportDataBackup(void)
{
UINT8 i = 0X00;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL == tmpAtt) || (NULL == tmpConst))
{
return;
}
//tmpAtt->AttData3T2Para.AttCtlBKFlag = POSE_OK;
ATTSetBase3ToBuff(&tmpAtt->sModePara.WorkMode, &tmpAtt->AttData3T2Para.WorkModeBK[0]);
ATTSetBase3ToBuff(&tmpConst->AttCmdFlashPara.WorkModeChangeAuto, &tmpAtt->AttData3T2Para.WorkModeChangeAutoBK[0]);
for (i = 0; i < STAR_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sErrCtlPara.SS_err[i], &tmpAtt->AttData3T2Para.SS_errBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sErrCtlPara.Mag_err[i], &tmpAtt->AttData3T2Para.Mag_errBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sErrCtlPara.Gyro_err[i], &tmpAtt->AttData3T2Para.Gyro_errBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sErrCtlPara.Whl_Err[i], &tmpAtt->AttData3T2Para.Whl_ErrBK[i][0]);
}
ATTSetBase3ToBuff(&tmpAtt->sErrCtlPara.PPU_Err, &tmpAtt->AttData3T2Para.PPU_ErrBK[0]);
for (i = 0; i < STAR_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.SSUseState[i], &tmpAtt->AttData3T2Para.SSUseStateBK[i][0]);
}
for (i = 0; i < ASS_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.AssUseState[i], &tmpAtt->AttData3T2Para.AssUseStateBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.MagUseState[i], &tmpAtt->AttData3T2Para.MagUseStateBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.GyroUseState[i], &tmpAtt->AttData3T2Para.GyroUseStateBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.Whl_Use[i], &tmpAtt->AttData3T2Para.Whl_UseBK[i][0]);
}
for (i = 0; i < MT_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.MTUseStatus[i], &tmpAtt->AttData3T2Para.MTUseStatusBK[i][0]);
}
ATTSetBase3ToBuff(&tmpAtt->sIMPTPara.PPUUseStatus, &tmpAtt->AttData3T2Para.PPUUseStatusBK[0]);
for (i = 0; i < 3; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AttCmdFlashPara.Gyro_ComRESW[i], &tmpAtt->AttData3T2Para.Gyro_ComRESWBK[i][0]);
}
for (i = 0; i < 3; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AttCmdFlashPara.Mag_ComRESW[i], &tmpAtt->AttData3T2Para.Mag_ComRESWBK[i][0]);
}
for (i = 0; i < STAR_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sPerPara.SsPara[i].ssPrior, &tmpAtt->AttData3T2Para.ssPriorBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpAtt->sPerPara.GyroPara[i].GyroPrior, &tmpAtt->AttData3T2Para.GyroPriorBK[i][0]);
}
for (i = 0; i < STAR_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AllowssONOFF[i], &tmpAtt->AttData3T2Para.AllowssONOFFBK[i][0]);
}
for (i = 0; i < GYRO_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AllowGyroONOFF[i], &tmpAtt->AttData3T2Para.AllowGyroONOFFBK[i][0]);
}
for (i = 0; i < MAG_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AllowMagONOFF[i], &tmpAtt->AttData3T2Para.AllowMagONOFFBK[i][0]);
}
for (i = 0; i < WHEEL_NUM; ++i)
{
ATTSetBase3ToBuff(&tmpConst->AllowWhlONOFF[i], &tmpAtt->AttData3T2Para.AllowWhlONOFFBK[i][0]);
}
ATTSetBase3ToBuff(&tmpConst->AllowPPUONOFF, &tmpAtt->AttData3T2Para.AllowPPUONOFFBK[0]);
ATTSetBase3ToBuff(&tmpConst->Allow_Sensor_CThr, &tmpAtt->AttData3T2Para.Allow_Sensor_CThrBK[0]);
ATTSetBase3ToBuff(&tmpConst->AllowAttLoseDiagFlg, &tmpAtt->AttData3T2Para.AllowAttLoseDiagFlgBK[0]);
ATTSetBase3ToBuff(&tmpConst->AllowAttExDiagFlg, &tmpAtt->AttData3T2Para.AllowAttExDiagFlgBK[0]);
ATTSetBase3ToBuff(&tmpConst->AllowAttStaDiagFlg, &tmpAtt->AttData3T2Para.AllowAttStaDiagFlgBK[0]);
}
/************************************************˫<><CBAB>FIFO********************************************************************/
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD><EFBFBD><EFBFBD>׼<EFBFBD><EFBFBD>
***********************************************/
void ZKDataBackUp(void)
{
UINT8 i = 0X00;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sAttModPara_t *pWMod = NULL;
AttBKPara_t *pBK = NULL;
sAttErrCtlPara_t *pREcheck = NULL;
AttIMPTPara_t *pImpt = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL == tmpAtt) || (NULL == tmpConst))
{
return;
}
pWMod =&tmpAtt->sModePara;
pBK =&tmpAtt->AttBKPara;
pREcheck=&tmpAtt->sErrCtlPara;
pImpt=&tmpAtt->sIMPTPara;
//PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
//PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
//PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
pBK->BK_WorkMode = pWMod->WorkMode;
pBK->BK_WorkModeChangeAuto = tmpConst->AttCmdFlashPara.WorkModeChangeAuto;
for(i = 0;i<STAR_NUM;i++)
{
pBK->BK_SS_err[i] = pREcheck->SS_err[i];
}
for(i = 0;i<MAG_NUM;i++)
{
pBK->BK_Mag_err[i] = pREcheck->Mag_err[i];
}
for(i = 0;i<GYRO_NUM;i++)
{
pBK->BK_Gyro_err[i] = pREcheck->Gyro_err[i];
}
for(i = 0;i<WHEEL_NUM;i++)
{
pBK->BK_Whl_Err[i] = pREcheck->Whl_Err[i];
}
pBK->BK_PPU_Err = pREcheck->PPU_Err;
for(i = 0;i<STAR_NUM;i++)
{
pBK->BK_SSUseState[i] = pImpt->SSUseState[i];
}
for(i = 0;i<GYRO_NUM;i++)
{
pBK->BK_GyroUseState[i] = pImpt->GyroUseState[i];
}
for(i = 0;i<WHEEL_NUM;i++)
{
pBK->BK_Whl_Use[i] = pImpt->Whl_Use[i];
}
for(i = 0;i<MAG_NUM;i++)
{
pBK->BK_MagUseState[i] = pImpt->MagUseState[i];
}
for(i = 0;i<ASS_NUM;i++)
{
pBK->BK_AssUseState[i] = pImpt->AssUseState[i];
}
for(i = 0;i<MT_NUM;i++)
{
pBK->BK_MTUseStatus[i] = pImpt->MTUseStatus[i];
}
pBK->BK_PPUUseStatus= 0;
if (POSE_OK == pImpt->PPUUseStatus)
{
pBK->BK_PPUUseStatus= pBK->BK_PPUUseStatus | 0x05;
}
else
{
pBK->BK_PPUUseStatus= pBK->BK_PPUUseStatus & 0xF0;
}
if (POSE_OK == pImpt->PPUSafeStatus)
{
pBK->BK_PPUUseStatus= pBK->BK_PPUUseStatus | 0x50;
}
else
{
pBK->BK_PPUUseStatus= pBK->BK_PPUUseStatus & 0x0F;
}
for(i = 0;i<MT_NUM;i++)
{
pBK->BK_MTUseStatus[i]= pImpt->MTUseStatus[i];
}
for (i = 0;i<3;i++)
{
pBK->BK_Gyro_ComRESW[i] = tmpConst->AttCmdFlashPara.Gyro_ComRESW[i];
}
for (i = 0;i<3;i++)
{
pBK->BK_Mag_ComRESW[i] = tmpConst->AttCmdFlashPara.Mag_ComRESW[i];
}
for (i = 0;i<STAR_NUM;i++)
{
pBK->BK_ssPrior[i] = tmpConst->SSUsePrior[i];
}
for (i = 0;i<GYRO_NUM;i++)
{
pBK->BK_GyroPrior[i] = tmpConst->GyroUsePrior[i];
}
for (i = 0;i<STAR_NUM;i++)
{
pBK->BK_ss_Diag[i] = pREcheck->ss_Diag[i];
}
for (i = 0;i<MAG_NUM;i++)
{
pBK->BK_Mag_Diag[i] = pREcheck->Mag_Diag[i];
}
for (i = 0;i<GYRO_NUM;i++)
{
pBK->BK_Gyro_Diag[i] = pREcheck->Gyro_Diag[i];
}
for (i = 0;i<WHEEL_NUM;i++)
{
pBK->BK_Whl_Diag[i]= pREcheck->Whl_Diag[i];
}
for (i = 0;i<STAR_NUM;i++)
{
pBK->BK_AllowssONOFF[i]= tmpConst->AllowssONOFF[i];
}
for (i = 0;i<GYRO_NUM;i++)
{
pBK->BK_AllowGyroONOFF[i]= tmpConst->AllowGyroONOFF[i];
}
for (i = 0;i<MAG_NUM;i++)
{
pBK->BK_AllowMagONOFF[i]= tmpConst->AllowMagONOFF[i];
}
for (i = 0;i<WHEEL_NUM;i++)
{
pBK->BK_AllowWhlONOFF[i]= tmpConst->AllowWhlONOFF[i];
}
pBK->BK_AllowPPUONOFF= tmpConst->AllowPPUONOFF;
pBK->BK_Allow_Sensor_CThr = tmpConst->Allow_Sensor_CThr;
pBK->BK_AllowAttLoseDiagFlg = tmpConst->AllowAttLoseDiagFlg;
pBK->BK_AllowAttExDiagFlg = tmpConst->AllowAttExDiagFlg;
pBK->BK_AllowAttStaDiagFlg = tmpConst->AllowAttStaDiagFlg;
}
/**************************************
**˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD><EFBFBD><EFBFBD>
***************************************/
void ZKDoActDataTer(void)
{
UINT8 *tmpAddr = 0X00;
AttBKPara_t *tmpPCInfo = NULL;
sAttPriData *tmpAtt = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{
return;
}
tmpPCInfo =&tmpAtt->AttBKPara;
AttBKPara_t *tmpBKStr = NULL;
tmpAddr = (UINT8 *)(ST_IMPDATA_ADDR + ST_IMPDATA_HEAD + ST_IMPDATA_ATTMNG_OFFSET * 64); /*<2A><>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ洢<DDB4><E6B4A2><EFBFBD>򣬵<EFBFBD>9<EFBFBD><39>*/
if(tmpAddr==NULL)
return;
tmpBKStr = (AttBKPara_t *)tmpAddr;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<DDBD><E1B9B9><EFBFBD><EFBFBD>Դ<EFBFBD>ṹ<EFBFBD><E1B9B9>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD>һ<EFBFBD>£<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>*/
//memset(((UINT8 *)tmpBKStr + sizeof(sSaveDataHead)), 0, 64*ST_IMPDATA_ATTMNG_BLOCKS);
//memcpy(((UINT8 *)tmpBKStr + sizeof(sSaveDataHead)),(UINT8 *)tmpPCInfo,(sizeof(AttBKPara_t)-sizeof(sSaveDataHead) - 4));
memcpy(((UINT8 *)tmpBKStr),(UINT8 *)tmpPCInfo,(sizeof(AttBKPara_t)));
/*<2A>趨У<E8B6A8><D0A3>*/
//tmpBKStr->sBKHead.iDataLength = 64*ST_IMPDATA_ATTMNG_BLOCKS - sizeof(sSaveDataHead);
tmpBKStr->sBKHead.iDataLength = sizeof(AttBKPara_t) - sizeof(sSaveDataHead);
tmpBKStr->sBKHead.iDataType = BACKUP_TYPE_ATTFIFO;
tmpBKStr->iVldFlag = 0x55555555;
tmpBKStr->sBKHead.iDataCheck = DoGetCheckSum(tmpAddr + sizeof(sSaveDataHead), tmpBKStr->sBKHead.iDataLength );
/*<2A><><EFBFBD>ø<EFBFBD><C3B8>±<EFBFBD>־*/
tmpAddr = (UINT8 *)ST_IMPDATA_ADDR;
tmpAddr[ST_IMPDATA_ATTMNG_OFFSET] = 0x55;
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݻָ<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKCtrlReinitBK(void)
{
UINT8 i = 0;
//UINT8 bChkOK = POSE_NO;
UINT8 *tmpAddr = 0X00;
UINT16 *tmpData16 = NULL;
UINT16 tmpDA1 = 0U;
TYPE_CAL fData1 = 0.0F;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
AttBKPara_t *tmpBKStr = NULL;
//sTaskMngDataRecord *tmpTaskMngDataRcd = NULL;
sDevInfo *tmpDevInfo = NULL;
//tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst = (AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL == tmpAtt) || (NULL == tmpConst))
{
return;
}
/*<2A><>ȡ˫<C8A1><CBAB>FIFO<46><4F><EFBFBD>ݵ<EFBFBD>ַ*/
tmpAddr = (UINT8 *)(ST_IMPDATA_ADDR + ST_IMPDATA_HEAD + ST_IMPDATA_ATTMNG_OFFSET * 64);
tmpBKStr = (AttBKPara_t *)tmpAddr;
/*<2A>Թؼ<D4B9><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD>ȡ*/
ATTGetImportDataFromBuff();
/*<2A>״<EFBFBD><D7B4><EFBFBD><EFBFBD>Ǽӵ<C7BC><D3B5><EFBFBD><EFBFBD><EFBFBD>*/
ZKDptPowerOnPrc();
/*<2A><><EFBFBD><EFBFBD><EFBFBD>³<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if(0x55U == m_pZKTaskInfo->iReInit)
{
m_pZKTaskInfo->iReInit= 0;
tmpAtt->sModePara.ReDataBK = POSE_NO;
/*<2A><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>FLASH <20><><EFBFBD><EFBFBD>ID<49><44>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKREFlashBK();
/*<2A><><EFBFBD><EFBFBD>/<2F>л<EFBFBD>/<2F><>λ <20><><EFBFBD><EFBFBD>ת<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>0*/
SetWheel(1,0,WHL_CMD_CODE_CURR);
SetWheel(2,0,WHL_CMD_CODE_CURR);
SetWheel(3,0,WHL_CMD_CODE_CURR);
SetWheel(4,0,WHL_CMD_CODE_CURR);
/*У<>鱸<EFBFBD><E9B1B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD>ԣ<EFBFBD><D4A3><EFBFBD>У<EFBFBD><D0A3>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>У<EFBFBD><D0A3>һ<EFBFBD><D2BB>*/
if(DoCheckSaveData(tmpAddr, 64*ST_IMPDATA_ATTMNG_BLOCKS) && (0x55555555 == tmpBKStr->iVldFlag)&&(BACKUP_TYPE_ATTFIFO == tmpBKStr->sBKHead.iDataType ))
{
tmpAtt->sModePara.ReDataBK= POSE_OK;
}
else if(DoCheckSaveData(tmpAddr, 64*ST_IMPDATA_ATTMNG_BLOCKS) && (0x55555555 == tmpBKStr->iVldFlag)&&(BACKUP_TYPE_ATTFIFO == tmpBKStr->sBKHead.iDataType ))
{
tmpAtt->sModePara.ReDataBK= POSE_OK;
}
else
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ݻָ<DDBB><D6B8><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ<EFBFBD><D5B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᣬδչ<CEB4><D5B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>*/
tmpDevInfo = GETDEVINFO(DEV_NO_AD);
tmpData16 = (UINT16 *)(tmpDevInfo->cDevYCBuffer + INFO_ADDR_OFF);
tmpDA1 = *(tmpData16 + SW47 - 1); /*̫<><CCAB><EFBFBD><EFBFBD>չ<EFBFBD><D5B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ<EFBFBD><D5B9><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>*/
fData1 = 0.002442F * tmpDA1 - 5.0F; /*(idata * 10.0 / 4095 - 5)*/
if(POSE_ABSF(fData1) < 0.1)//0.1V<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
/*չ<><D5B9>*/
ZKModDoSet(ATTMOD_RATEDMP);
}
else
{
/*δչ<CEB4><D5B9>*/
ZKModDoSet(ATTMOD_NOCOLL);
}
return;
}
if (POSE_OK == tmpAtt->sModePara.ReDataBK)
{
/*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݻָ<DDBB>*/
tmpAtt->sModePara.WorkMode = tmpBKStr->BK_WorkMode;
tmpConst->AttCmdFlashPara.WorkModeChangeAuto= tmpBKStr->BK_WorkModeChangeAuto;
for(i = 0;i<STAR_NUM;i++)
{
tmpAtt->sErrCtlPara.SS_err[i]= tmpBKStr->BK_SS_err[i];
tmpConst->SSUsePrior[i]=tmpBKStr->BK_ssPrior[i];
tmpAtt->sErrCtlPara.ss_Diag[i]= tmpBKStr->BK_ss_Diag[i];
tmpConst->AllowssONOFF[i]= tmpBKStr->BK_AllowssONOFF[i];
tmpAtt->sIMPTPara.SSUseState[i]= tmpBKStr->BK_SSUseState[i];
}
for(i = 0;i<MAG_NUM;i++)
{
tmpAtt->sIMPTPara.MagUseState[i]= tmpBKStr->BK_MagUseState[i];
tmpAtt->sErrCtlPara.Mag_err[i]= tmpBKStr->BK_Mag_err[i];
tmpAtt->sErrCtlPara.Mag_Diag[i]= tmpBKStr->BK_Mag_Diag[i];
tmpConst->AllowMagONOFF[i]= tmpBKStr->BK_AllowMagONOFF[i];
tmpConst->AttCmdFlashPara.Mag_ComRESW[i]=tmpBKStr->BK_Mag_ComRESW[i];
}
for(i = 0;i<GYRO_NUM;i++)
{
tmpAtt->sErrCtlPara.Gyro_err[i]= tmpBKStr->BK_Gyro_err[i];
tmpAtt->sIMPTPara.GyroUseState[i]= tmpBKStr->BK_GyroUseState[i];
tmpConst->AttCmdFlashPara.Gyro_ComRESW[i]=tmpBKStr->BK_Gyro_ComRESW[i];
tmpConst->GyroUsePrior[i]=tmpBKStr->BK_GyroPrior[i];
tmpAtt->sErrCtlPara.Gyro_Diag[i]= tmpBKStr->BK_Gyro_Diag[i];
tmpConst->AllowGyroONOFF[i]= tmpBKStr->BK_AllowGyroONOFF[i];
}
for(i = 0;i<WHEEL_NUM;i++)
{
tmpAtt->sErrCtlPara.Whl_Err[i]= tmpBKStr->BK_Whl_Err[i];
tmpAtt->sIMPTPara.Whl_Use[i]= tmpBKStr->BK_Whl_Use[i];
tmpAtt->sErrCtlPara.Whl_Diag[i]= tmpBKStr->BK_Whl_Diag[i];
tmpConst->AllowWhlONOFF[i]= tmpBKStr->BK_AllowWhlONOFF[i];
}
tmpAtt->sErrCtlPara.PPU_Err= tmpBKStr->BK_PPU_Err;
for(i = 0;i<ASS_NUM;i++)
{
tmpAtt->sIMPTPara.AssUseState[i]= tmpBKStr->BK_AssUseState[i];
}
for(i = 0;i<MT_NUM;i++)
{
tmpAtt->sIMPTPara.MTUseStatus[i]= tmpBKStr->BK_MTUseStatus[i];
tmpAtt->sIMPTPara.MTUseStatus[i]= tmpBKStr->BK_MTUseStatus[i];
}
if (0x05 == (tmpBKStr->BK_PPUUseStatus & 0x0F))
{
tmpAtt->sIMPTPara.PPUUseStatus= POSE_OK;
}
else
{
tmpAtt->sIMPTPara.PPUUseStatus= POSE_NO;
}
if (0x50 == (tmpBKStr->BK_PPUUseStatus & 0xF0))
{
tmpAtt->sIMPTPara.PPUSafeStatus= POSE_OK;
}
else
{
tmpAtt->sIMPTPara.PPUSafeStatus= POSE_NO;
}
tmpConst->AllowPPUONOFF= tmpBKStr->BK_AllowPPUONOFF;
tmpConst->Allow_Sensor_CThr= tmpBKStr->BK_Allow_Sensor_CThr;
tmpConst->AllowAttLoseDiagFlg= tmpBKStr->BK_AllowAttLoseDiagFlg;
tmpConst->AllowAttExDiagFlg= tmpBKStr->BK_AllowAttExDiagFlg;
tmpConst->AllowAttStaDiagFlg= tmpBKStr->BK_AllowAttStaDiagFlg;
if (ATTMOD_BREAKPRE == tmpAtt->sModePara.WorkMode)
{
tmpAtt->sModePara.WorkMode = ATTMOD_NOCOLL;
}
/*<2A>жϻָ<CFBB><D6B8><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD>ģʽ<C4A3><CABD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧģʽ<C4A3><CABD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>*/
ZKReModBK();
}
///*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ָ<EFBFBD><D6B8>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ݴ洢<DDB4><E6B4A2>*/
//ATTImportDataBackup();
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKCtrlDataBackUp(void)
{
if(POSE_OK != m_pZKTaskInfo->iReInit)
{
ZKDataBackUp();
ZKDoActDataTer();
}
}
/************************************************Flash********************************************************************/
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Flash<EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ע<EFBFBD><EFBFBD>:
***********************************************/
void ATTRestoreFromFlash(void)
{
AttBackupFlash_t AttBackupFlashPara;
UINT8 *tmpAddr = (UINT8 *)(ST_FIXDATA_ATTSADA_ADDR);
AttBackupFlash_t *tmpBackup = (AttBackupFlash_t *)(ST_FIXDATA_ATTSADA_ADDR); /*<2A>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
AttCmdFlash_t *PFlash = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
PFlash = &tmpConst->AttCmdFlashPara;
if ( NULL == tmpBackup || NULL == tmpConst || NULL == tmpAtt)
{
return;
}
/* <20><><EFBFBD>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><D3B2>ص<EFBFBD><D8B5><EFBFBD>Flash<73><68><EFBFBD>ݹ<EFBFBD><DDB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>Ч */
if ( BACKUP_TYPE_ATTFLASH == tmpBackup->sBKHead.iDataType && TRUE == DoCheckSaveData(tmpAddr, tmpBackup->sBKHead.iDataLength) && 0x55555555 == tmpBackup->iVldFlag )
{
FixDataStorFlashByType(FIXDATA_TYPE_ATTSADA, (UINT8*)tmpAddr, sizeof(AttBackupFlash_t)); /* <20><><EFBFBD>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ浽<DDB4><E6B5BD><EFBFBD><EFBFBD>Flash */
}
/* <20><><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD>ӱ<EFBFBD><D3B1><EFBFBD>Flash<73><68>ȡ<EFBFBD><C8A1><EFBFBD>ݵ<EFBFBD><DDB5>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
else
{
/* <20><>ȡFlash<73><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
memset((UINT8 *)tmpAddr, 0, ST_FIXDATA_ATTSADA_SIZE);
FixDataLoadFlashByType(FIXDATA_TYPE_ATTSADA, (UINT8 *)tmpAddr, sizeof(AttBackupFlash_t));
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺͳ<CDBA><CDB3>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>򲻴<EFBFBD>Flash<73>ָ<EFBFBD> */
if ( BACKUP_TYPE_ATTFLASH != tmpBackup->sBKHead.iDataType || 0 == tmpBackup->sBKHead.iDataLength || 0xFFFFFFFF == tmpBackup->sBKHead.iDataLength )
{
return;
}
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>־λ<D6BE><CEBB><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>򲻴<EFBFBD>Flash<73>ָ<EFBFBD> */
if ( TRUE != DoCheckSaveData(tmpAddr, tmpBackup->sBKHead.iDataLength) || 0x55555555 != tmpBackup->iVldFlag )
{
return;
}
}
memcpy((UINT8 *)(&AttBackupFlashPara),((UINT8 *)tmpBackup ),sizeof(AttBackupFlash_t));
//<2F>˿<EFBFBD>Flash<73><68><EFBFBD>ݶ<EFBFBD>ȡ
memcpy((UINT8 *)PFlash,((UINT8 *)(&AttBackupFlashPara.AttFlashPara) ),sizeof(AttCmdFlash_t));
//<2F><><EFBFBD><EFBFBD>Flash<73><68><EFBFBD>ݶ<EFBFBD>ȡ
memcpy((UINT8 *)(&tmpAtt->ATSFlashPara),((UINT8 *)(&AttBackupFlashPara.SadaFlashPara) ),sizeof(SadaCmdFlash_t));
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD>Flash
ע<EFBFBD><EFBFBD>:
***********************************************/
void ATTBackupToFlash(void)
{
AttBackupFlash_t AttBackupFlashPara;
UINT8 *tmpAddr = (UINT8 *)(ST_FIXDATA_ATTSADA_ADDR);
AttBackupFlash_t *tmpBackup = (AttBackupFlash_t *)(ST_FIXDATA_ATTSADA_ADDR); /*<2A>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
AttCmdFlash_t *PFlash = NULL;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
PFlash = &tmpConst->AttCmdFlashPara;
//QSPI<50><49>ѯ״̬(0x55:<3A><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>)ִ<><D6B4>FLASH<53><48><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
//QspiFlashWriteProcess(FIXDATA_TYPE_ATTSADA, sizeof(AttBackupFlash_t));
if ( NULL == tmpBackup || NULL == tmpConst || NULL == tmpAtt )
{
return;
}
/* <20><><EFBFBD>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>и<EFBFBD><D0B8>£<EFBFBD><C2A3><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>˳<EFBFBD> */
if ( POSE_OK != tmpAtt->sModePara.ATTFlashSwitch )
{
return;
}
//<2F>˿<EFBFBD>Flash<73><68><EFBFBD><EFBFBD>д
memcpy((UINT8 *)(&AttBackupFlashPara.AttFlashPara),((UINT8 *)PFlash),sizeof(AttCmdFlash_t));
//SADA<44><41><EFBFBD><EFBFBD>д
memcpy((UINT8 *)(&AttBackupFlashPara.SadaFlashPara),(UINT8 *)(&tmpAtt->ATSFlashPara),sizeof(SadaCmdFlash_t));
//д<><D0B4>Flash<73><68><EFBFBD><EFBFBD>
memcpy((UINT8 *)tmpBackup,((UINT8 *)(&AttBackupFlashPara) ),sizeof(AttBackupFlash_t));
/* <20>趨<EFBFBD><E8B6A8>Ч<EFBFBD><D0A7>־λ */
tmpBackup->iVldFlag = 0x55555555;
/* <20>趨Flashͷ<68>ij<EFBFBD><C4B3>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>͡<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD> */
tmpBackup->sBKHead.iDataLength = sizeof(AttBackupFlash_t) - sizeof(sSaveDataHead);
tmpBackup->sBKHead.iDataType = (UINT16)BACKUP_TYPE_ATTFLASH;
tmpBackup->sBKHead.iDataCheck = DoGetCheckSum(tmpAddr+sizeof(sSaveDataHead), tmpBackup->sBKHead.iDataLength);
/* д<><D0B4>Flash */
FixDataStorFlashByType(FIXDATA_TYPE_ATTSADA, (UINT8*)tmpAddr, sizeof(AttBackupFlash_t));
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>±<EFBFBD>־λ */
tmpAtt->sModePara.ATTFlashSwitch = POSE_NO;
}
/***********************************************
˵<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 ZKPreAction(void)
{
UINT8 i = 0x00;
UINT16 *tmpAddr = (UINT16 *)MAG_BASE_ADDR;
sAttPriData *tmpAtt = NULL;
sAttOrbitGetPara_t *pOrbit = NULL;
OrbitPriData_t *tmpOrbit = NULL;
sAttModPara_t *pWMod = NULL;
MtPara_t *pMt = NULL;
sSadaCtrlInfo* tmpSadaCtlInfo = (sSadaCtrlInfo *)SADACTR_BASE_ADDR;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if ((NULL == tmpSadaCtlInfo) || (NULL == tmpAtt ))
{
return;
}
pOrbit = &tmpAtt->sOrbitPara;
pWMod =&tmpAtt->sModePara;
pMt=&tmpAtt->sPerPara.MtPara;
/*<2A><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
tmpOrbit = (OrbitPriData_t *)ORBIT_BASE_ADDR;
for(i = 0; i< 3; ++i)
{
pOrbit->JPos[i] = (TYPE_CAL)tmpOrbit->sPoseUseOrbit.Rj[i];
pOrbit->JVel[i] = (TYPE_CAL)tmpOrbit->sPoseUseOrbit.Vj[i];
}
pOrbit->GAST = (TYPE_CAL)tmpOrbit->sPoseUseOrbit.GAST;
pOrbit->MJC = (TYPE_CAL)tmpOrbit->sPoseUseOrbit.MJC;
pOrbit->MJCLv = tmpOrbit->sPoseUseOrbit.MJCLv;
pOrbit->OrbitVld = tmpOrbit->sPoseUseOrbit.ObtTZKVld;
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pWMod->WModSadaOpenPre= pWMod->WModSadaOpen;
pWMod->WModSadaOpen=tmpSadaCtlInfo->sSadaDataUP.SadaOpenFlag; //<2F><><EFBFBD><EFBFBD>չ<EFBFBD><D5B9><EFBFBD>ź<EFBFBD>
pMt->MTStatus = 0xFF & ReadReg(tmpAddr,MAG_REG_SATUS);
//<2F><><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>ȡ
ZKPowerState();
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>飬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><EFBFBD><EFBFBD>ǰģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKOCOrbitCtl(void)
{
UINT16 tmpiDataCheck = 0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
UINT8 cmdDataBuff[50] = {0};
UINT32 SatTime[2] ={0,0};
UINT8 PPUOn[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
PPUPara_t *PPPU = NULL;
sTaskInfo* tmpTaskInfo = NULL;
sAttPriData *tmpAtt = NULL;
sAttOrbitCtlData_t *tmpOrbCmd = NULL;
sAttOrbitCtlInfo_t *pOrb = NULL;
sAttModPara_t *pWMod =NULL;
AttIMPTPara_t *pImpt = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
if ((NULL == tmpOrbCmd) || (NULL == tmpAtt))
{
return;
}
pWMod=&tmpAtt->sModePara;
pOrb=&tmpAtt->sOrbitInp;
PPPU = &tmpAtt->sPerPara.PPUPara;
pImpt= &tmpAtt->sIMPTPara;
memset(cmdDataBuff, 0x00, 50);
GetTime(SatTime); //<2F><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ʱ
if (POSE_OK != pOrb->OrbDataIn_OK)/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>Ч<EFBFBD><D0A7>ֱ<EFBFBD><D6B1><EFBFBD>˳<EFBFBD>*/
{
return;
}
if (pOrb->CurOrbCtrlPcak_ID >= 50)
{
DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1>
if (POSE_OK == pOrb->OrbDataIn_OK)
{
if ((pOrb->CurOrbCtrlPcak_ID == (tmpOrbCmd->OrbCtrlPack[pOrb->CurOrbCtrlPcak_ID].OrbCtrlPcak_ID -1))&&(pOrb->CurOrbCtrlPcak_PreID != pOrb->CurOrbCtrlPcak_ID))
{
pOrb->CurOrbCtrlPcak_PreID = pOrb->CurOrbCtrlPcak_ID;
tmpiDataCheck =DoGetCheckSum(&tmpOrbCmd->OrbCtrlPack[pOrb->CurOrbCtrlPcak_ID].OrbCtrlPcak_ID , 24);
if ((pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]== 0x55)&&(tmpiDataCheck == tmpOrbCmd->OrbCtrlPack[pOrb->CurOrbCtrlPcak_ID].OrbDateSum))
{
memcpy(&(pOrb->OrbCtrlPackCur), &(tmpOrbCmd->OrbCtrlPack[pOrb->CurOrbCtrlPcak_ID]), sizeof(AttOrbCtrlPara_t));
}
else
{
DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
/* <20><><EFBFBD>˿ص<CBBF>ָ<EFBFBD><EFBFBD><EEB4A6><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
return;
}
}
}
if((POSE_OK == pOrb->OrbDataIn_OK)
&&(0x55 == pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID])
)
{
if(((ATTMOD_WhlTOSUN == pWMod->WorkMode) ||(ATTMOD_WHEELTOEARTH == pWMod->WorkMode))
&&(SatTime[0]>=pOrb->OrbCtrlPackCur.OrbAttAjustTime)
&&(SatTime[0]<pOrb->OrbCtrlPackCur.OrbCtrl_StartTime)
)
{
DoSetInt32To8(ATT_WORKMODE_ORBITCTLPRE, 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);
}
}
else if((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)
&&((SatTime[0]>=pOrb->OrbCtrlPackCur.OrbCtrl_StartTime)
&&(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength)))
)
{
DoSetInt32To8(ATT_WORKMODE_ORBITCTL, 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_ORBITCTL);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
if (POSE_OK == pImpt->PPUUseStatus)
{
//PPU<50><55><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
dev_can_write(DEV_NO_CAN0,PPUOn,8);
PPPU->PPUOUT_Sta = POSE_OK;
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
}
}
}
else if((ATTMOD_ORBITCTL == pWMod->WorkMode)
&&((SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength))&&
(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength + 1))))
{
/*<2A><><EFBFBD>뵽<EFBFBD><EBB5BD><EFBFBD>˵<EFA3AC><CBB5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD>*/
switch(pOrb->OrbCtrlPackCur.OrbCtrl_Mod)
{
case 0x11:
DoSetInt32To8(ATT_CMDMODE_WHEELTOEARTH, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID ++;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
case 0x22:
DoSetInt32To8(ATT_CMDMODE_WhlTOSUN, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID ++;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
case 0x33:
DoSetInt32To8(ATT_WORKMODE_ORBITCTLPRE, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID ++;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
case 0x44:
DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
case 0x55:
DoSetInt32To8(ATT_CMDMODE_WhlTOSUN, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
default:
DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
pOrb->CurOrbCtrlPcak_ID = 0;
pOrb->CurOrbCtrlPcak_PreID =0xFF;
pOrb->OrbCtrlPcak_ReceiveFlg[pOrb->CurOrbCtrlPcak_ID]= 0x00;
PPPU->PPUT_FIR_Cnt = 0;
pOrb->OrbDataIn_OK = POSE_NO;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>Ĺ<EFBFBD>PPU<50><55>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;/*<2A><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ر<EFBFBD>־ʹ<D6BE><CAB9>*/
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
break;
}
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);
}
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
//PPU<50>ػ<EFBFBD>ָ<EFBFBD><D6B8>
dev_can_write(DEV_NO_CAN0, PPUOff, 8);/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹػ<C6B9><D8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ5<D2AA><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹ<EFBFBD><C6B9><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 ZKOCTarCtl(void)
{
UINT16 tmpiDataCheck =0; /*<2A><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>*/
UINT8 cmdDataBuff[50] = {0};
UINT32 SatTime[2] ={0,0};
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sAttTARCCtlData_t *tmpTARCmd = NULL;
sAttTARCCtlInfo_t *pTARC = NULL;
sAttModPara_t *pWMod =NULL;
sTaskInfo* tmpTaskInfo = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
tmpTARCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
if (NULL == tmpTARCmd || NULL == tmpAtt || NULL == tmpConst)
{
return;
}
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
pWMod=&tmpAtt->sModePara;
pTARC=&tmpAtt->sTARCInp;
if (POSE_OK != pTARC->TARDataIn_OK)/*Ŀ<>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD>Ч<EFBFBD><D0A7>ֱ<EFBFBD><D6B1><EFBFBD>˳<EFBFBD>*/
{
return;
}
if (pTARC->CurTARCtrlPcak_ID >= 30)
{
pTARC->CurTARCtrlPcak_ID = 0;
return;
}
memset(cmdDataBuff, 0x00, 50);
GetTime(SatTime); //<2F><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ʱ
if (POSE_OK==pTARC->TARDataIn_OK)
{
if ((pTARC->CurTARCtrlPcak_ID == (tmpTARCmd->TARCtrlPack[pTARC->CurTARCtrlPcak_ID].TARCtrlPcak_ID -1)) &&(pTARC->CurTARCtrlPcak_ID != pTARC->CurTARCtrlPcak_PreID))
{
pTARC->CurTARCtrlPcak_PreID = pTARC->CurTARCtrlPcak_ID;
tmpiDataCheck=DoGetCheckSum(&(tmpTARCmd->TARCtrlPack[pTARC->CurTARCtrlPcak_ID].TARCtrlPcak_ID), 23);
if ((pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID]== 0x55)&&(tmpiDataCheck == tmpTARCmd->TARCtrlPack[pTARC->CurTARCtrlPcak_ID].TARCtrlDateSum))
{
memcpy(&(pTARC->TARCtrlPackCur), &(tmpTARCmd->TARCtrlPack[pTARC->CurTARCtrlPcak_ID]), sizeof(sAttTARCCtlPara_t));
}
else
{
pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID] = 0x00;
DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
{
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
return ;
}
}
}
if((POSE_OK == pTARC->TARDataIn_OK)
&&(0x55 == pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID])
)
{
if(((ATTMOD_WhlTOSUN == pWMod->WorkMode) ||(ATTMOD_WHEELTOEARTH == pWMod->WorkMode))
&&(SatTime[0]>=pTARC->TARCtrlPackCur.TARCtrl_JustTime)
&&(SatTime[0]<pTARC->TARCtrlPackCur.TARCtrl_StartTime)
)
{
DoSetInt32To8(ATT_WORKMODE_ATTAJUST, 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);
}
}
else if((ATTMOD_ATTAJUST == pWMod->WorkMode)
&&((SatTime[0]>=pTARC->TARCtrlPackCur.TARCtrl_StartTime)
&&(SatTime[0]<= (pTARC->TARCtrlPackCur.TARCtrl_StartTime+pTARC->TARCtrlPackCur.TARCtrl_TimeLength)))
)
{
DoSetInt32To8(ATT_WORKMODE_ONLTOTAR, 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_ORBITCTL);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
else if((ATTMOD_ONLTOTAR == pWMod->WorkMode)
&&((SatTime[0]>=(pTARC->TARCtrlPackCur.TARCtrl_StartTime+pTARC->TARCtrlPackCur.TARCtrl_TimeLength))&&
(SatTime[0]<= (pTARC->TARCtrlPackCur.TARCtrl_StartTime+pTARC->TARCtrlPackCur.TARCtrl_TimeLength + 1))))
{
if (pTARC->CurTARCtrlPcak_ID < 29)
{
pTARC->CurTARCtrlPcak_ID ++;
if(0 == tmpTARCmd->TARCtrlPack[pTARC->CurTARCtrlPcak_ID].TARCtrlPcak_ID)/*<2A><>һ<EFBFBD><D2BB>IDΪ0<CEAA><30><EFBFBD><EFBFBD>ΪĿ<CEAA>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
{
pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID] = 0x00;
DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARDataIn_OK = POSE_NO;
}
else
{
DoSetInt32To8(ATT_WORKMODE_ATTAJUST, cmdDataBuff);
}
}
else
{
DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID]= 0x00;
pTARC->TARDataIn_OK = POSE_NO;
}
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_ORBITCTL);*/
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
}
}
}
if((POSE_OK == pTARC->TARDataIn_OK) && (ATTMOD_ONLTOTAR == pWMod->WorkMode))
{
/*<2A>жϵ<D0B6>ǰʱ<C7B0>䣬<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if (!(
(SatTime[0]>=(pTARC->TARCtrlPackCur.TARCtrl_StartTime-2)) &&
(SatTime[0]<= (pTARC->TARCtrlPackCur.TARCtrl_StartTime + pTARC->TARCtrlPackCur.TARCtrl_TimeLength+2))
))
{
/*<2A><><EFBFBD>ڵĻ<DAB5><C4BB>˳<EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD>Եض<D4B5><D8B6><EFBFBD>ģʽ*/
DoSetInt32To8(ATT_CMDMODE_ONLTOEARTH, cmdDataBuff);
pTARC->CurTARCtrlPcak_ID=0;
pTARC->CurTARCtrlPcak_PreID =0xFF;
pTARC->TARCtrlPcak_ReceiveFlg[pTARC->CurTARCtrlPcak_ID]= 0x00;
pTARC->TARDataIn_OK = POSE_NO;
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);
}
return;
}
}
}
///***********************************************
//˵<><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>
//ע<><D7A2>: ÿ0.5<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
//***********************************************/
void ZKMiniPowerOn(void)
{
// UINT8 i = 0;
UINT8 *tmpAddr = 0X00;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
// AttBKPara_t *tmpBKStr = NULL;
tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst = (AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL == tmpAtt) || (NULL == tmpConst))
{
return;
}
/*<2A><>ȡ˫<C8A1><CBAB>FIFO<46><4F><EFBFBD>ݵ<EFBFBD>ַ*/
tmpAddr = (UINT8 *)(ST_IMPDATA_ADDR + ST_IMPDATA_HEAD + ST_IMPDATA_ATTMNG_OFFSET * 64);
// tmpBKStr = (AttBKPara_t *)tmpAddr;
/*<2A>״<EFBFBD><D7B4><EFBFBD><EFBFBD>Ǽӵ<C7BC><D3B5><EFBFBD><EFBFBD><EFBFBD>*/
ZKDptPowerOnPrc();
/*<2A><><EFBFBD><EFBFBD><EFBFBD>³<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if(POSE_OK == tmpAtt->sModePara.MiniDataSet)
{
m_pZKTaskInfo->iReInit= 0;
tmpAtt->sModePara.MiniDataSet =POSE_NO;
tmpAtt->sModePara.ReDataBK = POSE_NO;
/*<2A><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>FLASH <20><><EFBFBD><EFBFBD>ID<49><44>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKREFlashBK();
/*<2A><>ȡflash<73><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ATTRestoreFromFlash();
/*<2A><><EFBFBD><EFBFBD>/<2F>л<EFBFBD>/<2F><>λ <20><><EFBFBD><EFBFBD>ת<EFBFBD>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>0*/
SetWheel(1,0,WHL_CMD_CODE_CURR);
SetWheel(2,0,WHL_CMD_CODE_CURR);
SetWheel(3,0,WHL_CMD_CODE_CURR);
SetWheel(4,0,WHL_CMD_CODE_CURR);
ZKModDoSet(ATTMOD_RATEDMP);
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD><EFBFBD><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 ZKAction(void)
{
sAttPriData *tmpAtt = NULL;
MtPara_t *pMt = NULL;
//sAttModPara_t *pWMod;
sTaskMngDataRecord *tmpTaskMngDataRcd = NULL;
tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if((NULL == tmpAtt)||(NULL == tmpTaskMngDataRcd))
return;
pMt=&tmpAtt->sPerPara.MtPara;
if (POSE_OK == tmpAtt->sModePara.ModeSwitchFlg)
{
ZKModChgInit();
}
/*׼<><D7BC>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKPreAction();
m_pZKTaskInfo->iRunPos++;
#ifdef MINMODULE_TYPE
ZKMiniPowerOn();
m_pZKTaskInfo->iRunPos++;
/*ģʽ<C4A3><CABD><EFBFBD><EFBFBD>*/
ZKDoModeAct();
m_pZKTaskInfo->iRunPos++;
/*<2A><><EFBFBD><EFBFBD>ģ<EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>*/
ZKEnvAct();
m_pZKTaskInfo->iRunPos++;
if (0xFFFFFFFE > pMt-> MTCtrlTime)
{
pMt-> MTCtrlTime ++; //<2F>˼<EFBFBD><CBBC><EFBFBD>Ӧ<EFBFBD>÷<EFBFBD><C3B7>ڴ<EFBFBD>ǿ<EFBFBD>ƺʹ<C6BA>ģ<EFBFBD><C4A3>ǰ<EFBFBD><C7B0>
}
else
{
pMt-> MTCtrlTime = 0;
}
//<2F><><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD>
ZKDPAction();
m_pZKTaskInfo->iRunPos++;
//*<2A><>̬ȷ<CCAC><C8B7>*/
ZKPGAct();
m_pZKTaskInfo->iRunPos++;
//*<2A><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>
ZKPosCtlAct(&tmpAtt->sDeterPara, &tmpAtt->sGuidLawPara, &tmpAtt->sCtlPara);
m_pZKTaskInfo->iRunPos++;
/*ָ<><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKCmdDspAct();
tmpAtt->sModePara.WorkModePre = tmpAtt->sModePara.WorkMode;
m_pZKTaskInfo->iRunPos++;
#else
/*ģʽ<C4A3><CABD><EFBFBD><EFBFBD>*/
ZKDoModeAct();
m_pZKTaskInfo->iRunPos++;
/*<2A><><EFBFBD><EFBFBD>ģ<EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>*/
ZKEnvAct();
m_pZKTaskInfo->iRunPos++;
if (0xFFFFFFFE > pMt-> MTCtrlTime)
{
pMt-> MTCtrlTime ++; //<2F>˼<EFBFBD><CBBC><EFBFBD>Ӧ<EFBFBD>÷<EFBFBD><C3B7>ڴ<EFBFBD>ǿ<EFBFBD>ƺʹ<C6BA>ģ<EFBFBD><C4A3>ǰ<EFBFBD><C7B0>
}
else
{
pMt-> MTCtrlTime = 0;
}
/*<2A><><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD>*/
ZKDPAction();
m_pZKTaskInfo->iRunPos++;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ع<EFBFBD>*/
ZKREAction();
m_pZKTaskInfo->iRunPos++;
///*<2A><>̬ȷ<CCAC><C8B7>*/
ZKPGAct();
m_pZKTaskInfo->iRunPos++;
///*<2A><><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>*/
ZKDYAction();
m_pZKTaskInfo->iRunPos++;
///*<2A><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>*/
ZKPosCtlAct(&tmpAtt->sDeterPara, &tmpAtt->sGuidLawPara, &tmpAtt->sCtlPara);
m_pZKTaskInfo->iRunPos++;
//Ŀ<>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD>ƴ<EFBFBD><C6B4><EFBFBD>
ZKOCTarCtl();
m_pZKTaskInfo->iRunPos++;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƴ<EFBFBD><C6B4><EFBFBD>
ZKOCOrbitCtl();
m_pZKTaskInfo->iRunPos++;
/*ָ<><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
ZKCmdDspAct();
m_pZKTaskInfo->iRunPos++;
#endif
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺taskID: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><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 AttCtrlMng(UINT8 taskID)
{
sTaskMngDataRecord *tmpTaskMngDataRcd = NULL;
tmpTaskMngDataRcd = (sTaskMngDataRecord *)ST_TASKMNG_ADDR;
/*<2A><>ʼ<EFBFBD><CABC>ģ<EFBFBD><C4A3>*/
if(!AttCtrlInit(taskID))
{
m_pZKTaskInfo->iErrCode = ERR_CODE_INIT;
m_pZKTaskInfo->iStatus = TASK_STATUS_FAIL;
return;
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD>*/
while(TRUE)
{
/*<2A>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD>ѣ<EFBFBD><D1A3><EFBFBD><EFBFBD>õȴ<C3B5>*/
OSSemPend(&m_pZKTaskInfo->sMngSem, 0, OS_OPT_PEND_BLOCKING,
&m_pZKTaskInfo->sMngTs,
&m_pZKTaskInfo->sMngErr);
StartTaskAction(m_pZKTaskInfo);
ZKCtrlReinitBK();
m_pZKTaskInfo->iRunPos++;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
if(0x55 == tmpTaskMngDataRcd->bSHDepart)
{
//<2F>˹<EFBFBD><CBB9>ؿ<EFBFBD><D8BF><EFBFBD>
ZKAction();
m_pZKTaskInfo->iRunPos++;
}
//<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
ZKCmdResolve();
m_pZKTaskInfo->iRunPos++;
//*<2A><><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD>*/
ZKCtrlDataBackUp();
m_pZKTaskInfo->iRunPos++;
//flash<73><68><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD>
ATTBackupToFlash();
m_pZKTaskInfo->iRunPos++;
ATTImportDataBackup();
m_pZKTaskInfo->iRunPos++;
//m_pZKTaskInfo->iRunPos++;
StopTaskAction(m_pZKTaskInfo);
/*<2A>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>*/
if(TASK_PERMIT_STOP == m_pZKTaskInfo->iPermit)
{
m_pZKTaskInfo->iTaskID = 0;
m_pZKTaskInfo->iStatus = TASK_STATUS_STOP;
break;
}
}
}
Reference in New Issue Copy Permalink