461 lines
13 KiB
C
461 lines
13 KiB
C
/*
|
||
* 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();
|
||
}
|
||
|
||
|