927 lines
25 KiB
C
927 lines
25 KiB
C
/*
|
||
* 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"
|
||
|
||
/*******************************************************
|
||
说明：指令分配数据初始化
|
||
*******************************************************/
|
||
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;
|
||
|
||
//飞轮
|
||
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));
|
||
|
||
//磁力矩器
|
||
for (i = 0; i < MT_NUM; i++)
|
||
{
|
||
pMt->MTOnOff[i] = MT_OFF;
|
||
pMt->MTCtrlOutP0[i] = 0.0;
|
||
pMt->MTDirect[i] =MT_DIRECT_POS;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：PPU开关机处理
|
||
输入：
|
||
开关状态 0x55:电推开机，0xAA:电推关机；其他：无操作
|
||
输出：
|
||
***********************************************/
|
||
//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))/*电推关机*/
|
||
//{
|
||
///*当前拍的关动作*/
|
||
//dev_can_write(DEV_NO_CAN0,PPUOff,8);
|
||
///*下一拍的关动作标志*/
|
||
//tmpAtt->sErrCtlPara.AttPPUOffReq = POSE_OK;
|
||
//}
|
||
////else if (0x55 == (OnState))/*电推开机*/
|
||
////{
|
||
/////*当前拍的开动作*/
|
||
////OCOut(OC68_PPU_ON, 0, 16);
|
||
/////*下一拍的关动作标志*/
|
||
////tmpAtt->sErrCtlPara.AttPPUOnReq = POSE_OK;
|
||
////}
|
||
//
|
||
//}
|
||
|
||
///***********************************************
|
||
//说明：轨道控制模块，轨控主函数.
|
||
//输入：上注轨控数据，当前模式
|
||
//输出：PPU开关
|
||
//***********************************************/
|
||
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); //获取当前星上时
|
||
|
||
//if((ATTMOD_ATTAJUSTPRE == pMod->WorkMode) || (ATTMOD_ORBITCTL == pMod->WorkMode))/*轨控期间*/
|
||
if(ATTMOD_ORBITCTL == pMod->WorkMode)/*轨控期间*/
|
||
{
|
||
if (pImpt->PPUUseStatus == POSE_NO)/*电推进状态为不可用*/
|
||
{
|
||
//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))
|
||
{
|
||
/* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
|
||
/*轨控异常，当前拍的关PPU动作*/
|
||
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
/*2.5S后关PPU电源*/
|
||
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)
|
||
{
|
||
/*判断当前时间，是否在轨控时间段内*/
|
||
if (((SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime+30)) &&
|
||
(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength-30))
|
||
))
|
||
{
|
||
if (ATTMOD_ORBITCTL != pMod->WorkMode)
|
||
{
|
||
/*不在的话退出*/
|
||
//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))
|
||
//{
|
||
///* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
|
||
//BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
//}
|
||
|
||
/*轨控异常，当前拍的关PPU动作*/
|
||
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
/*2.5S后关PPU电源*/
|
||
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))
|
||
////{
|
||
///*不在的话退出*/
|
||
//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))
|
||
//{
|
||
///* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
|
||
//BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
//}
|
||
//
|
||
///*轨控异常，当前拍的关PPU动作*/
|
||
//dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
//PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
//tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
///*2.5S后关PPU电源*/
|
||
//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)
|
||
////{
|
||
/////*轨控异常，当前拍的关PPU动作*/
|
||
////dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
////PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
////tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
/////*2.5S后关PPU电源*/
|
||
////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))
|
||
{
|
||
/*判断当前时间，是否在轨控时间段内*/
|
||
if (!(
|
||
(SatTime[0]>=(pOrb->OrbCtrlPackCur.OrbCtrl_StartTime-2)) &&
|
||
(SatTime[0]<= (pOrb->OrbCtrlPackCur.OrbCtrl_StartTime + pOrb->OrbCtrlPackCur.OrbCtrl_TimeLength+2))
|
||
))
|
||
{
|
||
/*不在的话退出*/
|
||
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))
|
||
{
|
||
/* 在姿控的指令处理中 进行 ZKModDoSet(ATT_CMDMODE_MODSUNCAP);*/
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
|
||
/*轨控异常，当前拍的关PPU动作*/
|
||
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
/*2.5S后关PPU电源*/
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/*******************************************************
|
||
说明：发送飞轮指令
|
||
输入：ID 飞轮转速（RPM） 飞轮模式
|
||
输出：
|
||
*******************************************************/
|
||
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}; //飞轮控制指令组包
|
||
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) /* 转速指令 */
|
||
{
|
||
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)/*表示发送成功*/
|
||
{
|
||
PWhl->WhlIn_DataSta[WheelId-1] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[WheelId-1], 5, 1 );
|
||
}
|
||
}
|
||
else if ( WHL_CMD_CODE_TRQ == CmdType ) /* 力矩指令 */
|
||
{
|
||
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)/*表示发送成功*/
|
||
{
|
||
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) /* 电流指令 */
|
||
{
|
||
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)/*表示发送成功*/
|
||
{
|
||
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 );
|
||
}
|
||
}
|
||
|
||
//遥测下发记录
|
||
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 ;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：反作用轮指令分配
|
||
输入：
|
||
输出：
|
||
返回:
|
||
***********************************************/
|
||
void ZKWhlCmdDsp(void)
|
||
{
|
||
UINT8 i =0x00;
|
||
UINT8 j =0x00;
|
||
UINT8 tmpReversibleFlg =0x00;
|
||
UINT8 UnUseWheelId =0x00;//记录下不使用的是哪个飞轮，指令发送给其他三个，1234对应ABCD
|
||
TYPE_CAL tmpCu[9]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};//飞轮分配矩阵
|
||
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};//三个飞轮的控制指令
|
||
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;
|
||
}
|
||
|
||
//计算飞轮分配矩阵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;
|
||
}
|
||
|
||
//计算反作用轮控制指令(限幅)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));
|
||
//发送飞轮指令
|
||
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++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：配平轮指令分配
|
||
***********************************************/
|
||
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)
|
||
{
|
||
//飞轮D转速指令为标称转速
|
||
pCmdDsp->WheelD_RotatSpd_Cmd=tmpConst->WheelD_NomSpeed;
|
||
//发送飞轮D转速指令
|
||
SetWheel(4,pCmdDsp->WheelD_RotatSpd_Cmd,WHL_CMD_CODE_RATE);
|
||
}
|
||
else
|
||
{
|
||
//计算配平轮力矩指令
|
||
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;
|
||
}
|
||
}
|
||
//发送飞轮D力矩指令
|
||
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);
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：飞轮指令分配，反作用轮通过422控制
|
||
***********************************************/
|
||
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)轮控对日定向
|
||
case ATTMOD_ONLTOEARTH: //(0x55)对地定向
|
||
case ATTMOD_WHEELTOEARTH: //(0x66)稳态对地
|
||
case ATTMOD_ATTAJUSTPRE: //(0x77)轨控前调姿
|
||
case ATTMOD_ORBITCTL: //(0x88)轨道控制
|
||
case ATTMOD_ATTAJUST: //(0xAA)定向前调姿
|
||
case ATTMOD_ONLTOTAR: //(0xBB)对目标定向
|
||
|
||
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
|
||
}
|
||
|
||
/********************************************************************
|
||
功能：开关磁力矩器限幅
|
||
输入：
|
||
输出：
|
||
*********************************************************************/
|
||
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; //正开
|
||
MTOut[i] = 500* (TYPE_CAL)tmpConst->MTCtrlTIME;
|
||
}
|
||
else
|
||
{
|
||
MTDirect[i] = MT_DIRECT_NEG; //负开
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
/********************************************************************
|
||
功能：磁力矩器控制
|
||
输入：
|
||
输出：
|
||
*********************************************************************/
|
||
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}}; //磁力矩器安装矩阵
|
||
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}}; //磁力矩器分配矩阵中间量
|
||
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}}; //磁力矩器分配矩阵
|
||
TYPE_CAL tmpM_MAGAA[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //磁力矩器分配矩阵中间量
|
||
TYPE_CAL tmpM_MAGAANI[3][3]={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}}; //磁力矩器分配矩阵中间量求逆
|
||
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];
|
||
}
|
||
//计算转置
|
||
MatrixTransposeHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], 3, 4);
|
||
//单位阵
|
||
MatrixProductHL(tmpM_MAGCtrl[0], tmpM_MAGCtrlFP1[0], tmpM_MAGAA[0], 3, 4, 3);
|
||
//求逆
|
||
//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];
|
||
|
||
//磁力矩器限幅
|
||
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++)
|
||
{
|
||
//输出磁力矩占控比
|
||
//chn:通道号，0x11:X, 0x22:Y, 0x33:Z，0x44:S
|
||
//dir:方向，0：正向，1：反向
|
||
//delay_ms:启动延时，单位毫秒 （有效值 1--1000）
|
||
//pluse_ms:脉冲宽阔，单位毫秒 （有效值 1--1800000）
|
||
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
|
||
{
|
||
//输出磁力矩占控比为0
|
||
dir = 0;
|
||
pulse_ms = 0;
|
||
magnetic_out( chn, dir, delay_ms, pulse_ms);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//输出磁力矩占控比为0
|
||
for (i = 0; i < MT_NUM; i++)
|
||
{
|
||
chn = chn + 0x11;
|
||
dir = 0;
|
||
pulse_ms = 0;
|
||
magnetic_out( chn, dir, delay_ms, pulse_ms);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
/***********************************************
|
||
说明：指令分配模块主函数，所有动作均指定
|
||
|
||
注意: 分离后启动
|
||
***********************************************/
|
||
void ZKCmdDspAct(void)
|
||
{
|
||
|
||
#ifdef MINMODULE_TYPE
|
||
//指令初始化
|
||
ZKCtrlCmdInit();
|
||
//磁力矩器
|
||
ZKMtCmdDsp();
|
||
//飞轮指令分配主模块
|
||
ActuatorCmdDsp();
|
||
#else
|
||
//指令初始化
|
||
ZKCtrlCmdInit();
|
||
//飞轮指令分配主模块
|
||
ActuatorCmdDsp();
|
||
//磁力矩器
|
||
ZKMtCmdDsp();
|
||
//PPU
|
||
ZKPPUCmdDsp();
|
||
#endif
|
||
}
|
||
|