2137 lines
56 KiB
C
2137 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"
|
||
#include "..\PrjTelCtrlMng\TelCtrlMng.h"
|
||
|
||
/***********************************************
|
||
说明：数据处理模块初始化函数,进程开始调用一次
|
||
***********************************************/
|
||
void ZKDPInit(void)
|
||
{
|
||
UINT32 i = 0;
|
||
sAttPriData *tmpAtt = NULL;
|
||
SsPara_t *pStar= NULL;
|
||
GyroPara_t *pGyro= NULL;
|
||
AssPara_t *pAss= NULL;
|
||
MagPara_t *pMag= NULL;
|
||
WhlPara_t *PWhl= NULL;
|
||
PPUPara_t *PPPU= NULL;
|
||
|
||
tmpAtt = (sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL==tmpAtt)
|
||
{
|
||
return;
|
||
}
|
||
|
||
pAss = &tmpAtt->sPerPara.AssPara;
|
||
pMag = &tmpAtt->sPerPara.MagPara;
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
|
||
//星敏
|
||
for (i = 0; i < STAR_NUM; i++)
|
||
{
|
||
pStar = &tmpAtt->sPerPara.SsPara[i];
|
||
pStar->ssIn_AttVld = POSE_NO;
|
||
pStar->ssIn_DataSta = 0;
|
||
pStar->ssCrCheckFlg = NO_CC;
|
||
memset(pStar->ssIn_W, 0, 3*sizeof(TYPE_CAL));
|
||
pStar->ssExpose_Qbi[0] = 0;
|
||
pStar->ssExpose_Qbi[1] = 0;
|
||
pStar->ssExpose_Qbi[2] = 0;
|
||
pStar->ssExpose_Qbi[3] = 1;
|
||
memset(pStar->ssExpose_Wbi, 0, 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
tmpAtt->sPerPara.Ss_Qi[0] = 0;
|
||
tmpAtt->sPerPara.Ss_Qi[1] = 0;
|
||
tmpAtt->sPerPara.Ss_Qi[2] = 0;
|
||
tmpAtt->sPerPara.Ss_Qi[3] = 1;
|
||
memset(tmpAtt->sPerPara.Ss_Wi, 0.0, 3*sizeof(TYPE_CAL));
|
||
|
||
//陀螺
|
||
for (i = 0; i < GYRO_NUM; i++)
|
||
{
|
||
pGyro = &tmpAtt->sPerPara.GyroPara[i];
|
||
pGyro->GyroIn_AttVld = POSE_NO;
|
||
pGyro->Gyro_ComTHUse[0] = POSE_OK;
|
||
pGyro->Gyro_ComTHUse[1] = POSE_OK;
|
||
pGyro->Gyro_ComTHUse[2] = POSE_OK;
|
||
pGyro->GyroIn_DataSta = 0;
|
||
pGyro->GyroCrCheckFlg =NO_CC;
|
||
memset(pGyro->GyroIn_Tmper, 0, 3*sizeof(TYPE_CAL));
|
||
memset(pGyro->Gyro_Wi, 0, 3*sizeof(TYPE_CAL));
|
||
}
|
||
memset(tmpAtt->sPerPara.GyroIn_CombWi, 0, 3*sizeof(TYPE_CAL));
|
||
memset(tmpAtt->sPerPara.Gyro_ATTWi, 0, 3*sizeof(TYPE_CAL));
|
||
tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;
|
||
|
||
//模拟太敏
|
||
for (i = 0; i < ASS_NUM; i++)
|
||
{
|
||
pAss->AssIn_AttVld[i]= POSE_NO;
|
||
memset(pAss->AssIn_sensor[i], 0, 4*sizeof(TYPE_CAL));
|
||
pAss->AssCrCheckFlg =NO_CC;
|
||
}
|
||
pAss->Ass_SunVecb[0]= 0; //模拟太敏本体系太阳矢量XYZ
|
||
pAss->Ass_SunVecb[1]= 0; //模拟太敏本体系太阳矢量XYZ
|
||
pAss->Ass_SunVecb[2]= 0; //模拟太敏本体系太阳矢量XYZ
|
||
pAss->Ass_SunVecVld= POSE_NO; //模拟太敏太阳矢量有效标志
|
||
pAss->Ass_SunVecbF[0]= 0; //模拟太敏帆板系太阳矢量XYZ
|
||
pAss->Ass_SunVecbF[1]= 0; //模拟太敏帆板系太阳矢量XYZ
|
||
pAss->Ass_SunVecbF[2]= 0; //模拟太敏帆板系太阳矢量XYZ
|
||
pAss->Ass_SunATT[0] = 0;
|
||
pAss->Ass_SunATT[1] = 0;
|
||
pAss->Ass_SunOK= POSE_NO;
|
||
pAss->ADIn_Date= POSE_OK;
|
||
|
||
//磁强计
|
||
for (i = 0; i < MAG_NUM; i++)
|
||
{
|
||
pMag->MagIn_Valid[i]= POSE_NO; //模拟磁强计A/B有效标志
|
||
//输出量
|
||
memset(pMag->Mag_Bc_MeaAB[i],0,3*sizeof(TYPE_CAL));
|
||
memset(pMag->Mag_Bb_MeaAB[i],0,3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->MagIn_Valid[2]= POSE_NO; //模拟磁强计A/B有效标志
|
||
pMag->MagIn_DataSta= 0;
|
||
pMag->MagUseSwich= 0;
|
||
pMag->Mag_Bb_MeaZH[0] =0;
|
||
pMag->Mag_Bb_MeaZH[1] =0;
|
||
pMag->Mag_Bb_MeaZH[2] =0;
|
||
for (i = 0; i < MAG_NUM; i++)
|
||
{
|
||
pMag->MagCrCheckFlg[i]= NO_CC;
|
||
pMag->MagAvailableFlg[i] = POSE_OK;
|
||
}
|
||
memset(pMag->Mag_UBb,0,3*sizeof(TYPE_CAL));
|
||
|
||
//飞轮
|
||
for (i = 0; i < WHEEL_NUM; i++)
|
||
{
|
||
PWhl->WhlIn_DataSta[i] = 0;
|
||
PWhl->WhlIn_Valid[i] = POSE_NO;
|
||
PWhl->Whl_Current[i]= 0.0f;
|
||
PWhl->Whl_Tmper[i] = 0.0f;
|
||
PWhl->Whl_Momentum[i]= 0; //四个飞轮的角动量
|
||
}
|
||
for(i=0;i<3;i++)
|
||
{
|
||
PWhl->Whl_Momentum_XYZ[i]= 0.0; //飞轮三轴的角动量
|
||
}
|
||
PWhl->Whl_Momentum_Sum = 0;
|
||
|
||
#ifndef MINMODULE_TYPE
|
||
//电推
|
||
PPPU->PPUIn_Valid = POSE_NO; //PPU数据有效标志
|
||
PPPU->PPUIn_DataSta = 0; //PPU 数据无效原因
|
||
|
||
PPPU->PPUIn_ReCnt= 0; //PPU遥测请求计数
|
||
PPPU->PPUIn_AVoltage = 0; //推进阳极电源电压
|
||
PPPU->PPUIn_ACurrent = 0; //推进阳极电源电流
|
||
|
||
PPPU->PPUT_BOTTLE_Tmper1 = 0.0f;
|
||
PPPU->PPUT_BOTTLE_Tmper2 = 0.0f;
|
||
PPPU->PPUT_PIPE_Tmper1 = 0.0f;
|
||
PPPU->PPUT_PIPE_Tmper2 = 0.0f;
|
||
|
||
#endif
|
||
}
|
||
|
||
/***********************************************
|
||
说明：获取星敏数据，注意星敏A\B原包数据均为小端
|
||
***********************************************/
|
||
void Ss_DataGet(void ) //ST-APS2-2B(X)星敏感器
|
||
{
|
||
UINT8 sum = 0;
|
||
UINT8 i = 0;
|
||
UINT8 j = 0;
|
||
UINT16 tmpHEAD[3] = {0,0,0};
|
||
UINT16 tmpINFIN[3] ={0,0,0};
|
||
INT32 tempQdata[4] = {0,0,0,1};
|
||
INT16 tempWdata = 0;
|
||
INT32 tempQdataC = 0;
|
||
INT16 tempWdataC = 0;
|
||
sAttPriData *tmpAtt = NULL;
|
||
sDevInfo * tmpDevInfo[3]= {NULL};
|
||
SsPara_t *PStar[STAR_NUM]= {NULL};
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL==tmpAtt)
|
||
{
|
||
return;
|
||
}
|
||
|
||
//获取设备数据地址
|
||
tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART15); //星敏A（803）
|
||
tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART16); //星敏B（803）
|
||
tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART4); //星敏C（天银）
|
||
|
||
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
|
||
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
|
||
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
|
||
|
||
//记录前次原包数据
|
||
for (i = 0; i < STAR_NUM; i++)
|
||
{
|
||
memcpy(PStar[i]->ssIn_Qpre, PStar[i]->ssIn_Q, 4*sizeof(TYPE_CAL));
|
||
//初始化
|
||
PStar[i]->ssIn_Q[0] = 0;
|
||
PStar[i]->ssIn_Q[1] = 0;
|
||
PStar[i]->ssIn_Q[2] = 0;
|
||
PStar[i]->ssIn_Q[3] = 1;
|
||
}
|
||
|
||
//获取星敏单机包头
|
||
for (i = 0; i < STAR_NUM; i++)
|
||
{
|
||
tmpHEAD[i] = (tmpDevInfo[i]->cDevYCBuffer[0 + 20]<<8)|tmpDevInfo[i]->cDevYCBuffer[0 + 20+1];
|
||
}
|
||
//获取星敏单机信息字
|
||
tmpINFIN[0] = (tmpDevInfo[0]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + 20+3];
|
||
tmpINFIN[1] = (tmpDevInfo[1]->cDevYCBuffer[0 + 20 +2]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + 20+3];
|
||
tmpINFIN[2] = tmpDevInfo[2]->cDevYCBuffer[0 + 20+2];
|
||
|
||
//星敏AB数据获取
|
||
for (i = 0; i < (STAR_NUM -1); i++)
|
||
{
|
||
if (INFO_SIG_CHANGE == tmpDevInfo[i] ->cDevYCBuffer[0])
|
||
{
|
||
if ((STARAB_HEAD != tmpHEAD[i]) || (STARAB_ID != tmpINFIN[i]))
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_NO;
|
||
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta , 1, 1 ); /* errNo=0,星敏包头|ID号错误 */
|
||
}
|
||
else
|
||
{
|
||
sum = 0;
|
||
/*判断校验和*/
|
||
for(j = 0; j < (STARAB_LEN - 1); j++)
|
||
{
|
||
sum += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];
|
||
}
|
||
if ((0xFF & sum) != tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+STARAB_LEN-1])
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_NO;
|
||
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 2, 1 ); /* errNo=0,星敏数据校验和不对 */
|
||
}
|
||
else
|
||
{
|
||
//星敏数据包中状态字1判断
|
||
if (0x20 == (tmpDevInfo[i]->cDevYCBuffer[20+20] & 0x30))
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_NO;
|
||
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 3, 1 ); /* 星敏感器状态字1中数据无效*/
|
||
}
|
||
else
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_NO;
|
||
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 0, 1 ); /* errNo=0,星敏数据不更新 */
|
||
}
|
||
|
||
//解析星敏AB的数据
|
||
if (POSE_OK == PStar[i]->ssIn_AttVld)
|
||
{
|
||
//四元数
|
||
for(j = 0; j < 4; j++)
|
||
{
|
||
tempQdata[j] = DoGetInt32From8(&(tmpDevInfo[i]->cDevYCBuffer[20+4*j+4]));
|
||
}
|
||
PStar[i]->ssIn_Q[3] = tempQdata[0]/2147483647.5f;
|
||
PStar[i]->ssIn_Q[0] = tempQdata[1]/2147483647.5f;
|
||
PStar[i]->ssIn_Q[1] = tempQdata[2]/2147483647.5f;
|
||
PStar[i]->ssIn_Q[2] = tempQdata[3]/2147483647.5f;
|
||
|
||
//角速度
|
||
for(j = 0; j < 3; j++)
|
||
{
|
||
tempWdata = DoGetInt16From8(&tmpDevInfo[i]->cDevYCBuffer[20+2*j+45]);
|
||
PStar[i]->ssIn_W[j] = ((float)tempWdata)*ANG2RADIAN*POSE_2T13;
|
||
}
|
||
PStar[i]->ssIn_W[2] = - PStar[i]->ssIn_W[2];
|
||
}
|
||
}
|
||
|
||
//星敏C数据获取
|
||
if (INFO_SIG_CHANGE == tmpDevInfo[2] ->cDevYCBuffer[0])
|
||
{
|
||
if ((STARC_HEAD != tmpHEAD[2]) || (STARC_ID != tmpINFIN[2]))
|
||
{
|
||
PStar[2]->ssIn_AttVld = POSE_NO;
|
||
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta , 1, 1 ); /* errNo=0,星敏包头|ID号错误 */
|
||
}
|
||
else
|
||
{
|
||
sum = 0;
|
||
/*判断校验和*/
|
||
for(j = 2; j < (STARC_LEN - 1); j++)
|
||
{sum += tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+j];}
|
||
if ((0xFF & sum) != tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+STARC_LEN - 1])
|
||
{
|
||
PStar[2]->ssIn_AttVld = POSE_NO;
|
||
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 2, 1 ); /* errNo=0,星敏数据校验和不对 */
|
||
}
|
||
else
|
||
{
|
||
//星敏数据包中状态字1判断
|
||
if ((tmpDevInfo[2]->cDevYCBuffer[20+33] & 0x01) != 0x01)
|
||
{
|
||
PStar[2]->ssIn_AttVld = POSE_NO;
|
||
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 3, 1 ); /* 星敏感器状态字1中数据无效*/
|
||
}
|
||
else
|
||
{
|
||
PStar[2]->ssIn_AttVld = POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PStar[2]->ssIn_AttVld = POSE_NO;
|
||
PStar[2]->ssIn_DataSta = ZERO_TO_ONE(PStar[2]->ssIn_DataSta, 0, 1 ); /* errNo=0,星敏数据不更新 */
|
||
}
|
||
|
||
//解析星敏C的数据
|
||
if (POSE_OK == PStar[2]->ssIn_AttVld)
|
||
{
|
||
//四元数
|
||
for(j = 0; j < 4; j++)
|
||
{
|
||
tempQdataC = DoGetInt32From8(&tmpDevInfo[2]->cDevYCBuffer[20+4*j+3]);
|
||
PStar[2]->ssIn_Q[j] = ((float)tempQdataC)/2147483647.0f;
|
||
}
|
||
//角速度
|
||
for(j = 0; j < 3; j++)
|
||
{
|
||
tempWdataC = DoGetInt16From8(&tmpDevInfo[2]->cDevYCBuffer[20+2*j+42]);
|
||
PStar[2]->ssIn_W[j] = ((float)tempWdataC)*ANG2RADIAN*POSE_2T11;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：星敏数据处理
|
||
***********************************************/
|
||
void Ss_DataPre(void)
|
||
{
|
||
UINT8 i = 0;
|
||
UINT8 j = 0;
|
||
TYPE_CAL tmpRbs[9] = {0.0};
|
||
TYPE_CAL tmpRsi[9] = {0.0};
|
||
TYPE_CAL tmpRbi[9] = {0.0};
|
||
TYPE_CAL tmp_SS[3][3][3] = {0.0}; //星敏安装矩阵
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
SsPara_t *PStar[STAR_NUM] = {NULL};
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt)||(NULL==tmpConst))
|
||
return;
|
||
|
||
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
|
||
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
|
||
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
|
||
|
||
memcpy(&(tmp_SS[0][0][0]), &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9*sizeof(TYPE_CAL));
|
||
memcpy(&(tmp_SS[1][0][0]), &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9*sizeof(TYPE_CAL));
|
||
memcpy(&(tmp_SS[2][0][0]), &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9*sizeof(TYPE_CAL));
|
||
|
||
for (i = 0; i < STAR_NUM; i++)
|
||
{
|
||
if ((POSE_OK ==PStar[i]->ssIn_AttVld))
|
||
{
|
||
if (POSE_ABSF(PStar[i]->ssIn_Qpre[0]-PStar[i]->ssIn_Q[0])< SENSOR_ZERO
|
||
&&POSE_ABSF(PStar[i]->ssIn_Qpre[1]-PStar[i]->ssIn_Q[1])< SENSOR_ZERO
|
||
&&POSE_ABSF(PStar[i]->ssIn_Qpre[2]-PStar[i]->ssIn_Q[2])< SENSOR_ZERO
|
||
&&POSE_ABSF(PStar[i]->ssIn_Qpre[3]-PStar[i]->ssIn_Q[3])< SENSOR_ZERO)
|
||
{
|
||
if(PStar[i]->ssExpose_cnt<255)
|
||
{
|
||
PStar[i]->ssExpose_cnt++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PStar[i]->ssExpose_cnt =0;
|
||
}
|
||
|
||
if (PStar[i]->ssExpose_cnt >= 5)
|
||
{
|
||
PStar[i]->ssIn_AttVld = POSE_NO;
|
||
PStar[i]->ssIn_DataSta = ZERO_TO_ONE(PStar[i]->ssIn_DataSta, 4, 1 );
|
||
}
|
||
else
|
||
{
|
||
if(PStar[i]->ssIn_Q[3]<0)
|
||
{
|
||
for (j = 0; j < 4; j++)
|
||
{
|
||
PStar[i]->ssIn_Q[j] = - PStar[i]->ssIn_Q[j];
|
||
}
|
||
}
|
||
|
||
Q2Att(PStar[i]->ssIn_Q, tmpRsi);
|
||
memcpy(tmpRbs, &(tmp_SS[i][0][0]), 9 * sizeof(TYPE_CAL));
|
||
MatrixProductHL(tmpRbs, tmpRsi, tmpRbi, 3, 3, 3);
|
||
Att2Q(tmpRbi, PStar[i]->ssExpose_Qbi);
|
||
MatrixProductHL(tmpRbs, PStar[i]->ssIn_W, PStar[i]->ssExpose_Wbi, 3, 3, 1);
|
||
PStar[i]->ssIn_AttVld=POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：获取陀螺数据
|
||
***********************************************/
|
||
void Gyro_DataGet(void)
|
||
{
|
||
UINT8 i = 0;
|
||
UINT8 j = 0;
|
||
UINT8 k = 0;
|
||
INT32 tmpDtInt = 0;
|
||
INT16 tmpDtTmper = 0;
|
||
UINT8 sum[GYRO_NUM] = {0,0,0};
|
||
UINT16 tmpHEAD[GYRO_NUM] = {0,0,0};
|
||
UINT8 tmpLONG[GYRO_NUM] = {0,0,0};
|
||
UINT16 tmpDATAHEAD[GYRO_NUM] = {0,0,0};
|
||
TYPE_CAL tmpGyroIn_Speed[3]= {0.0,0.0,0.0};
|
||
TYPE_CAL tmpkGyroIn_Speed[3]= {0.0,0.0,0.0};
|
||
UINT8 tmpcnt[GYRO_NUM][3] = {{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK},{POSE_OK,POSE_OK,POSE_OK}};
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
sDevInfo * tmpDevInfo[GYRO_NUM] = {NULL};
|
||
GyroPara_t *PGyro[GYRO_NUM] = {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];
|
||
|
||
//获取陀螺地址
|
||
tmpDevInfo[0] = GETDEVINFO(DEV_NO_UART5) ;
|
||
tmpDevInfo[1] = GETDEVINFO(DEV_NO_UART6) ;
|
||
tmpDevInfo[2] = GETDEVINFO(DEV_NO_UART7) ;
|
||
//获取数据包头
|
||
tmpHEAD[0]=(tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[0]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
|
||
tmpHEAD[1]=(tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[1]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
|
||
tmpHEAD[2]=(tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF]<<8)|tmpDevInfo[2]->cDevYCBuffer[0 + INFO_ADDR_OFF+1];
|
||
tmpDATAHEAD[0] = GYRO_DATA_HEADA;
|
||
tmpDATAHEAD[1] = GYRO_DATA_HEADA;
|
||
tmpDATAHEAD[2] = GYRO_DATA_HEADC;
|
||
//获取数据长
|
||
tmpLONG[0] = GYRO_DATA_LENA;
|
||
tmpLONG[1] = GYRO_DATA_LENA;
|
||
tmpLONG[2] = GYRO_DATA_LENC;
|
||
|
||
//记录前次原包数据
|
||
for (i = 0; i < GYRO_NUM; i++)
|
||
{
|
||
memcpy(PGyro[i]->GyroIn_SpeedPre, PGyro[i]->GyroIn_Speed, 3*sizeof(TYPE_CAL));
|
||
//初始化
|
||
memset(PGyro[i]->GyroIn_Speed, 0, 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
for (i = 0; i < GYRO_NUM ; ++i)
|
||
{
|
||
if (INFO_SIG_CHANGE == tmpDevInfo[i]->cDevYCBuffer[0])
|
||
{
|
||
if (tmpHEAD[i] == tmpDATAHEAD[i])
|
||
{
|
||
/*判断校验和*/
|
||
for(j = 0; j < tmpLONG[i] - 1; j++)
|
||
{
|
||
sum[i] += tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+j];//陀螺的包长
|
||
}
|
||
|
||
if ((0xFF & sum[i]) == tmpDevInfo[i]->cDevYCBuffer[INFO_ADDR_OFF+tmpLONG[i] - 1])
|
||
{
|
||
PGyro[i]->GyroIn_AttVld = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->GyroIn_AttVld = POSE_NO;
|
||
PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 2, 1 ); /* errNo=0,陀螺A数据校验和错误 */
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->GyroIn_AttVld = POSE_NO;
|
||
PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta, 1, 1 ); /* errNo=0,陀螺A包头|包长错误 */
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->GyroIn_AttVld = POSE_NO;
|
||
PGyro[i]->GyroIn_DataSta= ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 0, 1 ); /* errNo=0,陀螺A数据不更新 */
|
||
}
|
||
}
|
||
|
||
//陀螺AB数据解析
|
||
for (j = 0; j < 2 ; ++j)
|
||
{
|
||
if (POSE_OK ==PGyro[j]->GyroIn_AttVld)
|
||
{
|
||
for (i = 0; i < 3 ; ++i)
|
||
{
|
||
//XYZ轴角速度,单位：°/s
|
||
tmpDtInt = DoGetInt32From8(&(tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+4*i+9]));
|
||
tmpGyroIn_Speed[i] = (float)tmpDtInt * (1.0E-5)*ANG2RADIAN ;
|
||
//XYZ轴角温度，单位：℃
|
||
tmpDtTmper= tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+21]<<8|tmpDevInfo[j]->cDevYCBuffer[INFO_ADDR_OFF+2*i+22];
|
||
PGyro[j]->GyroIn_Tmper[i] = (float)(tmpDtTmper)*(1.0E-2);
|
||
}
|
||
MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[j][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);
|
||
for (i = 0; i < 3 ; ++i)
|
||
{
|
||
//XYZ轴角速度,单位：°/s
|
||
PGyro[j]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[j][i] ;
|
||
}
|
||
}
|
||
}
|
||
|
||
//陀螺C数据解析
|
||
if (POSE_OK == PGyro[2]->GyroIn_AttVld)
|
||
{
|
||
for (i = 0; i < 3 ; ++i)
|
||
{
|
||
//XYZ轴角速度,单位：°/s
|
||
tmpGyroIn_Speed[i] = ((PoseGetInt32From24(&(tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+3*i+9]))))*POSE_POW(2,-18)*ANG2RADIAN ;
|
||
//XYZ轴角温度，单位：℃
|
||
tmpDtTmper= tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+19]<<8|tmpDevInfo[2]->cDevYCBuffer[INFO_ADDR_OFF+2*i+20];
|
||
PGyro[2]->GyroIn_Tmper[i] = (float)(tmpDtTmper)*POSE_POW(2,-8);
|
||
}
|
||
MatrixProductHL(tmpConst->AttCmdFlashPara.Gyro_Comd_K[2][0] , tmpGyroIn_Speed, tmpkGyroIn_Speed, 3, 3, 1);
|
||
for (i = 0; i < 3 ; ++i)
|
||
{
|
||
//XYZ轴角速度,单位：°/s
|
||
PGyro[2]->GyroIn_Speed[i] = tmpkGyroIn_Speed[i] + tmpConst->AttCmdFlashPara.Gyro_Comd_Bias[2][i] ;
|
||
}
|
||
}
|
||
|
||
//陀螺数据判断
|
||
for (i = 0; i < GYRO_NUM ; ++i)
|
||
{
|
||
if (POSE_OK == PGyro[i]->GyroIn_AttVld)
|
||
{ for (k = 0; k < 3 ; ++k)
|
||
{
|
||
if (POSE_ABSF(PGyro[i]->GyroIn_SpeedPre[k] - PGyro[i]->GyroIn_Speed[k]) < POSE_ZERO)
|
||
{
|
||
if (PGyro[i]->Gyro_ComCnt[k] < 60000)
|
||
{
|
||
PGyro[i]->Gyro_ComCnt[k]++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->Gyro_ComCnt[k] =0;
|
||
}
|
||
if (POSE_ABSF(PGyro[i]->GyroIn_Speed[k]) > 0.436)
|
||
{
|
||
tmpcnt[i][k] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
tmpcnt[i][k] = POSE_OK;
|
||
}
|
||
|
||
if ((PGyro[i]->Gyro_ComCnt[k] < 5)&& (tmpcnt[i][k] == POSE_OK))
|
||
{
|
||
PGyro[i]->Gyro_ComTHUse[k] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->Gyro_ComTHUse[k] = POSE_NO;
|
||
PGyro[i]->GyroIn_DataSta = ZERO_TO_ONE( PGyro[i]->GyroIn_DataSta , 3+k, 1 );
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PGyro[i]->Gyro_ComTHUse[0] = POSE_NO;
|
||
PGyro[i]->Gyro_ComTHUse[1] = POSE_NO;
|
||
PGyro[i]->Gyro_ComTHUse[2] = POSE_NO;
|
||
}
|
||
|
||
if ((POSE_NO == PGyro[i]->Gyro_ComTHUse[0])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[1])||(POSE_NO == PGyro[i]->Gyro_ComTHUse[2]))
|
||
{
|
||
PGyro[i]->GyroIn_AttVld= POSE_NO;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：陀螺数据处理
|
||
***********************************************/
|
||
void Gyro_DataPre(void)
|
||
{
|
||
TYPE_CAL tmpRbs[9]= {0.0};
|
||
TYPE_CAL tmpRbi[3]= {0.0,0.0,0.0};//陀螺输出角速度XYZ
|
||
UINT8 i = 0;
|
||
UINT8 k = 0;
|
||
UINT8 tmpGyro = POSE_NO;
|
||
UINT8 tmpComTHUse[3] = {0,0,0};
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
GyroPara_t *PGyro[GYRO_NUM] = {NULL};
|
||
AttIMPTPara_t *pIMU = NULL;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt)||(NULL==tmpConst))
|
||
{
|
||
return;
|
||
}
|
||
|
||
pIMU=&tmpAtt->sIMPTPara;
|
||
|
||
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
|
||
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
|
||
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
|
||
|
||
if ((0x55 == PGyro[0]->GyroIn_AttVld) || (0x55 == PGyro[1]->GyroIn_AttVld) || (0x55 == PGyro[2]->GyroIn_AttVld))
|
||
{
|
||
for (i = 0; i < GYRO_NUM ; ++i)
|
||
{
|
||
tmpGyro = POSE_NO;
|
||
if (POSE_OK == PGyro[i]->GyroIn_AttVld)
|
||
{
|
||
for (k = 0; k < 3 ; ++k)
|
||
{
|
||
tmpRbi[k] = PGyro[i]->GyroIn_Speed[k];
|
||
}
|
||
tmpGyro = POSE_OK;
|
||
}
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_GYRO[i][0][0]), 9 * sizeof(TYPE_CAL));
|
||
|
||
if (tmpGyro == POSE_OK)
|
||
{
|
||
MatrixProductHL(tmpRbs, tmpRbi, PGyro[i]->Gyro_Wi, 3, 3, 1);
|
||
}
|
||
}
|
||
}
|
||
|
||
if((POSE_NO == pIMU->GyroUseState[0]) && (POSE_NO == pIMU->GyroUseState[1]) && (POSE_NO == pIMU->GyroUseState[2]))
|
||
{
|
||
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
|
||
{
|
||
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][0];
|
||
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][0];
|
||
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][0];
|
||
tmpRbi[0] = PGyro[0]->GyroIn_Speed[0];
|
||
tmpComTHUse[0] = 0x55;
|
||
}
|
||
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
|
||
{
|
||
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][0];
|
||
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][0];
|
||
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][0];
|
||
tmpRbi[0] = PGyro[1]->GyroIn_Speed[0];
|
||
tmpComTHUse[0] = 0x55;
|
||
}
|
||
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[0])
|
||
{
|
||
tmpRbs[0] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][0];
|
||
tmpRbs[3] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][0];
|
||
tmpRbs[6] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][0];
|
||
tmpRbi[0] = PGyro[2]->GyroIn_Speed[0];
|
||
tmpComTHUse[0] = 0x55;
|
||
}
|
||
|
||
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
|
||
{
|
||
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][1];
|
||
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][1];
|
||
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][1];
|
||
tmpRbi[1] = PGyro[0]->GyroIn_Speed[1];
|
||
tmpComTHUse[1] = 0x55;
|
||
}
|
||
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
|
||
{
|
||
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][1];
|
||
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][1];
|
||
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][1];
|
||
tmpRbi[1] = PGyro[1]->GyroIn_Speed[1];
|
||
tmpComTHUse[1] = 0x55;
|
||
}
|
||
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[1])
|
||
{
|
||
tmpRbs[1] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][1];
|
||
tmpRbs[4] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][1];
|
||
tmpRbs[7] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][1];
|
||
tmpRbi[1] = PGyro[2]->GyroIn_Speed[1];
|
||
tmpComTHUse[1] = 0x55;
|
||
}
|
||
|
||
if (0x11 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
|
||
{
|
||
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[0][0][2];
|
||
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[0][1][2];
|
||
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[0][2][2];
|
||
tmpRbi[2] = PGyro[0]->GyroIn_Speed[2];
|
||
tmpComTHUse[2] = 0x55;
|
||
}
|
||
else if (0x22 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
|
||
{
|
||
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[1][0][2];
|
||
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[1][1][2];
|
||
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[1][2][2];
|
||
tmpRbi[2] = PGyro[1]->GyroIn_Speed[2];
|
||
tmpComTHUse[2] = 0x55;
|
||
}
|
||
else if (0x44 == tmpConst->AttCmdFlashPara.Gyro_ComRESW[2])
|
||
{
|
||
tmpRbs[2] = tmpConst->AttCmdFlashPara.M_GYRO[2][0][2];
|
||
tmpRbs[5] = tmpConst->AttCmdFlashPara.M_GYRO[2][1][2];
|
||
tmpRbs[8] = tmpConst->AttCmdFlashPara.M_GYRO[2][2][2];
|
||
tmpRbi[2] = PGyro[2]->GyroIn_Speed[2];
|
||
tmpComTHUse[2] = 0x55;
|
||
}
|
||
|
||
if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]) )
|
||
{
|
||
tmpAtt->sPerPara.GyroIn_Comb = POSE_OK;
|
||
MatrixProductHL(tmpRbs, tmpRbi, tmpAtt->sPerPara.GyroIn_CombWi, 3, 3, 1);
|
||
}
|
||
else
|
||
{
|
||
tmpAtt->sPerPara.GyroIn_Comb = POSE_NO;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：获取模拟太敏数据
|
||
***********************************************/
|
||
void Ass_DataGet(void )
|
||
{
|
||
UINT32 i = 0;
|
||
UINT32 j = 0;
|
||
INT16 tmpDtInt = 0;
|
||
INT16 *pAss16 = NULL;
|
||
sDevInfo * tmpDevInfo = NULL;
|
||
sAttPriData *tmpAtt = NULL;
|
||
AssPara_t *pAss = NULL;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL==tmpAtt)
|
||
{
|
||
return;
|
||
}
|
||
|
||
pAss = &tmpAtt->sPerPara.AssPara;
|
||
|
||
//UINT32 ID[6] = {97,113,101,117,121,125};
|
||
UINT32 ID[6] = {SA1_TM_XP1-1, SA9_TM_YN1-1,SA5_TM_XN1-1,SA13_TM_YP1-1,SA17_TM_ZN1-1,SA21_TM_P1-1};
|
||
|
||
//获取AD采集数据
|
||
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
|
||
pAss16 = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
|
||
//模拟太敏有效性判断
|
||
if (INFO_SIG_CHANGE ==tmpDevInfo->cDevYCBuffer[0])
|
||
{
|
||
for (i= 0; i < ASS_NUM; i++)
|
||
{
|
||
for (j= 0; j < 4; j++)
|
||
{
|
||
//获取模拟太敏6*4电池片电压
|
||
tmpDtInt = *(pAss16 + ID[i] +j);
|
||
pAss->AssIn_sensor[i][j] = (((TYPE_CAL)(tmpDtInt))*10/4095.0-5.0)/966.0*1000.0 ;
|
||
}
|
||
}
|
||
pAss->ADIn_Date = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pAss->ADIn_Date = POSE_NO;
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：太敏数据处理
|
||
***********************************************/
|
||
void Ass_DataPre(void)
|
||
{
|
||
UINT32 i = 0;
|
||
UINT32 j = 0;
|
||
UINT32 k = 0;
|
||
UINT32 iMax = 0;
|
||
UINT8 tempAssUseState[ASS_NUM]= {0,0,0,0,0,0};; //单个太敏可用标志
|
||
TYPE_CAL tmpSumMax = 0.0;
|
||
TYPE_CAL tmpAss_data_SUM[ASS_NUM] = {0.0,0.0,0.0,0.0,0.0,0.0};
|
||
TYPE_CAL tmpU[4] = {0.0,0.0,0.0,0.0};
|
||
TYPE_CAL tmpRbs[9]= {0.0};
|
||
TYPE_CAL tmpSm[3]= {0.0};
|
||
TYPE_CAL tmpXs = 0.0;
|
||
TYPE_CAL tmpYs = 0.0;
|
||
TYPE_CAL tmpX = 1.0;
|
||
TYPE_CAL tmpY = 1.0;
|
||
TYPE_CAL tmpa=5.0;//电池片宽度
|
||
TYPE_CAL tmpH=1.95;//光栅距离电池面距离
|
||
TYPE_CAL tmpTheta = 0.0;
|
||
TYPE_CAL tmpGamma = 0.0;
|
||
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
AssPara_t *pAss = 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;
|
||
}
|
||
|
||
pAss = &tmpAtt->sPerPara.AssPara;
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
|
||
if (POSE_OK == pAss->ADIn_Date)
|
||
{
|
||
for (i= 0; i < 4; i++)
|
||
{
|
||
for (j= 0; j < 4; j++)
|
||
{
|
||
tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
|
||
}
|
||
if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[0] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[0]))
|
||
{
|
||
pAss->AssIn_AttVld[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pAss->AssIn_AttVld[i] = POSE_NO;
|
||
}
|
||
}
|
||
for (i= 4; i < ASS_NUM; i++)
|
||
{
|
||
for (j= 0; j < 4; j++)
|
||
{
|
||
tmpAss_data_SUM[i] += pAss->AssIn_sensor[i][j];
|
||
}
|
||
if ((tmpAss_data_SUM[i] < tmpConst->Ass_MaxFour_limt[1] )&&(tmpAss_data_SUM[i] > tmpConst->Ass_MinFour_limt[1]))
|
||
{
|
||
pAss->AssIn_AttVld[i] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pAss->AssIn_AttVld[i] = POSE_NO;
|
||
}
|
||
}
|
||
}
|
||
|
||
if ((0x55 == pWMod->WModSadaOpen)&& (0x55 != pWMod->WModSadaOpenPre))
|
||
{
|
||
pImpt->AssUseState[4] = POSE_NO;
|
||
}
|
||
|
||
//选择当前使用的模拟太敏，如果均不可用，则没有太敏可用于计算太阳矢量，输出模拟太敏矢量无效
|
||
for (k= 0; k < ASS_NUM -1; k++)
|
||
{
|
||
if ((POSE_OK == pAss->AssIn_AttVld[k])&& (POSE_OK == pImpt->AssUseState[k]))
|
||
{
|
||
if(tmpAss_data_SUM[k]>tmpSumMax)
|
||
{
|
||
tmpSumMax=tmpAss_data_SUM[k];
|
||
iMax=k;
|
||
}
|
||
tempAssUseState[k] = 0x55;
|
||
}
|
||
}
|
||
|
||
if((0 == tempAssUseState[0])&&(0 == tempAssUseState[1])&&(0 == tempAssUseState[2])&&(0 == tempAssUseState[3])&&(0 == tempAssUseState[4]))//太敏均无效
|
||
{
|
||
pAss->Ass_SunVecVld = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
|
||
if(0 == iMax)
|
||
{
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSA[0][0]), 9 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(1 == iMax)
|
||
{
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSB[0][0]), 9 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(2 == iMax)
|
||
{
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSC[0][0]), 9 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(3 == iMax)
|
||
{
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSD[0][0]), 9 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(4 == iMax)
|
||
{
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[iMax][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSE[0][0]), 9 * sizeof(TYPE_CAL));
|
||
}
|
||
|
||
if ((iMax>=0)&&(iMax<4))
|
||
{
|
||
tmpH = 1.95;
|
||
}
|
||
else
|
||
{
|
||
tmpH = 4.0;
|
||
}
|
||
|
||
tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
|
||
if(tmpXs!=0)
|
||
{
|
||
tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
|
||
}
|
||
|
||
tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
|
||
if(tmpYs!=0)
|
||
{
|
||
tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
|
||
}
|
||
|
||
//根据坐标值求出太阳矢量方位角θ、虚余仰角r
|
||
tmpTheta = POSE_ATAN2F(tmpY, tmpX);
|
||
tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);
|
||
tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
|
||
tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
|
||
tmpSm[2] = POSE_COSF(tmpGamma);
|
||
MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecb, 3, 3, 1);
|
||
pAss->Ass_SunVecVld= POSE_OK;
|
||
|
||
}
|
||
|
||
|
||
//计算帆板模拟太敏方位角θ、虚余仰角r
|
||
if ((POSE_OK == pAss->AssIn_AttVld[5])&& (POSE_OK == pImpt->AssUseState[5]))
|
||
{
|
||
tmpH = 4.0;
|
||
memcpy(tmpU, &(pAss->AssIn_sensor[5][0]), 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs, &(tmpConst->AttCmdFlashPara.M_aSSF[0][0]), 9 * sizeof(TYPE_CAL));
|
||
tmpXs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpH;
|
||
if(tmpXs!=0)
|
||
{
|
||
tmpX=tmpa*(tmpU[0]-tmpU[1]-tmpU[2]+tmpU[3])/tmpXs;
|
||
}
|
||
|
||
tmpYs=2.0*(tmpU[0]+tmpU[1]+tmpU[2]+tmpU[3])+0.2*fabs(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpH;
|
||
if(tmpYs!=0)
|
||
{
|
||
tmpY=tmpa*(tmpU[0]+tmpU[1]-tmpU[2]-tmpU[3])/tmpYs;
|
||
}
|
||
//根据坐标值求出太阳矢量方位角θ、虚余仰角r
|
||
tmpTheta = POSE_ATAN2F(tmpY, tmpX);
|
||
tmpGamma = POSE_ATANF((POSE_SQRTF(tmpX*tmpX+tmpY*tmpY))/tmpH);
|
||
pAss->Ass_SunATT[0] = tmpTheta;
|
||
pAss->Ass_SunATT[1] = tmpGamma;
|
||
tmpSm[0] = -POSE_COSF(tmpTheta)*POSE_SINF(tmpGamma);
|
||
tmpSm[1] = -POSE_SINF(tmpTheta)*POSE_SINF(tmpGamma);
|
||
tmpSm[2] = POSE_COSF(tmpGamma);
|
||
MatrixProductHL(tmpRbs, tmpSm, pAss->Ass_SunVecbF, 3, 3, 1);
|
||
pAss->Ass_SunOK = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pAss->Ass_SunOK = POSE_NO;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：获取磁强计数据
|
||
***********************************************/
|
||
void Mag_DataGet(void)
|
||
{
|
||
TYPE_CAL tmpMag_Bb_Limit = 55000.0; //sjd20231123
|
||
|
||
INT16 tmpDtInt = 0;
|
||
INT16 *pMag16 = NULL;
|
||
UINT32 i = 0;
|
||
UINT32 j = 0;
|
||
TYPE_CAL tmpVm[MAG_NUM][3] = {{0,0,0},{0,0,0}};
|
||
UINT32 ID = 2;
|
||
ID = SV3_CQJA_X -1 ;
|
||
|
||
sDevInfo * tmpDevInfo = NULL;
|
||
sAttPriData *tmpAtt= NULL;
|
||
AttCtrlConst_t *tmpConst= NULL;
|
||
MagPara_t *pMag= NULL;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
pMag = &tmpAtt->sPerPara.MagPara;
|
||
|
||
//获取AD采集数据
|
||
tmpDevInfo = GETDEVINFO(DEV_NO_AD) ;
|
||
pMag16 = (INT16 *)(&tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]);
|
||
|
||
//模拟磁强计的补偿矩阵计算
|
||
if (INFO_SIG_CHANGE == tmpDevInfo -> cDevYCBuffer[0])
|
||
{
|
||
for (i = 0; i < MAG_NUM; ++i)
|
||
{
|
||
//计算Vm
|
||
for (j = 0; j< 3; j++)
|
||
{
|
||
tmpDtInt = *(pMag16 + ID +j + 3*i);
|
||
tmpVm[i][j] = tmpConst->AttCmdFlashPara.MAG_LK[i][j] * (((float)(tmpDtInt))*10/4095 - 5 ) + tmpConst->AttCmdFlashPara.MAG_LB[i][j]; //MAG_LK 24000
|
||
}
|
||
}
|
||
if ((POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)|| (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit))
|
||
{
|
||
if (POSE_ABSF(tmpVm[0][0]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 1, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
if (POSE_ABSF(tmpVm[0][1]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 2, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
if (POSE_ABSF(tmpVm[0][2]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 3, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
pMag->MagIn_Valid[0] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pMag->Mag_Bc_MeaAB[0], tmpVm[0], 3*sizeof(TYPE_CAL));
|
||
pMag->MagIn_Valid[0] = POSE_OK;
|
||
}
|
||
|
||
if ((POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit) || (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)||(POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit))
|
||
{
|
||
if (POSE_ABSF(tmpVm[1][0]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 4, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
if (POSE_ABSF(tmpVm[1][1]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 5, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
if (POSE_ABSF(tmpVm[1][2]) >tmpMag_Bb_Limit)
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 6, 1 ); //磁强计A测量值超阈值
|
||
}
|
||
|
||
pMag->MagIn_Valid[1] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pMag->Mag_Bc_MeaAB[1], tmpVm[1], 3*sizeof(TYPE_CAL));
|
||
pMag->MagIn_Valid[1] = POSE_OK;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pMag->MagIn_DataSta = ZERO_TO_ONE( pMag->MagIn_DataSta, 0, 1 ); //数据不更新
|
||
pMag->MagIn_Valid[0] = POSE_NO;
|
||
pMag->MagIn_Valid[1] = POSE_NO;
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：磁强计数据处理
|
||
***********************************************/
|
||
void Mag_DataPre(void)
|
||
{
|
||
UINT8 i = 0;
|
||
TYPE_CAL tmKm[3][3] = {{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0}};
|
||
TYPE_CAL tmpMag_Bc_MeaAB[3]= {0.0,0.0,0.0};
|
||
UINT8 tmpComTHUse[3] = {0,0,0};
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
MagPara_t *pMag = NULL;
|
||
AttIMPTPara_t *pIMU = 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;
|
||
|
||
//测量系磁强计A磁矢量
|
||
if (POSE_OK == pMag->MagIn_Valid[0])
|
||
{
|
||
memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[0], 9 * sizeof(TYPE_CAL));
|
||
MatrixPlusVectorN( tmKm[0], pMag->Mag_Bc_MeaAB[0], pMag->Mag_Bb_MeaAB[0], 3, 3 );
|
||
}
|
||
if (POSE_OK ==pMag->MagIn_Valid[1])
|
||
{
|
||
memcpy( tmKm[0], tmpConst->AttCmdFlashPara.M_MAG[1], 9 * sizeof(TYPE_CAL));
|
||
MatrixPlusVectorN( tmKm[0], pMag->Mag_Bc_MeaAB[1], pMag->Mag_Bb_MeaAB[1], 3, 3 );
|
||
}
|
||
|
||
if ((POSE_NO == pIMU->MagUseState[0])&&(POSE_NO == pIMU->MagUseState[1]))
|
||
{
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
if (0x11 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
|
||
{
|
||
tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[0][0][i];
|
||
tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[0][1][i];
|
||
tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[0][2][i];
|
||
tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[0][i];
|
||
tmpComTHUse[i] = 0x55;
|
||
}
|
||
else if (0x22 == tmpConst->AttCmdFlashPara.Mag_ComRESW[i])
|
||
{
|
||
tmKm[0][i] = tmpConst->AttCmdFlashPara.M_MAG[1][0][i];
|
||
tmKm[1][i] = tmpConst->AttCmdFlashPara.M_MAG[1][1][i];
|
||
tmKm[2][i] = tmpConst->AttCmdFlashPara.M_MAG[1][2][i];
|
||
tmpMag_Bc_MeaAB[i] =pMag->Mag_Bc_MeaAB[1][i];
|
||
tmpComTHUse[i] = 0x55;
|
||
}
|
||
|
||
}
|
||
if ((0x55 == tmpComTHUse[0])&&(0x55 == tmpComTHUse[1])&&(0x55 == tmpComTHUse[2]))
|
||
{
|
||
MatrixPlusVectorN( tmKm[0], tmpMag_Bc_MeaAB, pMag->Mag_Bb_MeaZH, 3, 3 );
|
||
pMag->MagIn_Valid[2] = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pMag->MagIn_Valid[2] = POSE_NO;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：获取飞轮数据
|
||
***********************************************/
|
||
void Whl_DataGet(void)
|
||
{
|
||
|
||
UINT8 sum= 0;
|
||
UINT32 i = 0;
|
||
UINT32 j = 0;
|
||
UINT16 tmpU16=0;
|
||
INT16 tmpI16=0;
|
||
UINT8 tmpWhlHEAD = 0;
|
||
UINT8 tmpWhlHEADP = 0;
|
||
UINT8 tmpWhlHEADID[4]={0};
|
||
UINT16 tmpWhlEnd = 0;
|
||
sDevInfo * tmpDevInfo = NULL;
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
WhlPara_t *PWhl = NULL;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{
|
||
return;
|
||
}
|
||
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
|
||
tmpWhlHEADID[0] = tmpConst->Whl_ID[0];
|
||
tmpWhlHEADID[1] = tmpConst->Whl_ID[1];
|
||
tmpWhlHEADID[2] = tmpConst->Whl_ID[2];
|
||
tmpWhlHEADID[3] = tmpConst->Whl_ID[3];
|
||
|
||
//记录数据
|
||
memcpy(PWhl->Whl_RatePre,PWhl->Whl_Rate,4*sizeof(TYPE_CAL));
|
||
memcpy(PWhl->Whl_TimePRE,PWhl->Whl_Time,4*sizeof(UINT32));
|
||
|
||
for (i = 0; i < (WHEEL_NUM); ++i)
|
||
{
|
||
tmpDevInfo = GETDEVINFO(i+ DEV_NO_UART8) ;
|
||
tmpWhlHEAD = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF];
|
||
tmpWhlHEADP = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
|
||
tmpWhlEnd = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+32]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+33];
|
||
|
||
/*飞轮数据更新*/
|
||
if (INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
|
||
{
|
||
sum = 0;
|
||
for(j = 2; j < WHL_DATA_LEN -1; j++)
|
||
{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +j];}
|
||
sum = 0xFF - sum;
|
||
|
||
if((WHL_DATA_END != tmpWhlEnd)||(WHL_CMD_CODE_REQ!=tmpWhlHEADP)||(tmpWhlHEADID[i]!=tmpWhlHEAD))
|
||
{
|
||
PWhl->WhlIn_Valid[i] = POSE_NO;
|
||
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 1, 1 ); /* errNo=0,飞轮包头|包长错误 */
|
||
}
|
||
else
|
||
{
|
||
|
||
if (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+ WHL_DATA_LEN -1]!=sum)
|
||
{
|
||
PWhl->WhlIn_Valid[i] = POSE_NO;
|
||
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 2, 1 ); /* errNo=0,飞轮包头|包长错误 */
|
||
}
|
||
else
|
||
{
|
||
PWhl->Whl_Time[i]=DoGetInt32From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +2]));
|
||
tmpI16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+22]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+23];
|
||
tmpU16 = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24]<<8|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+25];
|
||
PWhl->Whl_Rate[i] = DoGetFloatFrom8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+10]))*RADS2RPM;
|
||
PWhl->Whl_Current[i] = tmpI16;
|
||
PWhl->Whl_Tmper[i] = 0.4863 * (TYPE_CAL)(tmpU16) - 273.15 ;
|
||
PWhl->WhlIn_Valid[i] = POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PWhl->WhlIn_Valid[i] = POSE_NO;
|
||
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 0, 1 ); /* errNo=0,飞轮转速不更新 */
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：飞轮数据处理
|
||
***********************************************/
|
||
void Whl_DataPre(void)
|
||
{
|
||
UINT8 i = 0;
|
||
sAttPriData *tmpAtt = NULL;
|
||
WhlPara_t *PWhl = NULL;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{
|
||
return;
|
||
}
|
||
|
||
PWhl = &tmpAtt->sPerPara.WhlPara;
|
||
|
||
for (i = 0; i < WHEEL_NUM; ++i)
|
||
{
|
||
if (POSE_OK == PWhl->WhlIn_Valid[i])
|
||
{
|
||
if (POSE_ABSF( PWhl->Whl_Rate[i]- PWhl->Whl_RatePre[i] ) > 100*RADS2RPM) //200t230
|
||
{
|
||
PWhl->WhlIn_DataSta[i] = ZERO_TO_ONE( PWhl->WhlIn_DataSta[i], 4, 1 );
|
||
PWhl->WhlIn_Valid[i] = POSE_NO;
|
||
}
|
||
else
|
||
{
|
||
PWhl->WhlIn_Valid[i] = POSE_OK;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：霍尔电推数据处理
|
||
***********************************************/
|
||
void PPU_DataPre(void)
|
||
{
|
||
UINT16 tmpPPUHEAD = 0;
|
||
UINT8 tmpPPSUM = 0;
|
||
UINT8 sum = 0;
|
||
UINT8 i = 0;
|
||
UINT32 TempSatTime[2] ={0,0};
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
sDevInfo * tmpDevInfo = NULL;
|
||
PPUPara_t *PPPU = NULL;
|
||
sAttOrbitCtlInfo_t *pOrb = NULL;
|
||
sAttModPara_t *pWMod= NULL;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) || (NULL==tmpConst))
|
||
{
|
||
return;
|
||
}
|
||
|
||
pWMod=&tmpAtt->sModePara;
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
pOrb=&tmpAtt->sOrbitInp;
|
||
|
||
tmpDevInfo = GETDEVINFO(DEV_NO_CAN0) ;
|
||
tmpPPUHEAD = (tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF]<<8)|tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+1];
|
||
tmpPPSUM = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF + PPU_CMD_LEN +1];
|
||
|
||
/*获取星上时*/
|
||
GetTime(TempSatTime);
|
||
|
||
/*PPU单机数据更新判断*/
|
||
if(INFO_SIG_CHANGE == tmpDevInfo->cDevYCBuffer[0])
|
||
{
|
||
for(i = 0; i < PPU_CMD_LEN+1 ; i++)
|
||
{sum += tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF +i];}
|
||
/*PPU数据有效判断*/
|
||
if((PPU_CMD_LEN == tmpPPUHEAD )&&(sum == tmpPPSUM))
|
||
{
|
||
/*PPU数据有效更新上一拍、当前拍的请求计数（单机包中返回请求计数）*/
|
||
PPPU->PPUIn_ReCntPre = PPPU->PPUIn_ReCnt;
|
||
PPPU->PPUIn_ReCnt= tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+3];
|
||
|
||
/*PPU数据上一拍、当前拍数据 不更新，则PPU数据无效 ？？？？*/
|
||
if (PPPU->PPUIn_ReCntPre == PPPU->PPUIn_ReCnt)
|
||
{
|
||
PPPU->PPUIn_Valid = POSE_NO;
|
||
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 2, 1 );
|
||
}
|
||
else
|
||
{
|
||
/*PPU数据上一拍、当前拍数据更新，则PPU数据有效*/
|
||
PPPU->PPUIn_AVoltage=tmpConst->AttCmdFlashPara.PPUPVK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+15])) + tmpConst->AttCmdFlashPara.PPUPVB; //推进阳极电源电压
|
||
PPPU->PPUIn_ACurrent=tmpConst->AttCmdFlashPara.PPUPAK *DoGetInt16From8(&(tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+17])) + tmpConst->AttCmdFlashPara.PPUPAB; //推进阳极电源电流
|
||
|
||
/*跟新上一拍、当前拍的点火标志（单机数据包中的点火标志）*/
|
||
PPPU->PPUIn_FIREOKPre = PPPU->PPUIn_FIREOK;
|
||
PPPU->PPUIn_FIREOK = tmpDevInfo->cDevYCBuffer[INFO_ADDR_OFF+24]; //点火成功标志
|
||
PPPU->PPUIn_Valid = POSE_OK;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
/*PPU数据校验不通过，设置PPU数据无效*/
|
||
PPPU->PPUIn_Valid = POSE_NO;
|
||
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 1, 1 );
|
||
///*PPU数据校验不通过，设置上一拍、当前拍点火成功标志 为 未点火*/
|
||
//PPPU->PPUIn_FIREOKPre = 0x00;
|
||
//
|
||
///*因PPU单机数据校验不通过，如果上一拍的点火成功，当前拍点火标志清零认为点火结束？？？，记录点火结束时间*/
|
||
//if(0x01 == PPPU->PPUIn_FIREOK)
|
||
//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
|
||
//
|
||
//PPPU->PPUIn_FIREOK = 0x00;
|
||
//
|
||
//return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
/*PPU单机数据不更新，设置PPU数据无效*/
|
||
PPPU->PPUIn_Valid = POSE_NO;
|
||
PPPU->PPUIn_DataSta = ZERO_TO_ONE( PPPU->PPUIn_DataSta, 0, 1 );
|
||
///*PPU数据校验不通过，设置上一拍、当前拍点火成功标志 为 未点火*/
|
||
//PPPU->PPUIn_FIREOKPre = 0x00;
|
||
//
|
||
///*因PPU单机数据未更新，如果上一拍的点火成功，当前拍点火标志清零认为点火结束？？？，记录点火结束时间*/
|
||
//if(0x01 == PPPU->PPUIn_FIREOK)
|
||
//PPPU->PPUT_FIR_EndLast = TempSatTime[0];
|
||
//
|
||
//PPPU->PPUIn_FIREOK = 0x00;
|
||
//
|
||
//return;
|
||
}
|
||
|
||
/*PPU点火成功 总次数 不清除 一直记录*/
|
||
if(0x01 == PPPU->PPUIn_FIREOK)
|
||
{
|
||
if (PPPU->PPUT_FIR_Sum< 0xFFFFFFFE)
|
||
{
|
||
PPPU->PPUT_FIR_Sum++ ;
|
||
}
|
||
else
|
||
{
|
||
PPPU->PPUT_FIR_Sum= 0;
|
||
}
|
||
}
|
||
|
||
/*PPU从 点火 到 未点火，数据记录*/
|
||
if ((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
|
||
{
|
||
if(PPPU->PPUT_FIR_Cnt>0)
|
||
PPPU->PPUT_FIR_Cnt--;
|
||
}
|
||
|
||
/*PPU从未点火 到 点火，数据记录*/
|
||
if ((0x01 != PPPU->PPUIn_FIREOKPre)&&(0x01 == PPPU->PPUIn_FIREOK))
|
||
{
|
||
//if(PPPU->PPUT_FIR_Cnt>0)
|
||
//PPPU->PPUT_FIR_Cnt--;
|
||
|
||
if(PPPU->PPUT_FIR_SumCnt < 60000)
|
||
{
|
||
PPPU->PPUT_FIR_SumCnt++;
|
||
}
|
||
else
|
||
{
|
||
PPPU->PPUT_FIR_SumCnt = 0;
|
||
}
|
||
|
||
if((pOrb->CurOrbCtrlPcak_ID >= 0) &&(pOrb->CurOrbCtrlPcak_ID< 50))
|
||
{
|
||
PPPU->PPUT_FIR_Start[pOrb->CurOrbCtrlPcak_ID] = TempSatTime[0];
|
||
}
|
||
}/*PPU从点火 到 未点火，数据记录*/
|
||
else if((0x01 == PPPU->PPUIn_FIREOKPre)&&(0x01 != PPPU->PPUIn_FIREOK))
|
||
{
|
||
tmpConst->AttCmdFlashPara.PPUT_FIR_EndLast = TempSatTime[0] ;
|
||
pWMod->ATTFlashSwitch = POSE_OK;
|
||
|
||
if(((pOrb->CurOrbCtrlPcak_ID >= 1) &&(pOrb->CurOrbCtrlPcak_ID <= 50)))
|
||
{
|
||
PPPU->PPUT_FIR_End[pOrb->CurOrbCtrlPcak_ID - 1] = TempSatTime[0];
|
||
}
|
||
else if(0==pOrb->CurOrbCtrlPcak_ID)
|
||
{
|
||
if((PPPU->PPUT_FIR_CntS >= 1) &&(PPPU->PPUT_FIR_CntS <= 50))
|
||
{
|
||
PPPU->PPUT_FIR_End[PPPU->PPUT_FIR_CntS - 1] = TempSatTime[0];
|
||
PPPU->PPUT_FIR_CntS =0;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{;} /*不做处理*/
|
||
}
|
||
|
||
/***********************************************
|
||
说明：星敏互校验
|
||
***********************************************/
|
||
void SSABC_CC(void)
|
||
{
|
||
UINT8 i=0;
|
||
UINT8 tmpCheckCnt=0;
|
||
TYPE_CAL tmpLb[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
|
||
TYPE_CAL tmpLi[3][3] ={{0.0,0.0,0.0}, {0.0,0.0,0.0}, {0.0,0.0,0.0}};
|
||
TYPE_CAL tmpRbs[3][9]={
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,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 tmpAab_b = 0.0;
|
||
TYPE_CAL tmpAbc_b = 0.0;
|
||
TYPE_CAL tmpAac_b = 0.0;
|
||
TYPE_CAL tmpAab_i = 0.0;
|
||
TYPE_CAL tmpAbc_i= 0.0;
|
||
TYPE_CAL tmpAac_i= 0.0;
|
||
TYPE_CAL tmpRsi[3][9]={
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,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 tmpRis[3][9]={
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
|
||
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
|
||
};
|
||
UINT8 Valid[3]={0};
|
||
UINT8 ssA_CCFlg=0;
|
||
UINT8 ssB_CCFlg=0;
|
||
UINT8 ssC_CCFlg=0;
|
||
sAttPriData *tmpAtt = NULL;
|
||
AttCtrlConst_t *tmpConst = NULL;
|
||
SsPara_t *ssA = NULL;
|
||
SsPara_t *ssB = NULL;
|
||
SsPara_t *ssC = NULL;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if((NULL==tmpAtt) ||( NULL==tmpConst))
|
||
{
|
||
return;
|
||
}
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
|
||
Valid[0]=ssA->ssIn_AttVld;
|
||
Valid[1]=ssB->ssIn_AttVld;
|
||
Valid[2]=ssC->ssIn_AttVld;
|
||
|
||
memcpy(tmpRbs[0], &(tmpConst->AttCmdFlashPara.M_SSA[0][0]), 9 * sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs[1], &(tmpConst->AttCmdFlashPara.M_SSB[0][0]), 9 * sizeof(TYPE_CAL));
|
||
memcpy(tmpRbs[2], &(tmpConst->AttCmdFlashPara.M_SSC[0][0]), 9 * sizeof(TYPE_CAL));
|
||
|
||
//计算星敏本体系光轴矢量
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpLb[i][0]=tmpRbs[i][2];
|
||
tmpLb[i][1]=tmpRbs[i][5];
|
||
tmpLb[i][2]=tmpRbs[i][8];
|
||
}
|
||
//计算星敏本体系光轴矢量夹角
|
||
CalVecArc(tmpLb[0], tmpLb[1], &tmpAab_b);
|
||
CalVecArc(tmpLb[0], tmpLb[2], &tmpAac_b);
|
||
CalVecArc(tmpLb[1], tmpLb[2], &tmpAbc_b);
|
||
|
||
//计算惯性系光轴矢量
|
||
Q2Att(ssA->ssIn_Q, tmpRsi[0]);
|
||
Q2Att(ssB->ssIn_Q, tmpRsi[1]);
|
||
Q2Att(ssC->ssIn_Q, tmpRsi[2]);
|
||
for(i=0;i<3;i++)
|
||
{
|
||
MatrixTransposeHL(tmpRsi[i], tmpRis[i],3, 3);
|
||
tmpLi[i][0]=tmpRis[i][2];
|
||
tmpLi[i][1]=tmpRis[i][5];
|
||
tmpLi[i][2]=tmpRis[i][8];
|
||
}
|
||
//计算惯性系光轴矢量夹角
|
||
CalVecArc(tmpLi[0], tmpLi[1], &tmpAab_i);
|
||
CalVecArc(tmpLi[0], tmpLi[2], &tmpAac_i);
|
||
CalVecArc(tmpLi[1], tmpLi[2], &tmpAbc_i);
|
||
|
||
if (0x55 == Valid[0])
|
||
{
|
||
ssA_CCFlg=CC_UNCERTAIN;
|
||
}
|
||
else
|
||
{
|
||
ssA_CCFlg=NO_CC;
|
||
}
|
||
|
||
if (0x55 == Valid[1])
|
||
{
|
||
ssB_CCFlg=CC_UNCERTAIN;
|
||
}
|
||
else
|
||
{
|
||
ssB_CCFlg=NO_CC;
|
||
}
|
||
if (0x55 == Valid[2])
|
||
{
|
||
ssC_CCFlg=CC_UNCERTAIN;
|
||
}
|
||
else
|
||
{
|
||
ssC_CCFlg=NO_CC;
|
||
}
|
||
|
||
if((0x55 == Valid[0])&&(0x55 == Valid[1]))
|
||
{
|
||
if(POSE_ABSF(tmpAab_i-tmpAab_b)<(tmpConst->SS_SS_CThr))
|
||
{
|
||
ssA_CCFlg=CC_CORRECT;
|
||
ssB_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
}
|
||
|
||
if((0x55 == Valid[0])&&(0x55 == Valid[2]))
|
||
{
|
||
if(POSE_ABSF(tmpAac_i-tmpAac_b)<(tmpConst->SS_SS_CThr))
|
||
{
|
||
ssA_CCFlg=CC_CORRECT;
|
||
ssC_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
}
|
||
|
||
if((0x55 == Valid[1])&&(0x55 == Valid[2]))
|
||
{
|
||
if(POSE_ABSF(tmpAbc_i-tmpAbc_b)<(tmpConst->SS_SS_CThr))
|
||
{
|
||
ssB_CCFlg=CC_CORRECT;
|
||
ssC_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
}
|
||
|
||
if(tmpCheckCnt>0)//存在校验正确的星敏
|
||
{
|
||
if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
|
||
if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
|
||
if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
|
||
}
|
||
ssA->ssCrCheckFlg=ssA_CCFlg;
|
||
ssB->ssCrCheckFlg=ssB_CCFlg;
|
||
ssC->ssCrCheckFlg=ssC_CCFlg;
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：星敏+太敏互校验
|
||
***********************************************/
|
||
void StarSun_CC(void)
|
||
{
|
||
UINT8 i,tmpCheckCnt=0;
|
||
UINT8 ssA_CCFlg, ssB_CCFlg, ssC_CCFlg, Sun_CCFlg;
|
||
TYPE_CAL tmpRbi[3][9];
|
||
TYPE_CAL tmpSb[3][3];
|
||
TYPE_CAL tmpAam,tmpAbm,tmpAcm;
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
SsPara_t *ssA, *ssB, *ssC;
|
||
AssPara_t *pASS;
|
||
sAttOrbitGetPara_t *pOrbit = NULL;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
pASS=&tmpAtt->sPerPara.AssPara;
|
||
pOrbit = &tmpAtt->sOrbitPara;
|
||
|
||
//校验条件判断（这里应该不判断也可）
|
||
if((CC_CORRECT == ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT == ssC->ssCrCheckFlg))
|
||
return;
|
||
if(0x00 == pASS->Ass_SunVecVld)
|
||
return;
|
||
if((0x00 == ssA->ssIn_AttVld)&&(0x00 == ssB->ssIn_AttVld)&&(0x00 == ssC->ssIn_AttVld))
|
||
return;
|
||
if(POSE_NO == pOrbit->MJCLv)
|
||
{
|
||
return;
|
||
}
|
||
|
||
ssA_CCFlg=CC_UNCERTAIN;
|
||
ssB_CCFlg=CC_UNCERTAIN;
|
||
ssC_CCFlg=CC_UNCERTAIN;
|
||
Sun_CCFlg=CC_UNCERTAIN;
|
||
|
||
//计算本体系太阳矢量
|
||
Q2Att(ssA->ssExpose_Qbi, tmpRbi[0]);
|
||
Q2Att(ssB->ssExpose_Qbi, tmpRbi[1]);
|
||
Q2Att(ssC->ssExpose_Qbi, tmpRbi[2]);
|
||
for(i=0;i<3;i++)
|
||
MatrixProductHL(tmpRbi[i], tmpAtt->sEnvPara.SunVecI, tmpSb[i], 3, 3, 1);
|
||
|
||
//计算本体系下星敏太阳矢量和太敏太阳矢量夹角
|
||
CalVecArc(tmpSb[0], pASS->Ass_SunVecb, &tmpAam);
|
||
CalVecArc(tmpSb[1], pASS->Ass_SunVecb, &tmpAbm);
|
||
CalVecArc(tmpSb[2], pASS->Ass_SunVecb, &tmpAcm);
|
||
|
||
if((0x55 == ssA->ssIn_AttVld)&&(tmpAam<tmpConst->SS_ASS_CCThr))
|
||
{
|
||
ssA_CCFlg=CC_CORRECT;
|
||
Sun_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
|
||
if((0x55 == ssB->ssIn_AttVld)&&(tmpAbm<tmpConst->SS_ASS_CCThr))
|
||
{
|
||
ssB_CCFlg=CC_CORRECT;
|
||
Sun_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
|
||
if((0x55 == ssC->ssIn_AttVld)&&(tmpAcm<tmpConst->SS_ASS_CCThr))
|
||
{
|
||
ssC_CCFlg=CC_CORRECT;
|
||
Sun_CCFlg=CC_CORRECT;
|
||
tmpCheckCnt++;
|
||
}
|
||
|
||
if(tmpCheckCnt>0)
|
||
{
|
||
if(ssA_CCFlg==CC_UNCERTAIN) ssA_CCFlg=CC_ERROR;
|
||
if(ssB_CCFlg==CC_UNCERTAIN) ssB_CCFlg=CC_ERROR;
|
||
if(ssC_CCFlg==CC_UNCERTAIN) ssC_CCFlg=CC_ERROR;
|
||
}
|
||
else
|
||
Sun_CCFlg=CC_UNCERTAIN;
|
||
|
||
ssA->ssCrCheckFlg=ssA_CCFlg;
|
||
ssB->ssCrCheckFlg=ssB_CCFlg;
|
||
ssC->ssCrCheckFlg=ssC_CCFlg;
|
||
pASS->AssCrCheckFlg=Sun_CCFlg;
|
||
}
|
||
|
||
/***********************************************
|
||
说明：星敏校太敏
|
||
***********************************************/
|
||
void StarCheckSun(void)
|
||
{
|
||
TYPE_CAL tmpQbi[4];
|
||
TYPE_CAL tmpRbi[9];
|
||
TYPE_CAL tmpSb[3];
|
||
TYPE_CAL tmpAsm;
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
SsPara_t *ssA, *ssB, *ssC;
|
||
AssPara_t *pASS;
|
||
|
||
sAttOrbitGetPara_t *pOrbit = NULL;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
pASS=&tmpAtt->sPerPara.AssPara;
|
||
pOrbit = &tmpAtt->sOrbitPara;
|
||
|
||
if((0x00 == pASS->Ass_SunVecVld)||(POSE_NO == pOrbit->MJCLv))
|
||
{
|
||
pASS->AssCrCheckFlg=NO_CC;
|
||
return;
|
||
}
|
||
|
||
//利用星敏计算本体系太阳矢量
|
||
if(CC_CORRECT == ssA->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(CC_CORRECT == ssB->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(CC_CORRECT == ssC->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
|
||
Q2Att(tmpQbi, tmpRbi);
|
||
MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, tmpSb, 3, 3, 1);
|
||
//判断夹角
|
||
CalVecArc(tmpSb, pASS->Ass_SunVecb, &tmpAsm);
|
||
if(tmpAsm<tmpConst->SS_ASS_CThr)
|
||
pASS->AssCrCheckFlg=CC_CORRECT;
|
||
else
|
||
pASS->AssCrCheckFlg=CC_ERROR;
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：星敏校陀螺
|
||
因为进入的条件是必有正常星敏，那么该星敏一定校验为1且角速度有效，该子函数中不再进行判断
|
||
***********************************************/
|
||
void StarCheckGyro(void)
|
||
{
|
||
UINT8 i, j;
|
||
TYPE_CAL deltaA[GYRO_NUM][3];//三轴角速度偏差
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
SsPara_t *ssA, *ssB, *ssC;
|
||
GyroPara_t *pGyro[3];
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
|
||
pGyro[0]=&tmpAtt->sPerPara.GyroPara[0];
|
||
pGyro[1]=&tmpAtt->sPerPara.GyroPara[1];
|
||
pGyro[2]=&tmpAtt->sPerPara.GyroPara[2];
|
||
|
||
//校陀螺
|
||
if(CC_CORRECT ==ssA->ssCrCheckFlg)
|
||
{
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
for (j=0;j<3;j++)
|
||
{
|
||
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssA->ssExpose_Wbi[j]);
|
||
}
|
||
}
|
||
}
|
||
else if(CC_CORRECT ==ssB->ssCrCheckFlg)
|
||
{
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
for (j=0;j<3;j++)
|
||
{
|
||
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssB->ssExpose_Wbi[j]);
|
||
}
|
||
}
|
||
}
|
||
else if(CC_CORRECT ==ssC->ssCrCheckFlg)
|
||
{
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
for (j=0;j<3;j++)
|
||
{
|
||
deltaA[i][j]=POSE_ABSF(pGyro[i]->Gyro_Wi[j]-ssC->ssExpose_Wbi[j]);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
|
||
for(i=0;i<GYRO_NUM; i++)
|
||
{
|
||
if (POSE_NO == pGyro[i]->GyroIn_AttVld)
|
||
pGyro[i]->GyroCrCheckFlg=NO_CC;
|
||
else if((deltaA[i][0]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][1]<tmpConst->SS_GYRO_CThr)&&(deltaA[i][2]<tmpConst->SS_GYRO_CThr))
|
||
pGyro[i]->GyroCrCheckFlg=CC_CORRECT;
|
||
else
|
||
pGyro[i]->GyroCrCheckFlg=CC_ERROR;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：星敏校磁强计
|
||
***********************************************/
|
||
void StarCheckMag(void)
|
||
{
|
||
UINT8 i;
|
||
TYPE_CAL deltaA[MAG_NUM];
|
||
TYPE_CAL tmpQbi[4];
|
||
TYPE_CAL tmpRbi[9];
|
||
TYPE_CAL tmpSb[3];
|
||
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
SsPara_t *ssA, *ssB, *ssC;
|
||
MagPara_t *pMag;
|
||
sAttEnvPara_t *pEnv;
|
||
sAttOrbitGetPara_t *pObtIn;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
|
||
pObtIn = &tmpAtt->sOrbitPara;
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
pEnv=&tmpAtt->sEnvPara;
|
||
|
||
if(0x00 == pObtIn->OrbitVld)
|
||
{
|
||
pMag->MagCrCheckFlg[0]=NO_CC;
|
||
pMag->MagCrCheckFlg[1]=NO_CC;
|
||
return;
|
||
}
|
||
|
||
//利用星敏计算本体系太阳矢量
|
||
if(CC_CORRECT == ssA->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssA->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(CC_CORRECT == ssB->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssB->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else if(CC_CORRECT == ssC->ssCrCheckFlg)
|
||
{
|
||
memcpy(tmpQbi, ssC->ssExpose_Qbi, 4 * sizeof(TYPE_CAL));
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
|
||
Q2Att(tmpQbi, tmpRbi);
|
||
MatrixProductHL(tmpRbi, pEnv->fvBi, tmpSb, 3, 3, 1);
|
||
|
||
//校磁强计
|
||
for(i=0;i<MAG_NUM;i++)
|
||
{
|
||
if(POSE_OK == pMag->MagIn_Valid[i])
|
||
{
|
||
CalVecArc(tmpSb, pMag->Mag_Bb_MeaAB[i], &deltaA[i]);
|
||
|
||
if(deltaA[i]<tmpConst->AttCmdFlashPara.SS_MAG_CThr)
|
||
{
|
||
pMag->MagCrCheckFlg[i]=CC_CORRECT;
|
||
}
|
||
else
|
||
{
|
||
pMag->MagCrCheckFlg[i]=CC_ERROR;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pMag->MagCrCheckFlg[i]=NO_CC;
|
||
}
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：陀螺互校验
|
||
***********************************************/
|
||
void Gyro_CC(void)
|
||
{
|
||
UINT8 i=0;
|
||
UINT8 Valid[3];
|
||
UINT8 tmpCnt = 0;
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
GyroPara_t *pGyro[3];
|
||
|
||
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];
|
||
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
pGyro[i]->GyroCrCheckFlg=NO_CC;
|
||
|
||
Valid[i]=pGyro[i]->GyroIn_AttVld;
|
||
if (0x55 == Valid[i])
|
||
{
|
||
pGyro[i]->GyroCrCheckFlg=CC_UNCERTAIN;
|
||
}
|
||
}
|
||
|
||
if((0x55 == Valid[0])&&(0x55 ==Valid[1]))
|
||
{
|
||
if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[1]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[1]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[1]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
|
||
)
|
||
{
|
||
pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
|
||
pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
|
||
tmpCnt++;
|
||
}
|
||
else
|
||
{
|
||
;/*不做处理*/
|
||
}
|
||
}
|
||
|
||
if((0x55 == Valid[0])&&(0x55 == Valid[2]))
|
||
{
|
||
if((POSE_ABSF(pGyro[0]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[0]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
|
||
)
|
||
{
|
||
pGyro[0]->GyroCrCheckFlg=CC_CORRECT;
|
||
pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
|
||
tmpCnt++;
|
||
}
|
||
else
|
||
{
|
||
;/*不做处理*/
|
||
}
|
||
}
|
||
|
||
if((0x55 == Valid[1])&&(0x55 == Valid[2]))
|
||
{
|
||
if((POSE_ABSF(pGyro[1]->Gyro_Wi[0]-pGyro[2]->Gyro_Wi[0])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[1]->Gyro_Wi[1]-pGyro[2]->Gyro_Wi[1])<tmpConst->GYRO_GYRO_CThr)
|
||
&&(POSE_ABSF(pGyro[1]->Gyro_Wi[2]-pGyro[2]->Gyro_Wi[2])<tmpConst->GYRO_GYRO_CThr)
|
||
)
|
||
{
|
||
pGyro[1]->GyroCrCheckFlg=CC_CORRECT;
|
||
pGyro[2]->GyroCrCheckFlg=CC_CORRECT;
|
||
tmpCnt++;
|
||
}
|
||
else
|
||
{
|
||
;/*不做处理*/
|
||
}
|
||
}
|
||
|
||
for(i=0; i<3; i++)
|
||
{
|
||
if((NO_CC == pGyro[i]->GyroCrCheckFlg)
|
||
||(CC_CORRECT == pGyro[i]->GyroCrCheckFlg)
|
||
)
|
||
continue;
|
||
|
||
if(tmpCnt > 0)
|
||
pGyro[i]->GyroCrCheckFlg = CC_ERROR;
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：磁强计互校验
|
||
***********************************************/
|
||
void Mag_CC(void)
|
||
{
|
||
UINT8 i;
|
||
TYPE_CAL deltaA[3];
|
||
sAttPriData *tmpAtt;
|
||
AttCtrlConst_t *tmpConst;
|
||
MagPara_t *pMag;
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL==tmpAtt || NULL==tmpConst)
|
||
return;
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
|
||
if((POSE_OK == pMag->MagIn_Valid[0])&&(POSE_OK ==pMag->MagIn_Valid[1]))
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
deltaA[i]=POSE_ABSF(pMag->Mag_Bb_MeaAB[0][i]-pMag->Mag_Bb_MeaAB[1][i]);
|
||
}
|
||
if((deltaA[0]<tmpConst->MAG_MAG_CThr)&&(deltaA[1]<tmpConst->MAG_MAG_CThr)&&(deltaA[2]<tmpConst->MAG_MAG_CThr))
|
||
{
|
||
pMag->MagCrCheckFlg[0]=CC_CORRECT;
|
||
pMag->MagCrCheckFlg[1]=CC_CORRECT;
|
||
}
|
||
else
|
||
{
|
||
pMag->MagCrCheckFlg[0]=CC_UNCERTAIN;
|
||
pMag->MagCrCheckFlg[1]=CC_UNCERTAIN;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
pMag->MagCrCheckFlg[0]=NO_CC;
|
||
pMag->MagCrCheckFlg[1]=NO_CC;
|
||
}
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：单机互校验
|
||
***********************************************/
|
||
void Att_CrossCheck(void)
|
||
{
|
||
UINT8 tmpValidSum;
|
||
sAttPriData *tmpAtt;
|
||
SsPara_t *ssA, *ssB, *ssC;
|
||
AssPara_t *pASS;
|
||
|
||
tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL ==tmpAtt)
|
||
return;
|
||
|
||
ssA=&tmpAtt->sPerPara.SsPara[0];
|
||
ssB=&tmpAtt->sPerPara.SsPara[1];
|
||
ssC=&tmpAtt->sPerPara.SsPara[2];
|
||
|
||
pASS=&tmpAtt->sPerPara.AssPara;
|
||
|
||
tmpValidSum=ssA->ssIn_AttVld+ssB->ssIn_AttVld+ssC->ssIn_AttVld;
|
||
|
||
if((0xAA ==tmpValidSum)||(0xFF ==tmpValidSum))//2或3个星敏有效
|
||
{
|
||
SSABC_CC();//2、3个星敏间互校验
|
||
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
|
||
{
|
||
StarCheckSun();//正常星敏校太敏
|
||
StarCheckGyro();//正常星敏校陀螺
|
||
StarCheckMag();//正常星敏校磁强计
|
||
}
|
||
else
|
||
{
|
||
if(0x55 == pASS->Ass_SunVecVld)//太敏有效
|
||
{
|
||
StarSun_CC();//星敏太敏互校验
|
||
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT ==ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
|
||
{
|
||
StarCheckGyro();//正常星敏校陀螺
|
||
StarCheckMag();//正常星敏校磁强计
|
||
}
|
||
else
|
||
{
|
||
Gyro_CC();
|
||
Mag_CC();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Gyro_CC();
|
||
Mag_CC();
|
||
}
|
||
}
|
||
}
|
||
else if(0x55 ==tmpValidSum)//单星敏有效
|
||
{
|
||
if(0x55 ==pASS->Ass_SunVecVld)//太敏有效
|
||
{
|
||
StarSun_CC();//星敏太敏互校验
|
||
if((CC_CORRECT ==ssA->ssCrCheckFlg)||(CC_CORRECT == ssB->ssCrCheckFlg)||(CC_CORRECT ==ssC->ssCrCheckFlg))//有校验正常星敏
|
||
{
|
||
StarCheckGyro();//正常星敏校陀螺
|
||
StarCheckMag();//正常星敏校磁强计
|
||
}
|
||
else
|
||
{
|
||
Gyro_CC();
|
||
Mag_CC();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Gyro_CC();
|
||
Mag_CC();
|
||
}
|
||
}
|
||
else if(0 ==tmpValidSum)//没有有效星敏
|
||
{
|
||
Gyro_CC();
|
||
Mag_CC();
|
||
}
|
||
}
|
||
|
||
/************************************************
|
||
说明：数据预处理模块每周期调用操作
|
||
输入：
|
||
输出：
|
||
************************************************/
|
||
void ZKDPAction(void)
|
||
{
|
||
|
||
AttCtrlConst_t *tmpConst;
|
||
tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
if(NULL == tmpConst)
|
||
return;
|
||
//初始化
|
||
ZKDPInit();
|
||
|
||
//星敏
|
||
Ss_DataGet();
|
||
Ss_DataPre();
|
||
|
||
//陀螺
|
||
Gyro_DataGet();
|
||
Gyro_DataPre();
|
||
|
||
//模拟太敏
|
||
Ass_DataGet();
|
||
Ass_DataPre();
|
||
|
||
//磁强计
|
||
Mag_DataGet();
|
||
Mag_DataPre();
|
||
|
||
//飞轮
|
||
Whl_DataGet();
|
||
Whl_DataPre();
|
||
|
||
#ifndef MINMODULE_TYPE
|
||
//电推
|
||
PPU_DataPre();
|
||
//互校验
|
||
if (tmpConst->Allow_Sensor_CThr == 0x55 )
|
||
{
|
||
Att_CrossCheck();
|
||
}
|
||
#endif
|
||
}
|
||
|