2062 lines
56 KiB
C
2062 lines
56 KiB
C
/*
|
||
* Created: 2022/11/4 11:02:40
|
||
* Author: wangzk zhengmengxing
|
||
*/
|
||
|
||
#include "..\PrjCommon\CommonDef.h"
|
||
#include "..\PrjCommon\DevDefine.h"
|
||
#include "AttMath.h"
|
||
#include "AttCtrlMain.h"
|
||
|
||
/***********************************************
|
||
说明：模块初始化函数,进程开始调用一次
|
||
注意:
|
||
***********************************************/
|
||
void ZKREInit(void)
|
||
{
|
||
UINT8 i =0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
AttIMPTPara_t *pIMU =NULL;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
//WhlPara_t *PWhl;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
//PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
//陀螺
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
pREcheck->GyroIn_PowerOn[i]=POSE_OK;
|
||
pREcheck->Gyro_PowerOnCnt[i]=0;
|
||
//tmpConst->AllowGyroDiagFlg[i]=POSE_OK;
|
||
//tmpConst->AllowGyroDiagFlg[i]=0;
|
||
tmpConst->GyroDiagThr = 100;
|
||
pREcheck->GyroErrCnt[i] =0;
|
||
pREcheck->Gyro_Err_Cnt1[i] =0;
|
||
pREcheck->Gyro_Err_Cnt2[i] =0;
|
||
pREcheck->Gyro_Err_Cnt3[i] =0;
|
||
pREcheck->Gyro_Diag[i] =POSE_OK;
|
||
pREcheck->Gyro_err[i] =POSE_OK;
|
||
tmpConst->AllowGyroONOFF[i] = POSE_OK;//POSE_NO; 20240630
|
||
pIMU->GyroUseState[i] = POSE_OK; //使用状态置0
|
||
//tmpConst->GyroUsePrior[i] = PriorHig;
|
||
//tmpConst->Gyro_ComRESW[i]=0;
|
||
}
|
||
|
||
tmpConst->GyroUsePrior[0] = PriorHig;
|
||
tmpConst->GyroUsePrior[1] = PriorMid;
|
||
tmpConst->GyroUsePrior[2] = PriorLow;
|
||
tmpConst->AttCmdFlashPara.Gyro_ComRESW[0]=0;
|
||
tmpConst->AttCmdFlashPara.Gyro_ComRESW[1]=0;
|
||
tmpConst->AttCmdFlashPara.Gyro_ComRESW[2]=0;
|
||
tmpConst->Gyro_Delay_CntLimt =6; //陀螺开机异常延迟判断计数
|
||
tmpConst->Gyro_Err_CntLimt =8; //陀螺异常判断计数
|
||
tmpConst->Gyro_OK_CntLimt =8; //陀螺正常判断计数
|
||
tmpConst->Gyro_ReSet_CntLimt =5; //陀螺重启总次数
|
||
tmpConst->Gyro_ReSetGAP_CntLimt =10; //陀螺重启间隔拍数
|
||
|
||
//星敏
|
||
for(i=0;i<STAR_NUM;i++)
|
||
{
|
||
pREcheck->ssIn_PowerOn[i]=POSE_OK;
|
||
pREcheck->ss_PowerOnCnt[i]=0;
|
||
//tmpConst->AllowssDiagFlg[i]=POSE_OK;
|
||
tmpConst->AllowssONOFF[i]= POSE_OK;//POSE_NO;20240630
|
||
tmpConst->ssDiagThr = 1200;
|
||
pREcheck->ssErrCnt[i] =0;
|
||
pREcheck->ss_Err_Cnt1[i] =0;
|
||
pREcheck->ss_Err_Cnt2[i] =0;
|
||
pREcheck->ss_Err_Cnt3[i] =0;
|
||
pREcheck->ss_Diag[i] =POSE_OK;
|
||
pREcheck->SS_err[i] =POSE_OK;
|
||
pIMU->SSUseState[i] = POSE_OK; //使用状态置0
|
||
}
|
||
tmpConst->SSUsePrior[0] = PriorHig;
|
||
tmpConst->SSUsePrior[1] = PriorMid;
|
||
tmpConst->SSUsePrior[2] = PriorLow;
|
||
//tmpConst->Ss_DiagCnt_Limt=10;
|
||
tmpConst->Ss_Delay_CntLimt =12; //星敏开机异常延迟判断计数
|
||
//tmpConst->Ss_Err_CntLimt =14; //星敏异常判断计数
|
||
tmpConst->Ss_Err_CntLimt =30; //星敏异常判断计数
|
||
tmpConst->Ss_OK_CntLimt =8; //星敏正常判断计数
|
||
tmpConst->Ss_ReSet_CntLimt =5; //星敏重启总次数
|
||
tmpConst->Ss_ReSetGAP_CntLimt =10; //星敏重启间隔拍数
|
||
|
||
//磁强计
|
||
for(i=0;i<MAG_NUM;i++)
|
||
{
|
||
pREcheck->MagIn_PowerOn[i]=POSE_OK;
|
||
pREcheck->Mag_PowerOnCnt[i] =0; //模拟磁强计加电状态计数
|
||
pREcheck->MagErrCnt[i]=0; //模拟磁强计异常诊断计数,上限60000
|
||
pREcheck->MagNoErrCnt[i]=0;
|
||
pREcheck->Mag_Err_Cnt1[i]=0; //模拟磁强计数据异常计数
|
||
pREcheck->Mag_Err_Cnt2[i]=0; //模拟磁强计故障重启计数
|
||
pREcheck->Mag_Err_Cnt3[i]=0; //模拟磁强计数据正常计数
|
||
pREcheck->Mag_Diag[i]=POSE_OK; //模拟磁强计故障标志
|
||
pREcheck->Mag_err[i]=POSE_OK;
|
||
//tmpConst->AllowMagDiagFlg[i]=POSE_OK; //磁强计异常诊断允许标志。0x55允许0不允许，默认为0x55
|
||
tmpConst->AllowMagONOFF[i]= POSE_OK;//POSE_NO; 20240630 //磁强计异常诊断允许标志。0x55允许0不允许，默认为0x55
|
||
|
||
}
|
||
tmpConst->MagDiagThr= 100; //判断时间阈值,单位s,可上注
|
||
tmpConst->Mag_Delay_CntLimt =12; //磁强计开机异常延迟判断计数
|
||
tmpConst->Mag_Err_CntLimt =14; //磁强计异常判断计数
|
||
tmpConst->Mag_OK_CntLimt =8; //磁强计正常判断计数
|
||
tmpConst->Mag_ReSet_CntLimt = 5; //磁强计重启总次数
|
||
tmpConst->Mag_ReSetGAP_CntLimt =10; //磁强计重启间隔拍数
|
||
|
||
//飞轮
|
||
for(i=0;i<WHEEL_NUM;i++)
|
||
{
|
||
pREcheck->WhlIn_PowerOn[i]=POSE_OK;
|
||
pREcheck->Whl_PowerOnCnt[i]=0;
|
||
//tmpConst->AllowWhlDiagFlg[i] = POSE_OK;
|
||
tmpConst->AllowWhlONOFF[i] = POSE_OK;//POSE_NO;
|
||
//tmpConst->AllowWhlONOFF = 0;
|
||
pREcheck->WhlOutErrCnt[i]= 0;
|
||
pREcheck->WhlTemErrCnt[i]= 0;
|
||
pREcheck->Whl_Err_Cnt1[i]= 0; //飞轮数据异常计数
|
||
pREcheck->Whl_Err_Cnt2[i]= 0; //飞轮故障重启计数
|
||
pREcheck->Whl_Err_Cnt3[i]= 0; //飞轮数据正常计数
|
||
pREcheck->Whl_Err[i]= POSE_OK;
|
||
pREcheck->Whl_Diag[i]=POSE_OK;
|
||
}
|
||
tmpConst->WhlTemperDiagThr = 90;
|
||
tmpConst->WhlDiag_TimeThr = 10;
|
||
tmpConst->WhlDiag_ErrThr=15;
|
||
|
||
tmpConst->Whl_Delay_CntLimt =6; //飞轮开机异常延迟判断计数
|
||
//tmpConst->Whl_Err_CntLimt= 8; //飞轮异常判断计数
|
||
tmpConst->Whl_Err_CntLimt= 16; //飞轮异常判断计数
|
||
//tmpConst->Whl_OK_CntLimt= 8; //飞轮正常判断计数
|
||
tmpConst->Whl_OK_CntLimt= 16; //飞轮正常判断计数
|
||
tmpConst->Whl_ReSet_CntLimt=5; //飞轮重启总次数
|
||
tmpConst->Whl_ReSetGAP_CntLimt=16; //飞轮重启间隔拍数
|
||
|
||
//PPU
|
||
pREcheck->PPU_Err=0;
|
||
//pREcheck->PPUTXT_Err=POSE_OK;
|
||
pREcheck->PPU_Diag=POSE_OK; //PPU故障标志
|
||
pREcheck->PPUOutErrCnt=0; //PPU输出不正常计数
|
||
pREcheck->PPUIn_ACurCnt=0; //阳极电源电流计数
|
||
pREcheck->PPUIn_VCurCnt=0; //阳极电源电压计数
|
||
pREcheck->PPUOUT_FIRECnt=0; //开机点火计数
|
||
pREcheck->PPUOUT_TemperECnt=0; //气瓶管路四路测温点温度
|
||
pREcheck->PPU_Err_Cnt1=0; //PPU数据异常计数
|
||
pREcheck->PPU_Err_Cnt2=0; //PPU故障重启计数
|
||
pREcheck->PPU_Err_Cnt3=0; //PPU数据正常计数
|
||
//PPU
|
||
//tmpConst->AllowPPUDiagFlg= POSE_OK; //PPU异常诊断允许标志。0x55允许0不允许，默认为0x55
|
||
//tmpConst->AllowPPUDiagFlg= 0; //PPU异常诊断允许标志。0x55允许0不允许，默认为0x55
|
||
tmpConst->AllowPPUONOFF= POSE_OK;//POSE_NO; 20240630 //PPU重构允许标志。0x55允许0不允许，默认为0x00
|
||
//tmpConst->SafePPUONOFF=POSE_OK;
|
||
tmpConst->PPU_Delay_CntLimt =6; //磁强计开机异常延迟判断计数
|
||
tmpConst->PPU_Err_CntLimt =8; //磁强计异常判断计数
|
||
tmpConst->PPU_OK_CntLimt =8; //磁强计正常判断计数
|
||
tmpConst->PPU_ReSet_CntLimt =5; //磁强计重启总次数
|
||
tmpConst->PPU_ReSetGAP_CntLimt=10; //磁强计重启间隔拍数
|
||
|
||
tmpConst->PPUTXTimeLimit=15; //电推通信故障时间阈值15s
|
||
tmpConst->PPUTATimeLimit=30; //电推过流故障时间阈值30s
|
||
tmpConst->AttCmdFlashPara.PPUTALimit=22; //电推过流故障阈值2.2
|
||
tmpConst->PPUTVTimeLimit=30; //电推过压故障时间阈值30s
|
||
tmpConst->AttCmdFlashPara.PPUTVLimit=260; //电推过压故障阈值260
|
||
tmpConst->AttCmdFlashPara.PPUTFireLimit=420; //电推点火故障阈值420S
|
||
tmpConst->PPUTemperTimeLimit=5; //电推温度故障时间阈值 5s
|
||
tmpConst->AttCmdFlashPara.PPUHTemperLimit=30; //电推温度故障上限阈值30
|
||
//tmpConst->PPULTemperLimit=10; //电推温度故障下限阈值10
|
||
tmpConst->AttCmdFlashPara.PPULTemperLimit=5; //电推温度故障下限阈值10
|
||
|
||
////姿态基准丢失
|
||
tmpConst->AllowAttLoseDiagFlg = 0x55;//0x00;20240630
|
||
tmpConst->TimeThr_AttLoseDiag[0] =30; //30s
|
||
tmpConst->TimeThr_AttLoseDiag[1] = 60*60; //60min
|
||
tmpConst->TimeThr_AttLoseDiag[2] = 60*60; //60min
|
||
tmpConst->TimeThr_AttLoseDiag[3] = 30; //30s
|
||
tmpConst->TimeThr_AttLoseDiag[4] = 30; //30s
|
||
tmpConst->TimeThr_AttLoseDiag[5] = 30; //30s
|
||
tmpConst->TimeThr_AttLoseDiag[6] = 10; //10s
|
||
tmpConst->TimeThr_AttLoseDiag[7] = 30; //30s
|
||
tmpConst->TimeThr_AttLoseDiag[8] = 30; //30s
|
||
pREcheck->AttLoseCnt=0;
|
||
|
||
//姿态超差判断
|
||
tmpConst->AllowAttExDiagFlg= 0x55;//0x00; 20240630 //姿态超差判断允许标志,默认为0x55
|
||
pREcheck->AttExFlg[0]=0x00;
|
||
pREcheck->AttExFlg[1]=0x00;
|
||
pREcheck->AttExFlg[2]=0x00;
|
||
//tmpConst->TimeThr_AttEx[0]=30; //30s
|
||
tmpConst->TimeThr_AttEx[0]=900; //900s
|
||
tmpConst->TimeThr_AttEx[1]=30; //30s
|
||
tmpConst->TimeThr_AttEx[2]=500; //500s
|
||
tmpConst->TimeThr_AttEx[3]=30; //30s
|
||
//tmpConst->TimeThr_AttEx[4]=30; //30s
|
||
tmpConst->TimeThr_AttEx[4]=2400; //2400s
|
||
tmpConst->TimeThr_AttEx[5]=30; //30s
|
||
tmpConst->TimeThr_AttEx[6]=20; //20s
|
||
tmpConst->TimeThr_AttEx[7]=30; //30s
|
||
tmpConst->TimeThr_AttEx[8]=30; //30s
|
||
// tmpConst->YawCntLimit=1200; //对地偏航/对目标偏航下偏航姿态超差计数
|
||
tmpConst->YawCntLimit=2400; //对地偏航/对目标偏航下偏航姿态超差计数
|
||
|
||
tmpConst-> AngThr_AttEx[0] =0; //不判断
|
||
tmpConst-> AngThr_AttEx[1] =0; //不判断
|
||
tmpConst-> AngThr_AttEx[2] = 0*ANG2RADIAN; //30°
|
||
tmpConst-> AngThr_AttEx[3] = 0*ANG2RADIAN; //30°
|
||
tmpConst-> AngThr_AttEx[4] = 10*ANG2RADIAN; //10°
|
||
tmpConst-> AngThr_AttEx[5] = 0*ANG2RADIAN; //10°
|
||
tmpConst-> AngThr_AttEx[6] = 10*ANG2RADIAN; //10°
|
||
tmpConst-> AngThr_AttEx[7] = 0*ANG2RADIAN; //10°
|
||
tmpConst-> AngThr_AttEx[8] = 10*ANG2RADIAN; //10°
|
||
|
||
tmpConst->RateThr_AttEx[0]=-0.002f*ANG2RADIAN; //-0.002°/s
|
||
tmpConst->RateThr_AttEx[1]=1*ANG2RADIAN; //-0.002°/s
|
||
tmpConst->RateThr_AttEx[2]=4*ANG2RADIAN; //1°/s
|
||
tmpConst->RateThr_AttEx[3]=1*ANG2RADIAN; //1°/s
|
||
tmpConst->RateThr_AttEx[4]=0.5f*ANG2RADIAN; //0.5°/s
|
||
tmpConst->RateThr_AttEx[5]=1*ANG2RADIAN; //0.5°/s
|
||
tmpConst-> RateThr_AttEx[6] = 0.5f*ANG2RADIAN; //10°
|
||
tmpConst-> RateThr_AttEx[7] = 1*ANG2RADIAN; //10°
|
||
tmpConst-> RateThr_AttEx[8] = 0.5f*ANG2RADIAN; //10°
|
||
|
||
memset(pREcheck->AngRateMod_RATEDMP,0,61*sizeof(TYPE_CAL));
|
||
pREcheck->YawCnt=0;
|
||
pREcheck->AttExCnt=0; //姿态超差计数
|
||
|
||
////姿态不稳定判断
|
||
tmpConst->AllowAttStaDiagFlg = 0x55;//0x00; 20240630
|
||
pREcheck->AttUnSta[0] = 0x00;
|
||
pREcheck->AttUnSta[1] = 0x00;
|
||
pREcheck->AttUnSta[2] = 0x00;
|
||
|
||
tmpConst->TimeThr_AttUnSta[0] = 10000; //10000s
|
||
//tmpConst->TimeThr_AttUnSta[1] = 1200; //1200s
|
||
tmpConst->TimeThr_AttUnSta[1] = 1800; //1800s
|
||
tmpConst->TimeThr_AttUnSta[2] = 15000; //15000s
|
||
tmpConst->TimeThr_AttUnSta[3] = 3600; //3600s
|
||
tmpConst->TimeThr_AttUnSta[4] = 10; //10s
|
||
tmpConst->TimeThr_AttUnSta[5] = 10; //10s
|
||
tmpConst->TimeMod_AttUnSta[0] = 0; //0
|
||
tmpConst->TimeMod_AttUnSta[1] = 0; //0
|
||
tmpConst->TimeMod_AttUnSta[2] = 0; //0
|
||
tmpConst->TimeMod_AttUnSta[3] = 0; //0
|
||
//tmpConst->TimeMod_AttUnSta[4] = 1500; //1500s
|
||
tmpConst->TimeMod_AttUnSta[4] = 1800; //1800s
|
||
tmpConst->TimeMod_AttUnSta[5] = 1500; //1500s
|
||
|
||
tmpConst->AngThr_AttUnSta[0] = 0*ANG2RADIAN; //20°
|
||
tmpConst->AngThr_AttUnSta[1] = 20.0*ANG2RADIAN; //20°
|
||
tmpConst->AngThr_AttUnSta[2] = 30.0*ANG2RADIAN; //30°
|
||
tmpConst->AngThr_AttUnSta[3] = 0*ANG2RADIAN; //20°
|
||
tmpConst->AngThr_AttUnSta[4] = 2.0*ANG2RADIAN; //20°
|
||
tmpConst->AngThr_AttUnSta[5] = 2.0*ANG2RADIAN; //20°
|
||
|
||
tmpConst->RateThr_AttUnSta[0] = 0*ANG2RADIAN; //1°/s
|
||
tmpConst->RateThr_AttUnSta[1] = 1.0*ANG2RADIAN; //1°/s
|
||
tmpConst->RateThr_AttUnSta[2] = 1.0*ANG2RADIAN; //0.1°/s
|
||
tmpConst->RateThr_AttUnSta[3] = 0*ANG2RADIAN; //0.1°/s
|
||
tmpConst->RateThr_AttUnSta[4] = 0.5*ANG2RADIAN; //0.5°/s
|
||
tmpConst->RateThr_AttUnSta[5] = 0.1*ANG2RADIAN; //0.1°/s
|
||
|
||
pREcheck->AttUnStaCnt=0;
|
||
pREcheck->AttModCnt=0;
|
||
|
||
}
|
||
|
||
/***********************************************
|
||
说明：单机加\断电处理
|
||
Chl :单机通道号
|
||
Cnt1:异常连续判断拍数
|
||
Cnt2:故障重启次数
|
||
Cnt3:正常连续判断拍数
|
||
err : 异常字
|
||
state :故障字
|
||
***********************************************/
|
||
void ImuPowerOnOff( UINT8 ChlON,UINT8 ChlOFF, UINT8 ONOFF,
|
||
UINT32 *Cnt1 , UINT32 *Cnt2, UINT32 *Cnt3,
|
||
UINT32 Cnt1Limt , UINT32 Cnt2Limt , UINT32 Cnt3Limt ,UINT32 Cnt4Limt ,
|
||
UINT8 err, UINT8 *state)
|
||
{
|
||
UINT8 PulT = 16; //160MS
|
||
UINT8 Type = 0;
|
||
|
||
if (POSE_NO == *state)
|
||
{
|
||
//state=POSE_NO;
|
||
*Cnt1 = 0;
|
||
//*Cnt2 = 0;
|
||
*Cnt3 = 0;
|
||
return;
|
||
}
|
||
|
||
if (POSE_NO == err)
|
||
{
|
||
if(*Cnt1<60000)
|
||
{(*Cnt1) ++;}
|
||
*Cnt3 = 0;
|
||
}
|
||
else
|
||
{
|
||
if(*Cnt3<60000)
|
||
{(*Cnt3)++;}
|
||
*Cnt1=0;
|
||
//Cnt2 = 0;
|
||
}
|
||
|
||
if ((*Cnt1) >= Cnt1Limt )
|
||
{
|
||
if (*Cnt2 >= Cnt2Limt)
|
||
{
|
||
*state = POSE_NO;
|
||
*Cnt1 = 0;
|
||
//*Cnt2 = 0;
|
||
*Cnt3 = 0;
|
||
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
if ((*Cnt1 >= (Cnt1Limt + Cnt4Limt) ) && (*Cnt1 < (Cnt1Limt + Cnt4Limt +1) ))
|
||
{
|
||
*Cnt1 = 0;
|
||
*Cnt3 = 0;
|
||
if(*Cnt2<60000)
|
||
{(*Cnt2) ++;}
|
||
|
||
if (*Cnt2 >= Cnt2Limt)
|
||
{
|
||
if ( POWER_OFF == ONOFF)
|
||
{
|
||
//留在关单机
|
||
OCOut(ChlOFF, Type, PulT);
|
||
}
|
||
else if (POWER_ON == ONOFF)
|
||
{
|
||
//开单机
|
||
OCOut(ChlON, Type, PulT);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//开单机
|
||
OCOut(ChlON, Type, PulT);
|
||
}
|
||
}
|
||
}
|
||
if ((*Cnt1 < (Cnt1Limt +1) )&&(*Cnt1 >= Cnt1Limt ))
|
||
{
|
||
//关单机
|
||
OCOut(ChlOFF, Type, PulT);
|
||
*state = POSE_IN;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (*Cnt3 >= Cnt3Limt)
|
||
{
|
||
*state = POSE_OK;
|
||
*Cnt2 = 0; //zmx20231011
|
||
}
|
||
else
|
||
{
|
||
*state = POSE_IN;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：陀螺异常诊断
|
||
|
||
***********************************************/
|
||
void GyroImuErrJudge(void)
|
||
{
|
||
UINT8 i =0;
|
||
UINT8 ONOFF = POWER_ON;
|
||
UINT8 ChlON[GYRO_NUM] = {OC66_GYROA_ON,OC69_GYROB_ON,OC71_GYROC_ON};
|
||
UINT8 ChlOFF[GYRO_NUM] = {OC67_GYROA_OFF,OC70_GYROB_OFF,OC72_GYROC_OFF};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
GyroPara_t *PGyro[GYRO_NUM];
|
||
AttIMPTPara_t *pIMU =NULL;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
|
||
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
|
||
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
if (pREcheck->Gyro_PowerOnCnt[i] >= tmpConst->Gyro_Delay_CntLimt)
|
||
{
|
||
if((POSE_NO == PGyro[i]->GyroIn_AttVld)||(CC_ERROR == PGyro[i]->GyroCrCheckFlg))
|
||
{
|
||
if(pREcheck->GyroErrCnt[i]<60000)
|
||
{pREcheck->GyroErrCnt[i]++;}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->GyroErrCnt[i]=0;
|
||
}
|
||
|
||
if(pREcheck->GyroErrCnt[i] >= tmpConst->GyroDiagThr*2)
|
||
{
|
||
pREcheck->Gyro_err[i] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
pREcheck->Gyro_err[i] = POSE_OK;
|
||
}
|
||
}
|
||
|
||
if (POSE_OK == tmpConst->AllowGyroONOFF[i])
|
||
{
|
||
//陀螺单机加断电
|
||
ImuPowerOnOff( ChlON[i], ChlOFF[i], ONOFF,
|
||
&pREcheck->Gyro_Err_Cnt1[i] , &pREcheck->Gyro_Err_Cnt2[i], &pREcheck->Gyro_Err_Cnt3[i],
|
||
tmpConst->Gyro_Err_CntLimt , tmpConst->Gyro_ReSet_CntLimt , tmpConst->Gyro_OK_CntLimt , tmpConst->Gyro_ReSetGAP_CntLimt ,
|
||
pREcheck->Gyro_err[i], &pREcheck->Gyro_Diag[i] );
|
||
|
||
if (POSE_NO == pREcheck->Gyro_Diag[i])
|
||
{
|
||
;//pIMU->GyroUseState[i] = POSE_NO;//使用状态置0
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->Gyro_Err_Cnt1[i] = 0;
|
||
pREcheck->Gyro_Err_Cnt2[i] = 0;
|
||
pREcheck->Gyro_Err_Cnt3[i] = 0;
|
||
pREcheck->Gyro_Diag[i] =POSE_OK;
|
||
pREcheck->Gyro_err[i] = POSE_OK;
|
||
pREcheck->GyroErrCnt[i]=0;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：星敏异常诊断
|
||
***********************************************/
|
||
void SsImuErrJudge(void)
|
||
{
|
||
UINT8 i =0;
|
||
UINT8 ONOFF = POWER_ON;
|
||
UINT8 ChlON = 0;
|
||
UINT8 ChlOFF = 0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
SsPara_t *Pss[STAR_NUM];
|
||
AttIMPTPara_t *pIMU;
|
||
sAttErrCtlPara_t *pREcheck;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
Pss[0] = &tmpAtt->sPerPara.SsPara[0];
|
||
Pss[1] = &tmpAtt->sPerPara.SsPara[1];
|
||
Pss[2] = &tmpAtt->sPerPara.SsPara[2];
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
|
||
for(i=0;i<STAR_NUM;i++)
|
||
{
|
||
|
||
if (pREcheck->ss_PowerOnCnt[i] >= tmpConst->Ss_Delay_CntLimt)
|
||
{
|
||
if((POSE_NO == Pss[i]->ssIn_AttVld)||(CC_ERROR == Pss[i]->ssCrCheckFlg))
|
||
{
|
||
if(pREcheck->ssErrCnt[i]<60000)
|
||
{pREcheck->ssErrCnt[i]++;}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->ssErrCnt[i]=0;
|
||
}
|
||
|
||
if(pREcheck->ssErrCnt[i] >= tmpConst->ssDiagThr*2)
|
||
{
|
||
pREcheck->SS_err[i] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
pREcheck->SS_err[i] = POSE_OK;
|
||
}
|
||
}
|
||
|
||
if (POSE_OK == tmpConst->AllowssONOFF[i])
|
||
{
|
||
ChlON = OC74_STA_ON + i*2 ;
|
||
ChlOFF = OC74_STA_ON + i*2 + 1;
|
||
if (i == 2)
|
||
{
|
||
ChlOFF = OC83_STC_OFF;
|
||
}
|
||
|
||
//星敏单机加断电
|
||
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
|
||
&pREcheck->ss_Err_Cnt1[i] , &pREcheck->ss_Err_Cnt2[i], &pREcheck->ss_Err_Cnt3[i],
|
||
tmpConst->Ss_Err_CntLimt , tmpConst->Ss_ReSet_CntLimt , tmpConst->Ss_OK_CntLimt , tmpConst->Ss_ReSetGAP_CntLimt ,
|
||
pREcheck->SS_err[i], &pREcheck->ss_Diag[i] );
|
||
|
||
if ((POSE_NO == pREcheck->ss_Diag[i]) && (POSE_OK == tmpConst->AllowssONOFF[i]))
|
||
{
|
||
;//pIMU->SSUseState[i] = POSE_NO;//使用状态置0
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->ss_Err_Cnt1[i] = 0;
|
||
pREcheck->ss_Err_Cnt2[i] = 0;
|
||
pREcheck->ss_Err_Cnt3[i] = 0;
|
||
pREcheck->ss_Diag[i] =POSE_OK;
|
||
pREcheck->ssErrCnt[i]=0;
|
||
pREcheck->SS_err[i] = POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：磁强计异常诊断
|
||
***********************************************/
|
||
void MagImuErrJudge(void)
|
||
{
|
||
UINT8 i =0;
|
||
UINT8 ONOFF = POWER_ON;
|
||
UINT8 ChlON = 0;
|
||
UINT8 ChlOFF = 0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
MagPara_t *PMag =NULL;
|
||
AttIMPTPara_t *pIMU =NULL;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
PMag = &tmpAtt->sPerPara.MagPara;
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
|
||
for(i=0;i<MAG_NUM;i++)
|
||
{
|
||
if (pREcheck->Mag_PowerOnCnt[i] >= tmpConst->Mag_Delay_CntLimt)
|
||
{
|
||
if((POSE_NO == PMag->MagIn_Valid[i])||(CC_ERROR == PMag->MagCrCheckFlg[i]))
|
||
{
|
||
if(pREcheck->MagErrCnt[i]<60000)
|
||
{pREcheck->MagErrCnt[i]++;}
|
||
pREcheck->MagNoErrCnt[i]=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->MagNoErrCnt[i]<60000)
|
||
{pREcheck->MagNoErrCnt[i]++;}
|
||
pREcheck->MagErrCnt[i]=0;
|
||
}
|
||
|
||
if(pREcheck->MagErrCnt[i] >= tmpConst->MagDiagThr*2)
|
||
{
|
||
pREcheck->Mag_err[i] = POSE_NO;
|
||
}
|
||
else if (pREcheck->MagNoErrCnt[i] > 2)
|
||
{
|
||
pREcheck->Mag_err[i] = POSE_OK;
|
||
}
|
||
}
|
||
|
||
if (POSE_OK == tmpConst->AllowMagONOFF[i])
|
||
{
|
||
ChlON = OC94_CQJA_ON + i*2 ;
|
||
ChlOFF = OC94_CQJA_ON + i*2 + 1;
|
||
|
||
//磁强计单机加断电
|
||
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
|
||
&pREcheck->Mag_Err_Cnt1[i] , &pREcheck->Mag_Err_Cnt2[i], &pREcheck->Mag_Err_Cnt3[i],
|
||
tmpConst->Mag_Err_CntLimt , tmpConst->Mag_ReSet_CntLimt , tmpConst->Mag_OK_CntLimt , tmpConst->Mag_ReSetGAP_CntLimt ,
|
||
pREcheck->Mag_err[i], &pREcheck->Mag_Diag[i] );
|
||
|
||
if ((POSE_NO == pREcheck->Mag_Diag[i]) && (POSE_OK == tmpConst->AllowMagONOFF[i]))
|
||
{
|
||
;//pIMU->MagUseState[i] = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->Mag_Err_Cnt2[i] = 0;
|
||
pREcheck->Mag_Diag[i] =POSE_OK;
|
||
pREcheck->Mag_Err_Cnt1[i]= 0;
|
||
pREcheck->Mag_Err_Cnt2[i]= 0;
|
||
pREcheck->Mag_Err_Cnt3[i]= 0;
|
||
pREcheck->MagErrCnt[i]=0;
|
||
pREcheck->Mag_err[i] = POSE_OK;
|
||
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：飞轮异常诊断
|
||
|
||
***********************************************/
|
||
void WhlImuErrJudge(void)
|
||
{
|
||
UINT8 i = 0;
|
||
UINT8 ChlON = 0;
|
||
UINT8 ChlOFF = 0;
|
||
UINT8 ONOFF = POWER_OFF;
|
||
UINT32 tmp_cnt = 0;
|
||
UINT8 cmdDataBuff[50] = {0};
|
||
sTaskInfo* tmpTaskInfo = NULL;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
WhlPara_t *PWhl =NULL;
|
||
AttIMPTPara_t *pImpt =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
pWMod =&tmpAtt->sModePara;
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if (POSE_OK != tmpConst->AllowWhlONOFF[i])
|
||
{
|
||
pREcheck->Whl_Err_Cnt1[i] = 0;
|
||
pREcheck->Whl_Err_Cnt2[i] = 0;
|
||
pREcheck->Whl_Err_Cnt3[i] = 0;
|
||
pREcheck->WhlOutErrCnt[i]=0;
|
||
pREcheck->WhlTemErrCnt[i] = 0;
|
||
pREcheck->Whl_Diag[i] =POSE_OK;
|
||
pREcheck->Whl_Err[i] = POSE_OK;
|
||
}
|
||
}
|
||
switch (pWMod->WorkMode)
|
||
{
|
||
case ATTMOD_UNCTL:
|
||
case ATTMOD_RATEDMP:
|
||
case ATTMOD_MagTOSUN: /*不进行飞轮检测*/
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if (pImpt->Whl_Use[i] == POSE_OK)
|
||
{
|
||
tmp_cnt++;
|
||
}
|
||
}
|
||
PWhl->Whl_UseCnt = tmp_cnt;
|
||
break;
|
||
case ATTMOD_WhlTOSUN:
|
||
case ATTMOD_ONLTOEARTH:
|
||
case ATTMOD_WHEELTOEARTH:
|
||
case ATTMOD_ATTAJUSTPRE:
|
||
case ATTMOD_ORBITCTL:
|
||
case ATTMOD_ATTAJUST:
|
||
case ATTMOD_ONLTOTAR:
|
||
//case ATTMOD_NOCOLL:
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if (pREcheck->Whl_PowerOnCnt[i] >= tmpConst->Whl_Delay_CntLimt)
|
||
{
|
||
//飞轮输出异常计数
|
||
if(PWhl->WhlIn_Valid[i]==0x55)
|
||
{pREcheck->WhlOutErrCnt[i]=0;}
|
||
else
|
||
{
|
||
if(pREcheck->WhlOutErrCnt[i]<60000)
|
||
{pREcheck->WhlOutErrCnt[i]++;}
|
||
}
|
||
//轴温异常计数
|
||
if(PWhl->Whl_Tmper[i]<(TYPE_CAL)tmpConst->WhlTemperDiagThr)
|
||
{pREcheck->WhlTemErrCnt[i]=0;}
|
||
else
|
||
{
|
||
if(pREcheck->WhlTemErrCnt[i]<60000)
|
||
{pREcheck->WhlTemErrCnt[i]++;}
|
||
}
|
||
//飞轮异常
|
||
if((pREcheck->WhlOutErrCnt[i]>=tmpConst->WhlDiag_ErrThr*2)||(pREcheck->WhlTemErrCnt[i]>=tmpConst->WhlDiag_TimeThr*2))
|
||
{
|
||
pREcheck->Whl_Err[i] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
pREcheck->Whl_Err[i] = POSE_OK;
|
||
}
|
||
}
|
||
|
||
if (POSE_OK == tmpConst->AllowWhlONOFF[i])
|
||
{
|
||
ChlON = OC58_WHLX_ON + i*2;
|
||
ChlOFF = OC58_WHLX_ON + i*2 + 1;
|
||
|
||
//飞轮单机加断电
|
||
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
|
||
&pREcheck->Whl_Err_Cnt1[i] , &pREcheck->Whl_Err_Cnt2[i], &pREcheck->Whl_Err_Cnt3[i],
|
||
tmpConst->Whl_Err_CntLimt , tmpConst->Whl_ReSet_CntLimt , tmpConst->Whl_OK_CntLimt , tmpConst->Whl_ReSetGAP_CntLimt ,
|
||
pREcheck->Whl_Err[i], &pREcheck->Whl_Diag[i] );
|
||
|
||
if (POSE_NO == pREcheck->Whl_Diag[i])
|
||
{
|
||
pImpt->Whl_Use[i] = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->Whl_Diag[i] =POSE_OK;
|
||
}
|
||
|
||
if (pImpt->Whl_Use[i] == POSE_OK)
|
||
{
|
||
tmp_cnt++;
|
||
}
|
||
}
|
||
|
||
PWhl->Whl_UseCnt = tmp_cnt;
|
||
|
||
if ((POSE_OK == tmpConst->AllowWhlONOFF[0])&&(POSE_OK == tmpConst->AllowWhlONOFF[1])&&(POSE_OK == tmpConst->AllowWhlONOFF[2])&&(POSE_OK == tmpConst->AllowWhlONOFF[3]))
|
||
{
|
||
if (PWhl->Whl_UseCnt >= 3)
|
||
{
|
||
//PWhl->Whl_OK = POSE_OK;
|
||
;
|
||
}
|
||
else
|
||
{
|
||
//PWhl->Whl_OK = POSE_NO;
|
||
|
||
//切换到新模式后，将相关计数器清零
|
||
memset(cmdDataBuff, 0x00, 50);
|
||
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
|
||
DoSetInt32To8(ATT_CMDMODE_MODSUNCAP, cmdDataBuff);
|
||
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
|
||
{
|
||
/* 在姿控的指令处理中 进行 ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
default:
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
pREcheck->WhlOutErrCnt[i] =0;
|
||
pREcheck->WhlTemErrCnt[i] =0;
|
||
}
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：PPU异常诊断
|
||
|
||
***********************************************/
|
||
void PPUImuErrJudge(void)
|
||
{
|
||
// UINT8 PPUOn[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
//UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
INT16 TemH[4] ={0};
|
||
TYPE_CAL TemR[4] ={0.0},Temper[4] ={0.0},TempBAR[4] ={0.0};
|
||
TYPE_CAL TemA =-6.01188f;
|
||
TYPE_CAL TemB =4622.53337f;
|
||
TYPE_CAL TemC =-86421.72414f;
|
||
TYPE_CAL TemD =273.15f;
|
||
TYPE_CAL TemEXP =2.718281828f;
|
||
INT16 *pTemper =NULL;
|
||
UINT8 ChlON = 0;
|
||
UINT8 ChlOFF = 0;
|
||
UINT8 TmpPPU_Err = POSE_OK;
|
||
UINT8 i = 0;
|
||
UINT8 ONOFF = POWER_ON;
|
||
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttErrCtlPara_t *pREcheck;
|
||
AttIMPTPara_t *pImpt;
|
||
PPUPara_t *PPPU;
|
||
sAttModPara_t *pWMod;
|
||
sDevInfo * tmpDevInfo;
|
||
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return;}
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
pWMod =&tmpAtt->sModePara;
|
||
|
||
//获取AD采集数据
|
||
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
|
||
pTemper = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
|
||
////读取PPU温度
|
||
TemH[0] = *(pTemper + SW22_BOTTLE1-1); //气瓶测温1
|
||
TemH[1] = *(pTemper + SW23_BOTTLE2-1); //气瓶测温1
|
||
TemH[2]= *(pTemper + SW24_PIPE1-1); //管路测温1
|
||
TemH[3]= *(pTemper + SW50_PIPE2-1); //管路测温2
|
||
|
||
for(i=0;i<4;i++)
|
||
{
|
||
if (((3849.3-TemH[i])<1801.8)&&((3849.3-TemH[i]) > 0))
|
||
{
|
||
//求电阻值
|
||
if(POSE_ABSF(3849.3-TemH[i])<POSE_ZERO)
|
||
{
|
||
continue;
|
||
}
|
||
TemR[i] = 10000*(1801.8/(3849.3-TemH[i]) - 1);
|
||
//求温度中间变量
|
||
TempBAR[i] =TemB*TemB - 4*TemC*(TemA -log(TemR[i])/log(TemEXP));
|
||
if (TempBAR[i] > POSE_ZERO)
|
||
{
|
||
if (POSE_ABSF(POSE_SQRTF(TempBAR[i]) - TemB)>POSE_ZERO)
|
||
{
|
||
Temper[i] =2*(TemC)/(POSE_SQRTF(TempBAR[i]) - TemB) - TemD;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
PPPU->PPUT_BOTTLE_Tmper1 = Temper[0];
|
||
PPPU->PPUT_BOTTLE_Tmper2 = Temper[1];
|
||
PPPU->PPUT_PIPE_Tmper1 = Temper[2];
|
||
PPPU->PPUT_PIPE_Tmper2 = Temper[3];
|
||
|
||
if (pREcheck->PPU_PowerOnCnt >= tmpConst->PPU_Delay_CntLimt)
|
||
{
|
||
//ppu输出异常计数
|
||
if(PPPU->PPUIn_Valid!=POSE_OK)/*PPU数据无效*/
|
||
{
|
||
if(pREcheck->PPUOutErrCnt<60000)
|
||
{pREcheck->PPUOutErrCnt++;}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPUOutErrCnt=0;
|
||
}
|
||
|
||
|
||
if(0x01 == PPPU->PPUIn_FIREOK)/*点火成功标志为1*/
|
||
{
|
||
if(PPPU->PPUIn_ACurrent < (((TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTALimit)/10.0)) //20231228
|
||
{pREcheck->PPUIn_ACurCnt=0;}
|
||
else
|
||
{
|
||
if(pREcheck->PPUIn_ACurCnt<60000)
|
||
{pREcheck->PPUIn_ACurCnt++;}
|
||
}
|
||
|
||
if(PPPU->PPUIn_AVoltage < (TYPE_CAL)tmpConst->AttCmdFlashPara.PPUTVLimit)
|
||
{pREcheck->PPUIn_VCurCnt=0;}
|
||
else
|
||
{
|
||
if(pREcheck->PPUIn_VCurCnt<60000)
|
||
{pREcheck->PPUIn_VCurCnt++;}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPUIn_ACurCnt=0;
|
||
pREcheck->PPUIn_VCurCnt=0;
|
||
}
|
||
|
||
if ((POSE_NO == PPPU->PPUOUT_Sta )||(POSE_IN == PPPU->PPUOUT_Sta))
|
||
{pREcheck->PPUOUT_FIRECnt=0;}
|
||
else
|
||
{
|
||
if(pREcheck->PPUOUT_FIRECnt<60000)
|
||
{pREcheck->PPUOUT_FIRECnt++;}
|
||
}
|
||
|
||
if ((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode))
|
||
{
|
||
if (((Temper[0]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[0]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[1]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[1]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))
|
||
&&((Temper[2]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[2]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit)))&&((Temper[3]>(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPUHTemperLimit)) || (Temper[3]<(TYPE_CAL)(tmpConst->AttCmdFlashPara.PPULTemperLimit))))
|
||
{
|
||
if(pREcheck->PPUOUT_TemperECnt<60000)
|
||
{pREcheck->PPUOUT_TemperECnt++; }
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPUOUT_TemperECnt=0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPUOUT_TemperECnt=0;
|
||
}
|
||
|
||
|
||
//if(((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&((ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL == pWMod->WorkMode)))&&(POSE_OK == tmpConst->AllowPPUONOFF))
|
||
if((pREcheck->PPUOutErrCnt>=tmpConst->PPUTXTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x03 ;
|
||
//pImpt->PPUUseStatus = POSE_NO;
|
||
//if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
|
||
if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
|
||
{
|
||
pImpt->PPUUseStatus = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xFC ;
|
||
|
||
}
|
||
|
||
|
||
if(((0x01 == PPPU->PPUIn_FIREOK)&&((pREcheck->PPUIn_ACurCnt>= (tmpConst->PPUTATimeLimit*2))||(pREcheck->PPUIn_VCurCnt>=(tmpConst->PPUTVTimeLimit*2))))&&(POSE_OK == tmpConst->AllowPPUONOFF))
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x0C ;
|
||
//pImpt->PPUUseStatus = POSE_NO;
|
||
if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
|
||
{
|
||
pImpt->PPUUseStatus = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xF3 ;
|
||
|
||
}
|
||
|
||
|
||
if(((pREcheck->PPUOUT_FIRECnt>=tmpConst->AttCmdFlashPara.PPUTFireLimit*2)&&(PPPU->PPUIn_FIREOK!=0x01))&&(POSE_OK == tmpConst->AllowPPUONOFF))
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err | 0x30 ;
|
||
if ((POSE_NO==tmpConst->AttCmdFlashPara.WorkModeChangeAuto) ||(POSE_OK == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
|
||
{
|
||
//pImpt->PPUUseStatus = POSE_NO;
|
||
if((ATTMOD_ORBITCTL == pWMod->WorkMode)||(ATTMOD_ATTAJUSTPRE == pWMod->WorkMode))/*轨控期间*/
|
||
{
|
||
pImpt->PPUUseStatus = POSE_NO;
|
||
}
|
||
}
|
||
else if ((POSE_OK==tmpConst->AttCmdFlashPara.WorkModeChangeAuto)&&(POSE_NO == tmpAtt->sCtlPara.ZK_Ctrl_Reserver111))
|
||
{
|
||
/*轨控异常，当前拍的关PPU动作*/
|
||
dev_can_write(DEV_NO_CAN0, PPUOff, 8);
|
||
PPPU->PPUOUT_Sta = POSE_NO;/*电推指令开关标志为关*/
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_IN;
|
||
/*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;
|
||
}
|
||
else
|
||
{
|
||
;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
|
||
|
||
}
|
||
|
||
if((pREcheck->PPUOUT_TemperECnt>=tmpConst->PPUTemperTimeLimit*2)&&(POSE_OK == tmpConst->AllowPPUONOFF))
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err | 0xC0;
|
||
//pImpt->PPUUseStatus = POSE_NO;
|
||
if(ATTMOD_ORBITCTL == pWMod->WorkMode)/*轨控期间*/
|
||
{
|
||
pImpt->PPUUseStatus = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err & 0x3F;
|
||
}
|
||
|
||
//if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
|
||
//{
|
||
//pREcheck->PPU_Err_Cnt1 = 0;
|
||
//pREcheck->PPU_Err_Cnt2 = 0;
|
||
//pREcheck->PPU_Err_Cnt3 = 0;
|
||
//return;
|
||
//}
|
||
|
||
//if (ATTMOD_ORBITCTL ==pWMod->WorkMode)
|
||
if (( ATTMOD_ATTAJUSTPRE == pWMod->WorkMode)||(ATTMOD_ORBITCTL ==pWMod->WorkMode))
|
||
{
|
||
pREcheck->PPU_Err_Cnt1 = 0;
|
||
pREcheck->PPU_Err_Cnt2 = 0;
|
||
pREcheck->PPU_Err_Cnt3 = 0;
|
||
if (POSE_OK == tmpConst->AllowPPUONOFF)
|
||
{
|
||
;
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Diag =POSE_OK;
|
||
pREcheck->PPU_Err = 0;
|
||
pREcheck->PPUOutErrCnt=0;
|
||
pREcheck->PPUIn_ACurCnt=0;
|
||
pREcheck->PPUIn_VCurCnt=0;
|
||
pREcheck->PPUOUT_FIRECnt=0;
|
||
pREcheck->PPUOUT_TemperECnt=0;
|
||
}
|
||
return;
|
||
}
|
||
}
|
||
|
||
if (0x03 == (pREcheck->PPU_Err & 0x03))
|
||
{
|
||
TmpPPU_Err = POSE_NO;
|
||
}
|
||
|
||
if (POSE_OK == tmpConst->AllowPPUONOFF)
|
||
{
|
||
ChlON = OC68_PPU_ON ;
|
||
ChlOFF = OC73_PPU_OFF;
|
||
|
||
//PPU单机加断电
|
||
ImuPowerOnOff( ChlON, ChlOFF, ONOFF,
|
||
&pREcheck->PPU_Err_Cnt1, &pREcheck->PPU_Err_Cnt2, &pREcheck->PPU_Err_Cnt3,
|
||
tmpConst->PPU_Err_CntLimt , tmpConst->PPU_ReSet_CntLimt , tmpConst->PPU_OK_CntLimt , tmpConst->PPU_ReSetGAP_CntLimt ,
|
||
TmpPPU_Err, &pREcheck->PPU_Diag );
|
||
|
||
if (POSE_NO == pREcheck->PPU_Diag)
|
||
{
|
||
pImpt->PPUUseStatus = POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pREcheck->PPU_Err_Cnt1 = 0;
|
||
pREcheck->PPU_Err_Cnt2 = 0;
|
||
pREcheck->PPU_Err_Cnt3 = 0;
|
||
pREcheck->PPU_Diag =POSE_OK;
|
||
pREcheck->PPU_Err = 0;
|
||
pREcheck->PPUOutErrCnt=0;
|
||
pREcheck->PPUIn_ACurCnt=0;
|
||
pREcheck->PPUIn_VCurCnt=0;
|
||
pREcheck->PPUOUT_FIRECnt=0;
|
||
pREcheck->PPUOUT_TemperECnt=0;
|
||
}
|
||
}
|
||
|
||
|
||
/*******************************************************************************************
|
||
说明：闭环姿态输出长时间无效判断
|
||
输出：0x55表示长时间姿态丢失，0表示姿态未丢失
|
||
*******************************************************************************************/
|
||
UINT8 AttLose(UINT8 i)
|
||
{
|
||
UINT8 result=0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{return 0;}
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
|
||
if(POSE_OK == tmpAtt->sDeterPara.CLPAttValid)
|
||
{
|
||
pREcheck->AttLoseCnt=0;
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttLoseCnt<60000)
|
||
{pREcheck->AttLoseCnt++;}
|
||
}
|
||
|
||
if(pREcheck->AttLoseCnt>=tmpConst->TimeThr_AttLoseDiag[i]*2)
|
||
{result=0x55;}
|
||
|
||
return result;
|
||
}
|
||
|
||
/***********************************************
|
||
说明：姿态丢失判断
|
||
***********************************************/
|
||
void AttLoseJudge(void)
|
||
{
|
||
UINT8 cmdDataBuff[50] = {0};
|
||
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
sTaskInfo* tmpTaskInfo = NULL;
|
||
UINT8 tmpNewMod = 0x00;
|
||
UINT16 tmpNewModID = 0x00;
|
||
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
|
||
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
|
||
sAttModPara_t *pWMod;
|
||
sAttOrbitCtlInfo_t *pOrb;
|
||
sAttTARCCtlInfo_t *pTARC;
|
||
PPUPara_t *PPPU = NULL;
|
||
|
||
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
|
||
{return;}
|
||
pWMod=&tmpAtt->sModePara;
|
||
pOrb=&tmpAtt->sOrbitInp;
|
||
pTARC=&tmpAtt->sTARCInp;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
tmpNewMod = pWMod->WorkMode;
|
||
|
||
|
||
switch(pWMod->WorkMode)
|
||
{
|
||
case ATTMOD_RATEDMP:
|
||
if(POSE_OK == AttLose(0))
|
||
{
|
||
tmpNewMod = ATTMOD_WhlTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_WhlTOSUN:
|
||
if(POSE_OK == AttLose(1))
|
||
{
|
||
tmpNewMod = ATTMOD_MagTOSUN; //gg20240522js
|
||
tmpNewModID = ATT_CMDMODE_MagTOSUN;
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_MagTOSUN:
|
||
if(POSE_OK == AttLose(2))
|
||
{
|
||
tmpNewMod = ATTMOD_UNCTL;
|
||
tmpNewModID = ATT_CMDMODE_UNCTL;
|
||
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ONLTOEARTH:
|
||
if(POSE_OK == AttLose(3))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_WHEELTOEARTH:
|
||
if(POSE_OK == AttLose(4))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ATTAJUSTPRE:
|
||
if(POSE_OK == AttLose(5))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
pOrb->CurOrbCtrlPcak_ID = 0;
|
||
pOrb->CurOrbCtrlPcak_PreID =0xFF;
|
||
PPPU->PPUT_FIR_Cnt = 0;
|
||
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
|
||
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
|
||
pOrb->OrbDataIn_OK = POSE_NO;
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ORBITCTL:
|
||
if(POSE_OK == AttLose(6))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
pOrb->CurOrbCtrlPcak_ID = 0;
|
||
pOrb->CurOrbCtrlPcak_PreID =0xFF;
|
||
PPPU->PPUT_FIR_Cnt = 0;
|
||
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
|
||
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
|
||
pOrb->OrbDataIn_OK = POSE_NO;
|
||
|
||
/*轨控时故障诊断异常，当前拍的关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;
|
||
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ATTAJUST:
|
||
if(POSE_OK == AttLose(7))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
pTARC->CurTARCtrlPcak_ID=0;
|
||
pTARC->CurTARCtrlPcak_PreID =0xFF;
|
||
pTARC->TARDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
|
||
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ONLTOTAR:
|
||
if(POSE_OK == AttLose(8))
|
||
{
|
||
//发速率阻尼指令
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
pTARC->CurTARCtrlPcak_ID=0;
|
||
pTARC->CurTARCtrlPcak_PreID =0xFF;
|
||
pTARC->TARDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
|
||
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
if(tmpNewMod != pWMod->WorkMode)
|
||
{
|
||
memset(cmdDataBuff, 0x00, 50);
|
||
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
|
||
DoSetInt32To8(tmpNewModID, cmdDataBuff);
|
||
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
|
||
{
|
||
/* 在姿控的指令处理中 进行 ZKModDoSet(ATTMOD_ATTAJUSTPRE);*/
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：姿态超差判断
|
||
***********************************************/
|
||
void AttExJudge(void)
|
||
{
|
||
UINT8 cmdDataBuff[50] = {0};
|
||
UINT8 PPUOff[8] = {0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
sTaskInfo* tmpTaskInfo = NULL;
|
||
UINT8 tmpNewMod = 0x00;
|
||
UINT16 tmpNewModID = 0x00;
|
||
UINT8 i =0;
|
||
UINT8 tmpAttNOK[3] = {POSE_OK,POSE_OK,POSE_OK};
|
||
UINT8 tmpWhlOK = POSE_OK;
|
||
TYPE_CAL tmpMod =0.0;
|
||
TYPE_CAL tmpAngE[3] ={0.0};//姿态角偏差
|
||
TYPE_CAL tmpAngRateE[3] ={0.0};//姿态角速度偏差
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
WhlPara_t *PWhl =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
WhlPara_t *pWhl =NULL;
|
||
AttIMPTPara_t *pIMU =NULL;
|
||
PPUPara_t *PPPU = NULL;
|
||
|
||
sAttOrbitCtlInfo_t *pOrb =NULL;
|
||
sAttTARCCtlInfo_t *pTARC =NULL;
|
||
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
|
||
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
|
||
|
||
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
|
||
return;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
pWhl=&tmpAtt->sPerPara.WhlPara;
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
|
||
pOrb=&tmpAtt->sOrbitInp;
|
||
pTARC=&tmpAtt->sTARCInp;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
tmpNewMod = pWMod->WorkMode;
|
||
|
||
switch(pWMod->WorkMode)
|
||
{
|
||
case ATTMOD_RATEDMP:
|
||
{
|
||
CalNormal(tmpAtt->sDeterPara.CLPAttAngRat, 3, &tmpMod);
|
||
//每30s记录一次闭环姿态角速度矢量模值
|
||
for(i=0;i<60;i++)
|
||
{pREcheck->AngRateMod_RATEDMP[i]=pREcheck->AngRateMod_RATEDMP[i+1];}
|
||
pREcheck->AngRateMod_RATEDMP[60]=tmpMod;
|
||
|
||
//角速度矢量模值与30s前差值判断
|
||
if ((pREcheck->AngRateMod_RATEDMP[60]-pREcheck->AngRateMod_RATEDMP[0])<(tmpConst->RateThr_AttEx[0]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
memset(pREcheck->AttExFlg,0,3*sizeof(UINT8));
|
||
}
|
||
else
|
||
{
|
||
memset(pREcheck->AttExFlg,0x55,3*sizeof(UINT8));
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[0]*2))
|
||
{
|
||
//进对日定向状态
|
||
if (PWhl->Whl_UseCnt >= 3)
|
||
{
|
||
tmpNewMod = ATTMOD_WhlTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
|
||
}
|
||
else
|
||
{
|
||
tmpNewMod = ATTMOD_MagTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_MagTOSUN;
|
||
}
|
||
////置陀螺不可用（考虑冷备份，只给加电陀螺置不可用）
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
if(POSE_OK == pREcheck->GyroIn_PowerOn[i])
|
||
{
|
||
pIMU->GyroUseState[i] = POSE_NO;//使用状态置0
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_WhlTOSUN:
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[1])
|
||
{
|
||
tmpAttNOK[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
tmpAttNOK[i] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pREcheck->AttExFlg[i]=tmpAttNOK[i];
|
||
}
|
||
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[1]*2))
|
||
{
|
||
//切磁控对日
|
||
tmpNewMod = ATTMOD_MagTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_MagTOSUN;
|
||
pREcheck->AttExCnt = 0;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_MagTOSUN:
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[2])
|
||
{
|
||
tmpAttNOK[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
tmpAttNOK[i] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pREcheck->AttExFlg[i]=tmpAttNOK[i];
|
||
}
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[2]*2))
|
||
{
|
||
//出口待确认
|
||
tmpNewMod = ATTMOD_UNCTL;
|
||
tmpNewModID = ATT_CMDMODE_UNCTL;
|
||
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ONLTOEARTH :
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[3])
|
||
{
|
||
tmpAttNOK[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
tmpAttNOK[i] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pREcheck->AttExFlg[i]=tmpAttNOK[i];
|
||
}
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[3]*2))
|
||
{
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_WHEELTOEARTH:
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if((POSE_ABSF(tmpAtt->sDeterPara.CLPAttAng[i])>tmpConst->AngThr_AttEx[4])||(POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i])>tmpConst->RateThr_AttEx[4]))
|
||
{pREcheck->AttExFlg[i]=0x55;}
|
||
else
|
||
{pREcheck->AttExFlg[i]=0;}
|
||
}
|
||
//XY超差计数
|
||
if((POSE_OK == pREcheck->AttExFlg[0])||(POSE_OK == pREcheck->AttExFlg[1]))
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttExCnt=0;}
|
||
//Z超差计数
|
||
if(POSE_OK == pREcheck->AttExFlg[2])
|
||
{
|
||
if(pREcheck->YawCnt<60000)
|
||
{pREcheck->YawCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->YawCnt=0;}
|
||
//连续超差
|
||
if((pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[4]*2))||
|
||
(pREcheck->YawCnt>=(tmpConst->YawCntLimit*2)))
|
||
{
|
||
pREcheck->YawCnt=0;
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ATTAJUSTPRE:
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[5])
|
||
{
|
||
tmpAttNOK[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
tmpAttNOK[i] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
if((POSE_NO == tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pREcheck->AttExFlg[i]=tmpAttNOK[i];
|
||
}
|
||
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[5]*2))
|
||
{
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
pOrb->CurOrbCtrlPcak_ID = 0;
|
||
pOrb->CurOrbCtrlPcak_PreID =0xFF;
|
||
PPPU->PPUT_FIR_Cnt = 0;
|
||
pOrb->OrbDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
|
||
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
}
|
||
break;
|
||
}
|
||
case ATTMOD_ORBITCTL:
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
|
||
if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[6])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[6]))
|
||
{pREcheck->AttExFlg[i]=0x55;}
|
||
else
|
||
{pREcheck->AttExFlg[i]=0;}
|
||
}
|
||
//任意飞轮转速大于6000rpm
|
||
for(i=0;i<4;i++)
|
||
{
|
||
//if (pWhl->Whl_Rate[i] > 6000*RPM2RADS)
|
||
if (POSE_ABSF(pWhl->Whl_Rate[i]) > (628.32 * RADS2RPM))
|
||
{
|
||
tmpWhlOK = POSE_NO;
|
||
break;
|
||
}
|
||
}
|
||
//超差计数
|
||
if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 == pREcheck->AttExFlg[2]) || (POSE_NO == tmpWhlOK))
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttExCnt=0;}
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[6]*2))
|
||
{
|
||
//发送进入对地定向指令
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
pOrb->CurOrbCtrlPcak_ID = 0;
|
||
pOrb->CurOrbCtrlPcak_PreID =0xFF;
|
||
PPPU->PPUT_FIR_Cnt = 0;
|
||
pOrb->OrbDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
|
||
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
|
||
|
||
/*轨控时故障诊断异常超差，当前拍的关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;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ATTAJUST:
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if (POSE_ABSF(tmpAtt->sDeterPara.CLPAttAngRat[i]) > tmpConst->RateThr_AttEx[7])
|
||
{
|
||
tmpAttNOK[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
tmpAttNOK[i] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
if((POSE_NO ==tmpAttNOK[0])&&(POSE_NO == tmpAttNOK[1])&&(POSE_NO == tmpAttNOK[2]))
|
||
{
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
else
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
|
||
for(i=0;i<3;i++)
|
||
{
|
||
pREcheck->AttExFlg[i]=tmpAttNOK[i];
|
||
}
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[7]*2))
|
||
{
|
||
//发送进入对地定向指令
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
pTARC->CurTARCtrlPcak_ID=0;
|
||
pTARC->CurTARCtrlPcak_PreID =0xFF;
|
||
pTARC->TARDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
|
||
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ONLTOTAR:
|
||
{
|
||
//超差判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
|
||
if((POSE_ABSF(tmpAngE[i])>tmpConst->AngThr_AttEx[8])||(POSE_ABSF(tmpAngRateE[i])>tmpConst->RateThr_AttEx[8]))
|
||
{pREcheck->AttExFlg[i]=0x55;}
|
||
else
|
||
{pREcheck->AttExFlg[i]=0;}
|
||
}
|
||
|
||
//超差计数
|
||
if((0x55 == pREcheck->AttExFlg[0])||(0x55 == pREcheck->AttExFlg[1])||(0x55 ==pREcheck->AttExFlg[2]) )
|
||
{
|
||
if(pREcheck->AttExCnt<60000)
|
||
{pREcheck->AttExCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttExCnt=0;}
|
||
//连续超差
|
||
if(pREcheck->AttExCnt>=(tmpConst->TimeThr_AttEx[8]*2))
|
||
{
|
||
//发送进入对地定向指令
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
pTARC->CurTARCtrlPcak_ID=0;
|
||
pTARC->CurTARCtrlPcak_PreID =0xFF;
|
||
pTARC->TARDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
|
||
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
|
||
}
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
|
||
if(tmpNewMod != pWMod->WorkMode)
|
||
{
|
||
memset(cmdDataBuff, 0x00, 50);
|
||
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
|
||
DoSetInt32To8(tmpNewModID, cmdDataBuff);
|
||
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
|
||
{
|
||
/* 在姿控的指令处理中 */
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：姿态不稳定判断
|
||
***********************************************/
|
||
void AttUnStaJudge(void)
|
||
{
|
||
UINT8 cmdDataBuff[50] = {0};
|
||
sTaskInfo* tmpTaskInfo = NULL;
|
||
UINT8 i =0;
|
||
UINT8 tmpNewMod = 0x00;
|
||
UINT16 tmpNewModID = 0x00;
|
||
TYPE_CAL tmpAngE[3]= {0.0f};//姿态角偏差
|
||
TYPE_CAL tmpAngRateE[3] ={0.0f};//姿态角速度偏差
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttModPara_t *pWMod =NULL;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
WhlPara_t *PWhl =NULL;
|
||
sAttOrbitCtlInfo_t *pOrb =NULL;
|
||
sAttTARCCtlInfo_t *pTARC =NULL;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
PPUPara_t *PPPU = NULL;
|
||
sAttOrbitCtlData_t *tmpOrbCmd=(sAttOrbitCtlData_t *)ATTCLT_DATA3_ADDR;
|
||
sAttTARCCtlData_t *tmpTARCCmd=(sAttTARCCtlData_t *)ATTCLT_DATA6_ADDR;
|
||
|
||
if((NULL==tmpAtt) || (NULL==tmpConst) || (NULL==tmpOrbCmd)|| (NULL==tmpTARCCmd))
|
||
{return;}
|
||
pWMod=&tmpAtt->sModePara;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
//pCtrl=&tmpAtt->sCtlPara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
tmpNewMod = pWMod->WorkMode;
|
||
pOrb=&tmpAtt->sOrbitInp;
|
||
pTARC=&tmpAtt->sTARCInp;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
|
||
switch(pWMod->WorkMode)
|
||
{
|
||
//阻尼
|
||
case ATTMOD_RATEDMP:
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{pREcheck->AttUnStaCnt++;}
|
||
if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[0]*2))
|
||
{
|
||
//发送进入对日定向指令
|
||
if (PWhl->Whl_UseCnt >= 3)
|
||
{
|
||
tmpNewMod = ATTMOD_WhlTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_WhlTOSUN;
|
||
}
|
||
else
|
||
{
|
||
tmpNewMod = ATTMOD_MagTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_MagTOSUN;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_WhlTOSUN:
|
||
{
|
||
//姿态不稳定判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
|
||
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[1])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[1]))
|
||
{pREcheck->AttUnSta[i]=0;}
|
||
else
|
||
{pREcheck->AttUnSta[i]=0x55;}
|
||
}
|
||
if((POSE_OK == pREcheck->AttUnSta[0])||(POSE_OK == pREcheck->AttUnSta[1])||(POSE_OK == pREcheck->AttUnSta[2]))
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{pREcheck->AttUnStaCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttUnStaCnt=0;}
|
||
//连续不稳定判断
|
||
if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[1]*2))
|
||
{
|
||
//切磁控对日
|
||
tmpNewMod = ATTMOD_MagTOSUN;
|
||
tmpNewModID = ATT_CMDMODE_MagTOSUN;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_MagTOSUN:
|
||
{
|
||
//姿态不稳定判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
//tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
tmpAngE[i]=pDeter->AngToSun[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[2])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[2]))
|
||
{pREcheck->AttUnSta[i]=0;}
|
||
else
|
||
{pREcheck->AttUnSta[i]=0x55;}
|
||
}
|
||
if((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{pREcheck->AttUnStaCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttUnStaCnt=0;}
|
||
|
||
//连续不稳定判断
|
||
if(pREcheck->AttUnStaCnt>=(tmpConst->TimeThr_AttUnSta[2]*2))
|
||
{
|
||
//发送无控指令
|
||
tmpNewMod = ATTMOD_UNCTL;
|
||
tmpNewModID = ATT_CMDMODE_UNCTL;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ONLTOEARTH:
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{
|
||
pREcheck->AttUnStaCnt++;
|
||
}
|
||
|
||
if(pREcheck->AttUnStaCnt>(tmpConst->TimeThr_AttUnSta[3]*2))
|
||
{
|
||
tmpNewMod = ATTMOD_RATEDMP;
|
||
tmpNewModID = ATT_CMDMODE_MODSUNCAP;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ATTAJUSTPRE:
|
||
{
|
||
if(pREcheck->AttModCnt<60000)
|
||
{pREcheck->AttModCnt++;}
|
||
//姿态不稳定判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
|
||
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[4])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[4]))
|
||
{pREcheck->AttUnSta[i]=0;}
|
||
else
|
||
{pREcheck->AttUnSta[i]=0x55;}
|
||
}
|
||
if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[4]*2)))
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{pREcheck->AttUnStaCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttUnStaCnt=0;}
|
||
//连续不稳定判断
|
||
if(pREcheck->AttUnStaCnt>= (tmpConst->TimeThr_AttUnSta[4]*2))
|
||
{
|
||
//发送进入对地定向指令
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_NO;
|
||
pOrb->CurOrbCtrlPcak_ID = 0;
|
||
pOrb->CurOrbCtrlPcak_PreID =0xFF;
|
||
PPPU->PPUT_FIR_Cnt = 0;
|
||
pOrb->OrbDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sOrbitInp.OrbCtrlPackCur, 0, sizeof(AttOrbCtrlPara_t));
|
||
memset(&tmpOrbCmd->OrbCtrlPack, 0, 50*sizeof(AttOrbCtrlPara_t));
|
||
}
|
||
break;
|
||
}
|
||
|
||
case ATTMOD_ATTAJUST:
|
||
{
|
||
if(pREcheck->AttModCnt<60000)
|
||
{pREcheck->AttModCnt++;}
|
||
//姿态不稳定判断
|
||
for(i=0;i<3;i++)
|
||
{
|
||
//计算姿态角偏差
|
||
tmpAngE[i]=tmpAtt->sDeterPara.CLPAttAng[i]-tmpAtt->sGuidLawPara.TargAng[i];
|
||
DeviAngleLimit(&tmpAngE[i]);
|
||
//计算姿态角速度偏差
|
||
tmpAngRateE[i]=tmpAtt->sDeterPara.CLPAttAngRat[i]-tmpAtt->sGuidLawPara.TargAngRat[i];
|
||
|
||
if((POSE_ABSF(tmpAngE[i])<tmpConst->AngThr_AttUnSta[5])&&(POSE_ABSF(tmpAngRateE[i])<tmpConst->RateThr_AttUnSta[5]))
|
||
{pREcheck->AttUnSta[i]=0;}
|
||
else
|
||
{pREcheck->AttUnSta[i]=0x55;}
|
||
}
|
||
if(((0x55 == pREcheck->AttUnSta[0])||(0x55 == pREcheck->AttUnSta[1])||(0x55 == pREcheck->AttUnSta[2]))&&(pREcheck->AttModCnt>= (tmpConst->TimeMod_AttUnSta[5]*2)))
|
||
{
|
||
if(pREcheck->AttUnStaCnt<60000)
|
||
{pREcheck->AttUnStaCnt++;}
|
||
}
|
||
else
|
||
{pREcheck->AttUnStaCnt=0;}
|
||
|
||
if(pREcheck->AttUnStaCnt > (tmpConst->TimeThr_AttUnSta[5]*2))
|
||
{
|
||
tmpNewMod = ATTMOD_ONLTOEARTH;
|
||
tmpNewModID = ATT_CMDMODE_ONLTOEARTH;
|
||
pTARC->CurTARCtrlPcak_ID=0;
|
||
pTARC->CurTARCtrlPcak_PreID =0xFF;
|
||
pTARC->TARDataIn_OK = POSE_NO;
|
||
memset(&tmpAtt->sTARCInp.TARCtrlPackCur, 0, sizeof(sAttTARCCtlPara_t));
|
||
memset(&tmpTARCCmd->TARCtrlPack, 0, 30*sizeof(sAttTARCCtlPara_t));
|
||
|
||
}
|
||
break;
|
||
}
|
||
|
||
default:
|
||
break;
|
||
}
|
||
if (tmpNewMod != pWMod->WorkMode)
|
||
{
|
||
memset(cmdDataBuff, 0x00, 50);
|
||
tmpTaskInfo = GetTaskInfoBT(TASK_ATTCTL);
|
||
DoSetInt32To8(tmpNewModID, cmdDataBuff);
|
||
if(CheckBlockRing(&tmpTaskInfo->sCmdBuffer, tmpTaskInfo->sCmdDataBuff, MOD_REQ_MAXLEN, MOD_MAX_REQUEST))
|
||
{
|
||
/* 在姿控的指令处理中 进行*/
|
||
BlockRingAdd(&tmpTaskInfo->sCmdBuffer, cmdDataBuff, TRUE);
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
/************************************************
|
||
说明：模块主函数，模块每周期调用的所有操作
|
||
输入：
|
||
输出：
|
||
************************************************/
|
||
void ZKREAction(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
sAttErrCtlPara_t *pREcheck =NULL;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
PPUPara_t *PPPU = NULL;
|
||
UINT8 PPUOn2[8] = {0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
return;
|
||
pREcheck=&tmpAtt->sErrCtlPara;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
|
||
//陀螺异常诊断
|
||
GyroImuErrJudge();
|
||
|
||
//星敏异常诊断
|
||
SsImuErrJudge();
|
||
|
||
//模拟磁强计异常诊断
|
||
MagImuErrJudge();
|
||
|
||
//飞轮异常诊断
|
||
WhlImuErrJudge();
|
||
|
||
//PPU异常诊断
|
||
PPUImuErrJudge();
|
||
|
||
/*处理因正常轨控退出开PPU电源操作，以及轨控异常退出关PPU操作，关动作优先开*/
|
||
if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_Err_Flag)/*电推轨控退出*/
|
||
{
|
||
if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt <= 250)/*对计数值做个最大值保护*/
|
||
PPPU->PPU_OrbitCtl_Quit_Err_Cnt++;
|
||
|
||
if (PPPU->PPU_OrbitCtl_Quit_Err_Cnt >= 5)/*2.5秒后，发送电推供电关指令*/
|
||
{
|
||
OCOut(OC73_PPU_OFF, 0, 16);
|
||
PPPU->PPU_OrbitCtl_Quit_Err_Cnt = 0;
|
||
PPPU->PPU_OrbitCtl_Quit_Err_Flag = POSE_NO;
|
||
}
|
||
}
|
||
else if(POSE_OK == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*电推轨控正常退出*/
|
||
{
|
||
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*对计数值做个最大值保护*/
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
|
||
|
||
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 5)/*5秒后，发送电推供电开指令*/
|
||
{
|
||
OCOut(OC68_PPU_ON, 0, 16);
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
|
||
}
|
||
}
|
||
else if(POSE_IN == PPPU->PPU_OrbitCtl_Quit_OK_Flag)/*二次点火*/
|
||
{
|
||
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt <= 250)/*对计数值做个最大值保护*/
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Cnt++;
|
||
|
||
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt == 5)/*2.5秒后，发送电推供电开指令*/
|
||
{
|
||
OCOut(OC68_PPU_ON, 0, 16);
|
||
}
|
||
|
||
if (PPPU->PPU_OrbitCtl_Quit_OK_Cnt >= 65)/*30秒后，发送电推供电开指令*/
|
||
{
|
||
//PPU开机指令
|
||
dev_can_write(DEV_NO_CAN0,PPUOn2,8);
|
||
PPPU->PPUOUT_Sta = POSE_OK;
|
||
tmpAtt->sCtlPara.ZK_Ctrl_Reserver111 = POSE_OK;
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Cnt = 0;
|
||
PPPU->PPU_OrbitCtl_Quit_OK_Flag = POSE_NO;
|
||
pREcheck->PPUOUT_FIRECnt=0;
|
||
pREcheck->PPU_Err = pREcheck->PPU_Err & 0xCF ;
|
||
}
|
||
}
|
||
else
|
||
{;}/*不做处理*/
|
||
//姿态基准丢失判断
|
||
if(0x55 == tmpConst->AllowAttLoseDiagFlg)
|
||
{
|
||
AttLoseJudge();
|
||
}
|
||
else
|
||
{
|
||
pREcheck->AttLoseCnt=0;
|
||
}
|
||
|
||
//姿态超差判断
|
||
if((0x55 == tmpConst->AllowAttExDiagFlg)&&(0x55 == pDeter->CLPAttValid))
|
||
{AttExJudge();}
|
||
else
|
||
{
|
||
pREcheck->AttExFlg[0]=0x00;
|
||
pREcheck->AttExFlg[1]=0x00;
|
||
pREcheck->AttExFlg[2]=0x00;
|
||
pREcheck->AttExCnt=0;
|
||
}
|
||
|
||
//姿态不稳定判断
|
||
if((0x55 == tmpConst->AllowAttStaDiagFlg)&&(0x55 == pDeter->CLPAttValid))
|
||
{AttUnStaJudge(); }
|
||
else
|
||
{
|
||
pREcheck->AttUnSta[0] = 0;
|
||
pREcheck->AttUnSta[1] = 0;
|
||
pREcheck->AttUnSta[2] = 0;
|
||
pREcheck->AttUnStaCnt=0;
|
||
}
|
||
}
|
||
|