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

927 lines
25 KiB
C
Raw Permalink Normal View History

增加代码知识库；修复文档处理内容；增加API设置
2026-05-16 20:20:10 +08:00
/*
* Created: 2022/11/4 11:02:40
* Author: wangzk zhengmengxing
*/
#include "..\PrjCommon\CommonDef.h"
#include "..\PrjCommon\DevDefine.h"
#include "..\PrjTelCtrlMng\TelCtrlMng.h"
#include "AttMath.h"
#include "AttCtrlMain.h"
/*******************************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD>ʼ<EFBFBD><EFBFBD>
*******************************************************/
void ZKCtrlCmdInit(void)
{
UINT8 i = 0x00;
sAttPriData *tmpAtt = NULL ;
AttCmdDsp_t *pCmdDsp = NULL;
MtPara_t *pMt = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL ==tmpAtt)
return;
pCmdDsp = &tmpAtt->sCmdDspPara;
pMt=&tmpAtt->sPerPara.MtPara;
//<2F><><EFBFBD><EFBFBD>
pCmdDsp->WheelD_MomOfForce_Cmd = 0;
pCmdDsp->WheelD_RotatSpd_Cmd = 0;
memset(pCmdDsp->Wheel_TwcCmd, 0, 3*sizeof(TYPE_CAL));
memset(pCmdDsp->Wheel_LJCmd, 0, 4*sizeof(TYPE_CAL));
memset(pCmdDsp->Whl_Torque, 0, 4*sizeof(TYPE_CAL));
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (i = 0; i < MT_NUM; i++)
{
pMt->MTOnOff[i] = MT_OFF;
pMt->MTCtrlOutP0[i] = 0.0;
pMt->MTDirect[i] =MT_DIRECT_POS;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD><EFBFBD>ػ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬ 0x55:<EFBFBD><EFBFBD><EFBFBD>ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0xAA:<EFBFBD><EFBFBD><EFBFBD>ƹػ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޲<EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
//void ZKDev_PPU_OnOffProcess(UINT8 OnState)
//{
//sAttPriData *tmpAtt = NULL;
//UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
//
//tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
//
//if((NULL == tmpAtt) || ((0x55 != (OnState)) && (0xAA != (OnState))))
//return;
//
//if (0xAA == (OnState))/*<2A><><EFBFBD>ƹػ<C6B9>*/
//{
///*<2A><>ǰ<EFBFBD>ĵĹض<C4B9><D8B6><EFBFBD>*/
//dev_can_write(DEV_NO_CAN0,PPUOff,8);
///*<2A><>һ<EFBFBD>ĵĹض<C4B9><D8B6><EFBFBD><EFBFBD><EFBFBD>־*/
//tmpAtt->sErrCtlPara.AttPPUOffReq = POSE_OK;
//}
////else if (0x55 == (OnState))/*<2A><><EFBFBD>ƿ<EFBFBD><C6BF><EFBFBD>*/
////{
/////*<2A><>ǰ<EFBFBD>ĵĿ<C4B5><C4BF><EFBFBD><EFBFBD><EFBFBD>*/
////OCOut(OC68_PPU_ON, 0, 16);
/////*<2A><>һ<EFBFBD>ĵĹض<C4B9><D8B6><EFBFBD><EFBFBD><EFBFBD>־*/
////tmpAtt->sErrCtlPara.AttPPUOnReq = POSE_OK;
////}
//
//}
///***********************************************
//˵<><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>飬<EFBFBD><E9A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
//<2F><><EFBFBD>룺<EFBFBD><EBA3BA>ע<EFBFBD><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ǰģʽ
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<50><55><EFBFBD><EFBFBD>
//***********************************************/
void ZKPPUCmdDsp(void)
{
// UINT8 PPUOn[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
UINT32 SatTime[2] ={0,0};
UINT8 cmdDataBuff[50] = {0};
sTaskInfo* tmpTaskInfo = NULL;
sAttPriData *tmpAtt = NULL;
PPUPara_t *PPPU = NULL;
sAttModPara_t *pMod = NULL;
AttIMPTPara_t *pImpt = NULL;
sAttOrbitCtlInfo_t *pOrb =NULL;
AttCtrlConst_t *tmpConst= NULL;
sAttErrCtlPara_t *pREcheck= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL ==tmpAtt|| NULL ==tmpConst)
return;
PPPU = &tmpAtt->sPerPara.PPUPara;
pMod = &tmpAtt->sModePara;
pImpt= &tmpAtt->sIMPTPara;
pOrb=&tmpAtt->sOrbitInp;
pREcheck=&tmpAtt->sErrCtlPara;
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
memset(cmdDataBuff, 0x00, 50);
GetTime(SatTime); //<2F><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ʱ
//if((ATTMOD_ATTAJUSTPRE == pMod->WorkMode) || (ATTMOD_ORBITCTL == pMod->WorkMode))/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
if(ATTMOD_ORBITCTL == pMod->WorkMode)/*<2A><><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD>*/
{
if (pImpt->PPUUseStatus == POSE_NO)/*<2A><><EFBFBD>ƽ<EFBFBD>״̬Ϊ<CCAC><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
{
//DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
pREcheck->PPU_Diag = POSE_NO;
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;
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);
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
return ;
}
}
if(POSE_OK == pOrb->OrbDataIn_OK)
{
/*<2A>жϵ<D0B6>ǰʱ<C7B0>䣬<EFBFBD>Ƿ<EFBFBD><C7B7>ڹ<EFBFBD><DAB9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if (((SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+30)) &&
(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength-30))
))
{
if (ATTMOD_ORBITCTL != pMod->WorkMode)
{
/*<2A><><EFBFBD>ڵĻ<DAB5><C4BB>˳<EFBFBD>*/
//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);
//}
/*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
return;
}
//if ((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
//{
////if ((POSE_NO==tmpConst->WorkModeChangeAuto)||(POSE_OK == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
////{
///*<2A><><EFBFBD>ڵĻ<DAB5><C4BB>˳<EFBFBD>*/
//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;
//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);
//}
//
///*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
//dev_can_write(DEV_NO_CAN0, PPUOff, 8);
//PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
//tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
///*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
//PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
//PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
//
//PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
//PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
//return;
////}
////else if (POSE_OK==tmpConst->WorkModeChangeAuto)
////{
/////*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
////dev_can_write(DEV_NO_CAN0, PPUOff, 8);
////PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
////tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/////*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
////PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
////PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
////
////PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_IN;
////PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
////return;
////}
////else
////{
////;
////}
//}
}
}
if((POSE_OK == pOrb->OrbDataIn_OK) && (ATTMOD_ORBITCTL == pMod->WorkMode))
{
/*<2A>жϵ<D0B6>ǰʱ<C7B0>䣬<EFBFBD>Ƿ<EFBFBD><C7B7>ڹ<EFBFBD><DAB9><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
if (!(
(SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime-2)) &&
(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength+2))
))
{
/*<2A><><EFBFBD>ڵĻ<DAB5><C4BB>˳<EFBFBD>*/
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);
}
/*<2A><><EFBFBD><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>ǰ<EFBFBD>ĵĹ<C4B5>PPU<50><55><EFBFBD><EFBFBD>*/
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
PPPU->PPUOUT_Sta = POSE_NO;/*<2A><><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>ر<EFBFBD>־Ϊ<D6BE><CEAA>*/
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
/*2.5S<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PPU<EFBFBD><EFBFBD>Դ*/
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_OK;
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
return;
}
}
}
/*******************************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺ID <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>٣<EFBFBD>RPM<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*******************************************************/
void SetWheel(UINT8 WheelId, TYPE_CAL Value, UINT8 CmdType)
{
UINT8 i = 0;
UINT8 sum = 0;
UINT16 tmpLen = 0 ;
UINT16 WhlLen = 8;
INT16 tmpI =0 ;
UINT8 tmpWhl_order[8] = {0X01,WHL_CMD_CODE_RATE,0xFF,0x0,0x0,0x0,0x0,0x0}; //<2F><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sAttDataPrePara_t *pData = NULL;
AttCmdDsp_t *pCmdDsp = NULL;
sDevInfo * tmpDevInfo = NULL;
WhlPara_t *PWhl = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL ==tmpAtt) || (NULL ==tmpConst))
return;
pData = &tmpAtt->sPerPara;
pCmdDsp = &tmpAtt->sCmdDspPara;
tmpDevInfo = GETDEVINFO(DEV_NO_UART8 + WheelId - 1);
PWhl = &tmpAtt->sPerPara.WhlPara;
tmpWhl_order[0] = tmpConst->Whl_ID[WheelId -1];
if (WHL_CMD_CODE_RATE == CmdType) /* ת<><D7AA>ָ<EFBFBD><D6B8> */
{
Value = Value * RPM2RADS;
DoSetFloatTo8(Value,tmpWhl_order+3);
sum = 0;
for(i = 0; i < 7; i++)
{sum += tmpWhl_order[i];}
sum = 0xFF - sum;
tmpWhl_order[7]= sum;
tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
if(tmpLen != WhlLen)/*<2A><>ʾ<EFBFBD><CABE><EFBFBD>ͳɹ<CDB3>*/
{
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 5, 1 );
}
}
else if ( WHL_CMD_CODE_TRQ == CmdType ) /* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> */
{
tmpWhl_order[1]= WHL_CMD_CODE_TRQ;
if (POSE_ABSF(Value) < 0.1)
{
DoSetFloatTo8(Value,tmpWhl_order+3);
sum = 0;
for(i = 0; i < 7; i++)
{sum += tmpWhl_order[i];}
sum = 0xFF - sum;
tmpWhl_order[7]= sum;
tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
if(tmpLen != WhlLen)/*<2A><>ʾ<EFBFBD><CABE><EFBFBD>ͳɹ<CDB3>*/
{
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 6, 1 );
}
}
else
{
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 6, 1 );
}
}
else if ( WHL_CMD_CODE_CURR == CmdType) /* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> */
{
tmpWhl_order[1]= WHL_CMD_CODE_CURR;
if (POSE_ABSF(Value) < 0.1)
{
tmpWhl_order[3] = 0xFF;
tmpWhl_order[4] = 0xFF;
tmpI =(INT16)(Value / 0.036 * 1000000.0 /103.0);
DoSetInt16To8(tmpI,tmpWhl_order+5);
sum = 0;
for(i = 0; i < 7; i++)
{sum += tmpWhl_order[i];}
sum = 0xFF - sum;
tmpWhl_order[7]= sum;
tmpLen = UartWrite(tmpDevInfo->iConNO, tmpWhl_order, WhlLen);
if(tmpLen != WhlLen)/*<2A><>ʾ<EFBFBD><CABE><EFBFBD>ͳɹ<CDB3>*/
{
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 7, 1 );
}
}
else
{
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 7, 1 );
}
}
//ң<><D2A3><EFBFBD>·<EFBFBD><C2B7><EFBFBD>¼
if( WHL_CMD_CODE_TRQ== CmdType)
{
pCmdDsp->Wheel_LJCmd[WheelId-1]=Value;
if (tmpConst->Whl_J[WheelId-1] > POSE_ZERO)
{
pCmdDsp->Whl_Torque[WheelId-1]= pData->WhlPara.Whl_Rate[WheelId-1] + Value * 0.5 * RADS2RPM /tmpConst->Whl_J[WheelId-1];
}
}
else if( WHL_CMD_CODE_RATE== CmdType)
{
pCmdDsp->Whl_Torque[WheelId-1]=Value * RADS2RPM ;
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><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 ZKWhlCmdDsp(void)
{
UINT8 i =0x00;
UINT8 j =0x00;
UINT8 tmpReversibleFlg =0x00;
UINT8 UnUseWheelId =0x00;//<2F><>¼<EFBFBD>²<EFBFBD>ʹ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD>֣<EFBFBD>ָ<EFBFBD><EFBFBD>͸<EFBFBD><CDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1234<33><34>ӦABCD
TYPE_CAL tmpCu[9]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};//<2F><><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TYPE_CAL tmpCp[9]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
TYPE_CAL tmpUse = 0.0;
TYPE_CAL tmpTwc[3]={0.0,0.0,0.0};//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵĿ<D6B5><C4BF><EFBFBD>ָ<EFBFBD><D6B8>
TYPE_CAL tmpTwi_Max =0.0;
TYPE_CAL tmpTwc_Max =0.0;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst = NULL;
sAttModPara_t *pMod = NULL;
WhlPara_t *PWhl = NULL;
AttIMPTPara_t *pImpt = NULL;
sAttDataPrePara_t *pData = NULL;
sAttCtlPara_t *pCtl = NULL;
AttCmdDsp_t *pCmdDsp = NULL;
sAttErrCtlPara_t *pREcheck = NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL ==tmpAtt) || (NULL ==tmpConst))
{
return;
}
pMod =&tmpAtt->sModePara;
PWhl = &tmpAtt->sPerPara.WhlPara;
pImpt=&tmpAtt->sIMPTPara;
pData = &tmpAtt->sPerPara;
pCtl = &tmpAtt->sCtlPara;
pCmdDsp = &tmpAtt->sCmdDspPara;
pREcheck=&tmpAtt->sErrCtlPara;
if((ATTMOD_RATEDMP == pMod->WorkMode)||(ATTMOD_MagTOSUN == pMod->WorkMode)||(ATTMOD_UNCTL == pMod->WorkMode))
{
return;
}
pREcheck->WhlUseState = 0;
for(i=0;i<4;i++)
{
pREcheck->WhlUseState =(pREcheck->WhlUseState |((POSE_OK == pImpt->Whl_Use[i]) << i));
}
if(count_onebit(pREcheck->WhlUseState,4)<3)
{
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>tmpCp
//XYZS/XYZ
UnUseWheelId=0;
if((0x0f ==pREcheck->WhlUseState)||(0x07 == pREcheck->WhlUseState))
{
memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][0], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][0], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][0], 3*sizeof(TYPE_CAL));
UnUseWheelId=4;
}
//YZS
else if(0x0e == pREcheck->WhlUseState)
{
memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][1], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][1], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][1], 3*sizeof(TYPE_CAL));
UnUseWheelId=1;
}
//XZS
else if(0x0d == pREcheck->WhlUseState)
{
memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][1], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][1], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][1], 3*sizeof(TYPE_CAL));
tmpCu[0]=tmpConst->AttCmdFlashPara.M_Wheel[0][0];
tmpCu[3]=tmpConst->AttCmdFlashPara.M_Wheel[1][0];
tmpCu[6]=tmpConst->AttCmdFlashPara.M_Wheel[2][0];
UnUseWheelId=2;
}
//XYS
else if(0x0b == pREcheck->WhlUseState)
{
memcpy(tmpCu, &tmpConst->AttCmdFlashPara.M_Wheel[0][0], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+3, &tmpConst->AttCmdFlashPara.M_Wheel[1][0], 3*sizeof(TYPE_CAL));
memcpy(tmpCu+6, &tmpConst->AttCmdFlashPara.M_Wheel[2][0], 3*sizeof(TYPE_CAL));
tmpCu[2]=tmpConst->AttCmdFlashPara.M_Wheel[0][3];
tmpCu[5]=tmpConst->AttCmdFlashPara.M_Wheel[1][3];
tmpCu[8]=tmpConst->AttCmdFlashPara.M_Wheel[2][3];
UnUseWheelId=3;
}
tmpUse=1.0e-4;
InvsymMatrix3(tmpCu, tmpCp, &tmpReversibleFlg, &tmpUse);
if(!tmpReversibleFlg)
{
return;
}
//<2F><><EFBFBD>㷴<EFBFBD><E3B7B4><EFBFBD><EFBFBD><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD>ָ<EFBFBD><D6B8>(<28>޷<EFBFBD>)tmpTwc
MatrixProductHL(tmpCp, pCtl->WheelCtrlResult, tmpTwc, 3, 3, 1);
for(i=0;i<3;i++)
{
if(POSE_ABSF(tmpTwc[i])>POSE_ABSF(tmpTwc_Max))
{
tmpTwi_Max=tmpTwc[i];
tmpTwc_Max=tmpTwc[i];
}
}
if(POSE_ABSF(tmpTwc_Max)>0.1)
{
if (tmpTwc_Max > 0)
{
tmpTwc_Max=0.1;
}
else
{
tmpTwc_Max=-0.1;
}
if(POSE_ABSF(tmpTwi_Max) > POSE_ZERO)
{
for(i=0;i<3;i++)
tmpTwc[i]=tmpTwc[i]*tmpTwc_Max/tmpTwi_Max;
}
}
memcpy(pCmdDsp->Wheel_TwcCmd,tmpTwc,3*sizeof(TYPE_CAL));
//<2F><><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>ָ<EFBFBD><D6B8>
if(0 != UnUseWheelId)
{
j=0;
for(i=1;i<=4;i++)
{
if(i!=UnUseWheelId)
{
if (WHL_CMD_CODE_RATE == PWhl->Whl_WorkMode[i-1])
{
if (tmpConst->Whl_J[j] > POSE_ZERO)
{
//tmpTwc[j] = pData->WhlPara.Whl_Rate[j] + tmpTwc[j] * 0.5 /PWhl->Whl_J[j]* RADS2RPM ;
tmpTwc[j] = pData->WhlPara.Whl_Rate[i-1] + tmpTwc[j] * 0.5 /tmpConst->Whl_J[j]* RADS2RPM ;
}
}
else if (WHL_CMD_CODE_TRQ == PWhl->Whl_WorkMode[i-1])
{
;
}
SetWheel(i,tmpTwc[j],PWhl->Whl_WorkMode[i-1]);
j++;
}
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ZKWhlDCmdDsp(void)
{
UINT8 i = 0x00;
TYPE_CAL tmpT[3] ={0.0,0.0,0.0};
TYPE_CAL tmpWhlM[3] ={0.0,0.0,0.0};
TYPE_CAL tmpWhlRate = 0.0;
sAttPriData *tmpAtt =NULL;
AttCtrlConst_t *tmpConst=NULL;
AttCmdDsp_t *pCmdDsp=NULL;
WhlPara_t *pWhl=NULL;
sAttDataPrePara_t *pData=NULL;
sAttErrCtlPara_t *pREcheck=NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if(NULL ==tmpAtt || NULL ==tmpConst)
{
return;
}
for(i=0;i<3;i++)
{
tmpWhlM[i]= tmpConst->AttCmdFlashPara.M_Wheel[i][3];
}
//pImpt=&tmpAtt->sIMPTPara;
pCmdDsp = &tmpAtt->sCmdDspPara;
pWhl=&tmpAtt->sPerPara.WhlPara;
pData = &tmpAtt->sPerPara;
pREcheck=&tmpAtt->sErrCtlPara;
if(0x0f != pREcheck->WhlUseState)
return;
if(POSE_ABSF(tmpConst->WheelD_NomSpeed-pWhl->Whl_Rate[3])<tmpConst->WheelD_SpeedErrThr)
{
//<2F><><EFBFBD><EFBFBD>Dת<44><D7AA>ָ<EFBFBD><D6B8>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
pCmdDsp->WheelD_RotatSpd_Cmd=tmpConst->WheelD_NomSpeed;
//<2F><><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>Dת<44><D7AA>ָ<EFBFBD><D6B8>
SetWheel(4,pCmdDsp->WheelD_RotatSpd_Cmd,WHL_CMD_CODE_RATE);
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
pCmdDsp->WheelD_MomOfForce_Cmd=tmpConst->WheelD_Default;
for(i=0;i<3;i++)
{
tmpT[i]=pCmdDsp->Wheel_TwcCmd[i]+tmpWhlM[i]*tmpConst->WheelD_Default;
if(POSE_ABSF(tmpT[i])>tmpConst->WheelD_AllDefault)
{
pCmdDsp->WheelD_MomOfForce_Cmd=0;
break;
}
}
//<2F><><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>D<EFBFBD><44><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
tmpWhlRate = tmpConst->WheelD_NomSpeed - pData->WhlPara.Whl_Rate[3];
if (tmpWhlRate > 0.0)
{
tmpWhlRate = 1.0;
}
else
{
tmpWhlRate = - 1.0;
}
pCmdDsp->WheelD_RotatSpd_Cmd = pData->WhlPara.Whl_Rate[3] + tmpWhlRate * pCmdDsp->WheelD_MomOfForce_Cmd * 0.5 /tmpConst->Whl_J[3] * RADS2RPM ;
SetWheel(4,pCmdDsp->WheelD_RotatSpd_Cmd,WHL_CMD_CODE_RATE);
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>422<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
***********************************************/
void ActuatorCmdDsp(void)
{
sAttPriData *tmpAtt = NULL;
sAttModPara_t *pMod= NULL;
WhlPara_t *pWhl= NULL;
AttCmdDsp_t *pCmdDsp= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
return;
pMod =&tmpAtt->sModePara;
pWhl=&tmpAtt->sPerPara.WhlPara;
pCmdDsp = &tmpAtt->sCmdDspPara;
#ifdef MINMODULE_TYPE
switch (pMod->WorkMode)
{
case ATTMOD_MagTOSUN:
case ATTMOD_RATEDMP:
{
if(pCmdDsp->Wheel_Cmd_Cnt<60000)
pCmdDsp->Wheel_Cmd_Cnt++;
if (pCmdDsp->Wheel_Cmd_Cnt <= 3)
{
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);
}
}
break;
default:
break;
}
#else
switch (pMod->WorkMode)
{
case ATTMOD_UNCTL:
break;
case ATTMOD_WhlTOSUN: //(0x33)<29>ֿض<D6BF><D8B6>ն<EFBFBD><D5B6><EFBFBD>
case ATTMOD_ONLTOEARTH: //(0x55)<29>Եض<D4B5><D8B6><EFBFBD>
case ATTMOD_WHEELTOEARTH: //(0x66)<29><>̬<EFBFBD>Ե<EFBFBD>
case ATTMOD_ATTAJUSTPRE: //(0x77)<29><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
case ATTMOD_ORBITCTL: //(0x88)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case ATTMOD_ATTAJUST: //(0xAA)<29><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
case ATTMOD_ONLTOTAR: //(0xBB)<29><>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>
if (pWhl->Whl_UseCnt >= 3)
{
ZKWhlCmdDsp();
ZKWhlDCmdDsp();
}
break;
case ATTMOD_NOCOLL:
if (pWhl->Whl_UseCnt >= 3)
{
ZKWhlCmdDsp();
}
break;
case ATTMOD_RATEDMP: //ZMX20230920ADD
case ATTMOD_MagTOSUN:
{
if(pCmdDsp->Wheel_Cmd_Cnt<60000)
pCmdDsp->Wheel_Cmd_Cnt++;
if (pCmdDsp->Wheel_Cmd_Cnt <= 3)
{
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);
}
}
break;
default:
break;
}
#endif
}
/********************************************************************
<EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ش<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
<EFBFBD><EFBFBD><EFBFBD>룺
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*********************************************************************/
void MTOcOutProc(TYPE_CAL *MTIn,TYPE_CAL *MTOut, UINT8 *MTOnOff, UINT8 *MTDirect)
{
UINT8 i= 0x00;
TYPE_CAL tmpMagMoment = 0.0;
//TYPE_CAL TIME = 1.0;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt)||(NULL==tmpConst))
{
return;
}
for (i = 0; i < MT_NUM; ++i)
{
tmpMagMoment = MTIn[i];
if (POSE_ABSF(tmpMagMoment) > tmpConst->MTOutXYZLimit[i]) //>P0
{
MTOnOff[i] = MT_ON;
if (tmpMagMoment > tmpConst->MTOutXYZLimit[i])
{
MTDirect[i] = MT_DIRECT_POS; //<2F><><EFBFBD><EFBFBD>
MTOut[i] = 500* (TYPE_CAL)tmpConst->MTCtrlTIME;
}
else
{
MTDirect[i] = MT_DIRECT_NEG; //<2F><><EFBFBD><EFBFBD>
MTOut[i] = 500* (TYPE_CAL)tmpConst->MTCtrlTIME;
}
}
else
{
MTOnOff[i] = MT_ON;
if (tmpMagMoment< 0)
{
MTDirect[i] = MT_DIRECT_NEG;
}
else
{
MTDirect[i] = MT_DIRECT_POS;
}
MTOut[i] = POSE_ABSF( tmpMagMoment )/tmpConst->MTOutXYZLimit[i] * 500.0* (TYPE_CAL)tmpConst->MTCtrlTIME;
}
if (MTOut[i] < 100.0)
{
MTOnOff[i] = MT_OFF;
}
}
}
/********************************************************************
<EFBFBD><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 ZKMtCmdDsp(void)
{
UINT8 i = 0x00;
UINT8 chn = 0x00;
UINT8 dir = 0x00;
UINT8 tmpReversibleFlg = 0x00;
UINT16 delay_ms = 0;
UINT32 pulse_ms = 0;
UINT32 tmpCNT = 0;
TYPE_CAL tmpMTin[MT_NUM] ={0.0,0.0,0.0,0.0};
TYPE_CAL tmpM_MAGCtrl[3][4] ={{0.0F,0.0F,0.0F,0.0F}, {0.0F,0.0F,0.0F,0.0F}, {0.0F,0.0F,0.0F,0.0F}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
TYPE_CAL tmpM_MAGCtrlFP1[4][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
TYPE_CAL tmpM_MAGCtrlFP[4][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TYPE_CAL tmpM_MAGAA[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
TYPE_CAL tmpM_MAGAANI[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TYPE_CAL tmpUse = 0.0001F;
sAttPriData *tmpAtt = NULL;
AttCtrlConst_t *tmpConst= NULL;
MtPara_t *pMt= NULL;
sAttCtlPara_t *pCtrl= NULL;
AttIMPTPara_t *pImpt= NULL;
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt)||(NULL==tmpConst))
{
return;
}
pMt=&tmpAtt->sPerPara.MtPara;
pCtrl=&tmpAtt->sCtlPara;
pImpt=&tmpAtt->sIMPTPara;
memset(&tmpM_MAGCtrl[0][0], 0, 12*sizeof(TYPE_CAL));
if (POSE_OK == pImpt->MTUseStatus[0])
{
tmpM_MAGCtrl[0][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][0];
tmpM_MAGCtrl[1][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][0];
tmpM_MAGCtrl[2][0]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][0];
}
if (POSE_OK == pImpt->MTUseStatus[1])
{
tmpM_MAGCtrl[0][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][1];
tmpM_MAGCtrl[1][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][1];
tmpM_MAGCtrl[2][1]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][1];
}
if (POSE_OK == pImpt->MTUseStatus[2])
{
tmpM_MAGCtrl[0][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][2];
tmpM_MAGCtrl[1][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][2];
tmpM_MAGCtrl[2][2]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][2];
}
if (POSE_OK == pImpt->MTUseStatus[3])
{
tmpM_MAGCtrl[0][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[0][3];
tmpM_MAGCtrl[1][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[1][3];
tmpM_MAGCtrl[2][3]=tmpConst->AttCmdFlashPara.M_MAGCtrl[2][3];
}
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
MatrixTransposeHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], 3, 4);
//<2F><>λ<EFBFBD><CEBB>
MatrixProductHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], tmpM_MAGAA[0], 3, 4, 3);
//<2F><><EFBFBD><EFBFBD>
//tmpUse=1.0e-4;
tmpUse=0.0001F;
InvsymMatrix3(tmpM_MAGAA[0], tmpM_MAGAANI[0], &tmpReversibleFlg, &tmpUse);
if(!tmpReversibleFlg)
return;
MatrixProductHL(tmpM_MAGCtrlFP1[0], tmpM_MAGAANI[0], tmpM_MAGCtrlFP[0], 4, 3, 3);
MatrixProductHL(tmpM_MAGCtrlFP[0], pCtrl->MagCtrlResult, tmpMTin, 4, 3, 1);
pCtrl->MagCtrlXYZZ[0] =tmpMTin[0];
pCtrl->MagCtrlXYZZ[1] =tmpMTin[1];
pCtrl->MagCtrlXYZZ[2] =tmpMTin[2];
pCtrl->MagCtrlXYZZ[3] =tmpMTin[3];
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
MTOcOutProc(tmpMTin, pMt->MTCtrlOutP0, pMt->MTOnOff, pMt->MTDirect );
//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
tmpCNT = pMt-> MTCtrlTime % 10;
//if (((tmpCNT >=1)&&(tmpCNT <= (UINT32)tmpConst->MTCtrlTIME))||( 0x88 == pMag->MagUseSwich))
if ((tmpCNT >=1)&&(tmpCNT <= (UINT32)tmpConst->MTCtrlTIME))
{
if (tmpCNT ==1)
{
for (i = 0; i < MT_NUM; i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>ر<EFBFBD>
//chn:ͨ<><CDA8><EFBFBD>ţ<EFBFBD>0x11:X, 0x22:Y, 0x33:Z<><5A>0x44:S
//dir:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//delay_ms:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Чֵ 1--1000<30><30>
//pluse_ms:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>Чֵ 1--1800000<30><30>
chn = chn + 0x11;
if (MT_DIRECT_POS == pMt->MTDirect[i])
{
dir = 0;
}
else if (MT_DIRECT_NEG == pMt->MTDirect[i])
{
dir = 1;
}
pulse_ms = (UINT32)(pMt->MTCtrlOutP0[i]);
if (MT_ON == pMt->MTOnOff[i])
{
magnetic_out( chn, dir, delay_ms, pulse_ms);
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>ر<EFBFBD>Ϊ0
dir = 0;
pulse_ms = 0;
magnetic_out( chn, dir, delay_ms, pulse_ms);
}
}
}
}
else
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>ر<EFBFBD>Ϊ0
for (i = 0; i < MT_NUM; i++)
{
chn = chn + 0x11;
dir = 0;
pulse_ms = 0;
magnetic_out( chn, dir, delay_ms, pulse_ms);
}
}
}
/***********************************************
˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><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 ZKCmdDspAct(void)
{
#ifdef MINMODULE_TYPE
//ָ<><D6B8><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
ZKCtrlCmdInit();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZKMtCmdDsp();
//<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>
ActuatorCmdDsp();
#else
//ָ<><D6B8><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
ZKCtrlCmdInit();
//<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>
ActuatorCmdDsp();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZKMtCmdDsp();
//PPU
ZKPPUCmdDsp();
#endif
}
Reference in New Issue Copy Permalink