zhoujl/rag_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加代码知识库；修复文档处理内容；增加API设置

Browse Source
This commit is contained in:
junlan
2026-05-16 20:20:10 +08:00
parent 69b49d28b2
commit 7aa3ce3294
119 changed files with 182273 additions and 793 deletions
Download Patch File Download Diff File Expand all files Collapse all files

926
RAG-TEST-TOOLS/PrjAttCtrlMng/AttCmdAct.c Normal file
View File

@@ -0,0 +1,926 @@
/*
* 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
}