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

460
RAG-TEST-TOOLS/PrjAttCtrlMng/AttModeSel.c Normal file
View File

@@ -0,0 +1,460 @@
/*
* Created: 2022/11/4 11:02:40
* Author: wangzk zhengmengxing
*/
#include "..\PrjCommon\CommonDef.h"
//#include "..\PrjCommon\BlockRing.h"
#include "..\PrjCommon\DataStorDefine.h"
#include "..\PrjCommon\DevDefine.h"
//#include "..\PrjCommon\DevDrvs.h"
//#include "..\PrjPowCtrlMng\PowCtlMng.h"
#include "AttMath.h"
#include "AttCtrlMain.h"
/********************************************************************/
//extern sAttPriData *m_pZKPriInfo;
/***********************************************
说明：姿控工作模式选择初始化,在模式切换时候进行部分参数初始化
输入：无
输出：
返回:
注意: 模式切换时候调用一次
***********************************************/
void ZKModChgInit(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCmdDsp_t *pCmdDsp= NULL;
if(NULL == tmpAtt)
{return;}
pCmdDsp = &tmpAtt->sCmdDspPara;
//切换到新模式后，将相关计数器清零
#ifdef MINMODULE_TYPE
tmpAtt->sModePara.WModCnt2[0]=0; //2、速率阻尼
tmpAtt->sModePara.WModCnt2[1]=0; //2、速率阻尼
#else
//pAttCtlPara->sWModePara.WModCnt1=0; //1、碰撞规避模式
tmpAtt->sModePara.WModCnt2[0]=0; //2、速率阻尼
tmpAtt->sModePara.WModCnt2[1]=0; //2、速率阻尼
tmpAtt->sModePara.WModCnt3=0; //3、轮控对日定向
tmpAtt->sModePara.WModCnt4=0; //4、磁控对日定向
tmpAtt->sModePara.WModCnt5=0; //5、对地定向
tmpAtt->sModePara.WModCnt6=0; //6、稳态对地
tmpAtt->sModePara.WModCnt7=0; //7、轨控前调姿
tmpAtt->sModePara.WModCnt8=0; //8、轨道控制
tmpAtt->sModePara.WModCntA=0; //11、对目标定向
pCmdDsp->Wheel_Cmd_Cnt = 0; /*飞轮电流指令发送计数*/
tmpAtt->sCtlPara.PD2PIDCnt=0; //PD转PID计数
tmpAtt->sCtlPara.CtrlPD2PID=0; //
memset(tmpAtt->sCtlPara.SumAerrPre, 0.0, 3*sizeof(TYPE_CAL)); //PID积分器清零
tmpAtt->sErrCtlPara.AttLoseCnt=0;//模式切换时不正常计数清零
tmpAtt->sErrCtlPara.AttExCnt=0;
tmpAtt->sErrCtlPara.AttExFlg[0]=0x00;
tmpAtt->sErrCtlPara.AttExFlg[1]=0x00;
tmpAtt->sErrCtlPara.AttExFlg[2]=0x00;
tmpAtt->sErrCtlPara.AttUnStaCnt=0;
tmpAtt->sErrCtlPara.AttModCnt=0;
tmpAtt->sErrCtlPara.AttUnSta[0] = 0;
tmpAtt->sErrCtlPara.AttUnSta[1] = 0;
tmpAtt->sErrCtlPara.AttUnSta[2] = 0;
tmpAtt->sErrCtlPara.YawCnt = 0;
#endif
}
/***********************************************
说明：设置当前模式
输入：工作模式
输出：当前工作模式
返回: 无
注意:
***********************************************/
void ZKModDoSet(UINT8 newWMode)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
if(NULL == tmpAtt)
{return;}
if (newWMode == tmpAtt->sModePara.WorkMode)
{return;}
tmpAtt->sModePara.WorkModePre = tmpAtt->sModePara.WorkMode;
tmpAtt->sModePara.WorkMode = newWMode;
tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;
}
/***********************************************
说明：姿控工作模式选择初始化,参数初始化
注意: 开始调用一次
***********************************************/
void ZKModInit(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
AttCmdDsp_t *pCmdDsp= NULL;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod =&tmpAtt->sModePara;
pCmdDsp = &tmpAtt->sCmdDspPara;
//工作模式初始化为碰撞规避
#ifdef MINMODULE_TYPE
pWMod->WorkMode = ATTMOD_RATEDMP;
#else
pWMod->WorkMode = ATTMOD_NOCOLL;
#endif
pWMod->ReDataBK = POSE_NO;
pWMod->ATTFlashSwitch= POSE_NO;
pWMod->Mod2UnctlFirst=0x55;
pWMod->WorkModePre = ATTMOD_NOCOLL;
tmpAtt->sModePara.ModeSwitchFlg = POSE_OK;
pWMod->ZKPowOnFirst =0; //首次分离/计算机冷启动
pWMod->ZKPowOnStep =0; //首次分离/计算机冷启动加电序列
//pWMod->ModeTRATEDMP =0;
tmpConst->AttCmdFlashPara.WorkModeChangeAuto=POSE_NO; //二次点火允许标志，默认禁止
//定义连续判断时长（单位为s）（可上注）
//tmpConst->judge_time1 =200;
tmpConst->judge_time1 =180; //ZMX20240115
tmpConst->judge_time2 = 60;
tmpConst->judge_time3=30;
tmpConst->AttCmdFlashPara.judgeWait_time=6000;
tmpConst->judge_time4 = 0;
tmpConst->judge_time5 = 30;
tmpConst->judge_time6 = 0;
tmpConst->judge_time7 = 0;
tmpConst->judge_time8 = 0;
tmpConst->judge_time9 = 30;
tmpConst->judge_timeA = 0;
tmpConst->judge_timeB = 120;
//判断阈值初始化（可上注）
tmpConst->RateThr_2T3 = 0.3f * ANG2RADIAN; //0.3°/s
tmpConst->HWHLThr_2T3 = 0.5f;
tmpConst->AngThr_3TB = 2.0f * ANG2RADIAN; //2.0° --对日定向转无控模式角度阈值
tmpConst->RateThr_3TB = 0.1f * ANG2RADIAN; //0.1°/s--对日定向转无控模式角速度阈值
tmpConst->AngThr_5T6 = 2.0f * ANG2RADIAN; //对地定向模式切换到稳定对地模式角度阈值
tmpConst->RateThr_5T6 = 0.1f * ANG2RADIAN; //对地定向模式切换到稳定对地模式角速度阈值
tmpConst->AngThr_9TA = 2.0f * ANG2RADIAN;
tmpConst->RateThr_9TA = 0.1f * ANG2RADIAN;
//计数器初始化
pWMod->WModCnt2[0]=0; //2、速率阻尼
pWMod->WModCnt2[1]=0; //2、速率阻尼
pWMod->WModCnt3=0; //3、轮控对日定向
pWMod->WModCnt4=0; //4、磁控对日定向
pWMod->WModCnt5=0; //5、对地定向
pWMod->WModCnt6=0; //6、稳态对地
pWMod->WModCnt7=0; //7、轨控前调姿
pWMod->WModCnt8=0; //8、轨道控制
pWMod->WModCntA=0; //11、对目标定向
pWMod->WModCntB= 0;
pWMod->WModCnt1 = 0;
pCmdDsp->Wheel_Cmd_Cnt = 0;
}
/***********************************************
说明：姿控工作模式中计数的管理，
输入：当前工作模式，闭环姿态数据
输出：模式计数值
返回:
注意: 每0.5秒调用一次
***********************************************/
void ZKDoModCal(void)
{
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
sAttDeterPara_t *pDeter =NULL;
WhlPara_t *PWhl =NULL;
//AttIMPTPara_t *pImpt;
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod=&tmpAtt->sModePara;
pDeter=&tmpAtt->sDeterPara;
PWhl = &tmpAtt->sPerPara.WhlPara;
//pImpt=&tmpAtt->sIMPTPara;
#ifdef MINMODULE_TYPE
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP://速率阻尼转对日判断计数
{
//滚动、俯仰和偏航各轴的惯性角速率绝对值连续10s不大于0.3°/s&&闭环姿态数据有效
if(
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
(pDeter->CLPAttValid==0x55))
{
if(pWMod->WModCnt2[0]<60000)
{pWMod->WModCnt2[0]++;}
}
else
{
pWMod->WModCnt2[0]=0;
}
//反作用轮卸载角动量和小于0.5Nms
if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
{
if(pWMod->WModCnt2[1]<60000)
{pWMod->WModCnt2[1]++;}
}
else
{
pWMod->WModCnt2[1]=0;
}
break;
}
default:
{
break;
}
}
#else
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP://速率阻尼转对日判断计数
{
//连续60s，闭环姿态数据有效标志为有效，闭环姿态角速度（滚动、俯仰和偏航各轴的惯性姿态角速率），在±0.3°/s内
if(
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_2T3) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_2T3) &&
(0x55 == pDeter->CLPAttValid))
{
if(pWMod->WModCnt2[0]<60000)
{pWMod->WModCnt2[0]++;}
}
else
{
pWMod->WModCnt2[0]=0;
}
/*连续60s，输出转速有效标志有效且反作用轮卸载三轴角动量和模小于0.5Nms*/
//反作用轮卸载角动量和小于0.5Nms
if ((POSE_OK == PWhl->WhlIn_Valid[0])&&(POSE_OK == PWhl->WhlIn_Valid[1])&&(POSE_OK == PWhl->WhlIn_Valid[2])&&(POSE_OK == PWhl->WhlIn_Valid[3])&&(POSE_ABSF(PWhl->Whl_Momentum_Sum)<tmpConst->HWHLThr_2T3))
{
if(pWMod->WModCnt2[1]<60000)
{pWMod->WModCnt2[1]++;}
}
else
{
pWMod->WModCnt2[1]=0;
}
break;
}
case ATTMOD_WhlTOSUN://对日定向转无控模式
//case ATTMOD_MagTOSUN://对日定向转无控模式
{
if((0x55 == pDeter->CLPAttValid) && (0x00 == pWMod->WModSadaOpen) && //帆板展开标志为未展开状态 注意帆板未展开未00，展开为55
//(POSE_ABSF(pDeter->CLPAttAng[1])<=tmpConst->AngThr_3TB) &&
//(POSE_ABSF(pDeter->CLPAttAng[2])<=tmpConst->AngThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_3TB) && //20230926
(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_3TB) &&
(POSE_ABSF(pDeter->CLPAttAngRat[2])<tmpConst->RateThr_3TB))
{
if(pWMod->WModCnt3<60000)
{pWMod->WModCnt3++;}
}
else {pWMod->WModCnt3=0;}
break;
}
case ATTMOD_UNCTL://无控模式转对日定向
{
if (0x55 == pWMod->WModSadaOpen)
{
if(pWMod->WModCntB<60000) //！！！WModCntB需要备份
{pWMod->WModCntB++;}
}
break;
}
case ATTMOD_ONLTOEARTH://对地定向模式切换到稳定对地模式条
{
if((0x55 == pDeter->CLPAttValid) &&
(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_5T6) &&
(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_5T6)
//(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
)
{
if(pWMod->WModCnt5<60000)
{pWMod->WModCnt5++;}
}
else {pWMod->WModCnt5=0;}
break;
}
//case ATTMOD_ATTAJUST://对地定向模式切换到稳定对地模式条
//{
//if((0x55 == pDeter->CLPAttValid) &&
//(POSE_ABSF(pDeter->CLPAttAng[0])<tmpConst->AngThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAng[1])<tmpConst->AngThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAngRat[0])<tmpConst->RateThr_9TA) &&
//(POSE_ABSF(pDeter->CLPAttAngRat[1])<tmpConst->RateThr_9TA)
////(POSE_ABSF(pDeter->CLPAttAngRat[2])<=pWMod->RateThr_SUNTOUNCTL)
//)
//{
//if(pWMod->WModCnt9<60000)
//{pWMod->WModCnt9++;}
//}
//else {pWMod->WModCnt9=0;}
//break;
//}
default:
{
break;
}
}
#endif
}
/***********************************************
说明：姿控工作模式自主切换判断，判断使用哪种模式
输入：当前工作模式，模式指令状态，闭环姿态数据
输出：工作模式
返回: 无
注意:
***********************************************/
void ZKModeSel_Auto(void)
{
UINT8 tmpNewMod =0;
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
sAttModPara_t *pWMod =NULL;
WhlPara_t *PWhl =NULL;
if((NULL==tmpAtt) || (NULL==tmpConst))
{return;}
pWMod=&tmpAtt->sModePara;
PWhl = &tmpAtt->sPerPara.WhlPara;
tmpNewMod=pWMod->WorkMode;
#ifdef MINMODULE_TYPE
switch(pWMod->WorkMode)
{
case ATTMOD_RATEDMP:
{
if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
{
tmpNewMod = ATTMOD_MagTOSUN;
pWMod->WModCnt2[0]=0;
pWMod->WModCnt2[1]=0;
}
break;
}
default:
{
break;
}
}
#else
switch(pWMod->WorkMode)
{
case ATTMOD_NOCOLL://碰撞规避模式
{
if (pWMod->WModCnt1>= (tmpConst->judge_time1*2) || (POSE_OK == pWMod->WModSadaOpen))
{
tmpNewMod = ATTMOD_RATEDMP;
}
break;
}
case ATTMOD_RATEDMP://速率阻尼
{
if((pWMod->WModCnt2[0]>= (tmpConst->judge_time2 * 2))&&((pWMod->WModCnt2[1]>= (tmpConst->judge_time2 * 2))
||((POSE_NO == PWhl->WhlIn_Valid[0])||(POSE_NO == PWhl->WhlIn_Valid[1])||(POSE_NO == PWhl->WhlIn_Valid[2])||(POSE_NO == PWhl->WhlIn_Valid[3]))))
{
if (PWhl->Whl_UseCnt >= 3)
{
tmpNewMod = ATTMOD_WhlTOSUN;
}
else
{
tmpNewMod = ATTMOD_MagTOSUN;
}
}
break;
}
case ATTMOD_WhlTOSUN:
{
if((pWMod->WModCnt3>=(tmpConst->judge_time3 * 2))&&(pWMod->WModCnt1>=(tmpConst->AttCmdFlashPara.judgeWait_time * 2)))
{
//pWMod->Mod2UnctlFirst=0x55;
tmpNewMod = ATTMOD_UNCTL;
}
break;
}
case ATTMOD_UNCTL:
{
if (0x55 == pWMod->Mod2UnctlFirst)
{
if(pWMod->WModCntB>= (tmpConst->judge_timeB* 2)) //连续120s
{
tmpNewMod = ATTMOD_RATEDMP;
pWMod->Mod2UnctlFirst=0x00;
}
}
break;
}
case ATTMOD_ONLTOEARTH:
{
if(pWMod->WModCnt5>=(tmpConst->judge_time5* 2)) //连续30s
{
tmpNewMod = ATTMOD_WHEELTOEARTH;
}
break;
}
default:
{
break;
}
}
#endif
tmpAtt->sModePara.ModeSwitchFlg = POSE_NO;
ZKModDoSet(tmpNewMod);
}
/***********************************************
说明：姿控工作模式选择主函数
注意: 每0.5秒调用一次
***********************************************/
void ZKDoModeAct(void)
{
//AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
//
//if(NULL==tmpConst)
//{
//return;
//}
ZKDoModCal();
ZKModeSel_Auto();
}