1250 lines
36 KiB
C
1250 lines
36 KiB
C
/*
|
||
* Created: 2022/11/4 11:02:40
|
||
* Author: wangzk zhengmengxing
|
||
*/
|
||
#include "..\PrjCommon\CommonDef.h"
|
||
#include "AttMath.h"
|
||
#include "AttCtrlMain.h"
|
||
//#include "../PrjTaskMng/TaskMng.h"
|
||
|
||
/************************************************
|
||
说明：定姿算法初始化，在最开始调用一次，进行常量的初始化动作
|
||
输入：
|
||
返回:
|
||
************************************************/
|
||
void ZKPGInit(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pDeter->AttDeter_Type =0;
|
||
#ifdef MINMODULE_TYPE
|
||
//对日太阳角
|
||
memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
|
||
pDeter->AngToSun_ValidFlg=0x55; //星敏本体系太阳矢量可用标志
|
||
pDeter->CalAngToSun_Source=0;
|
||
|
||
//闭环姿态角
|
||
memset(pDeter->CLPAttAng,0.0,3*sizeof(TYPE_CAL));
|
||
pDeter->CLPAttValid = POSE_OK;
|
||
#else
|
||
//对日太阳角
|
||
memset(pDeter->SunVecb,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AngToSun,0.0,3*sizeof(TYPE_CAL));
|
||
pDeter->AngToSun_ValidFlg=0x55; //星敏本体系太阳矢量可用标志
|
||
pDeter->CalAngToSun_Source=0;
|
||
|
||
//轨道系姿态
|
||
memset(pDeter->Qo,0.0,4*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttRateOrb,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttAngTar,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttAngOrb,0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->SunVeco,0.0,3*sizeof(TYPE_CAL));
|
||
pDeter->AttOrb_Vld = POSE_OK; //卫星轨道系姿态有效标志
|
||
|
||
//目标参考系姿态
|
||
memset(pDeter->AttAngTarRe,0,3*sizeof(TYPE_CAL));
|
||
//memset(pDeter->AttRateTarRe,0.00873,3*sizeof(TYPE_CAL));
|
||
pDeter->AttRateTarRe[0] = 0.00873f;
|
||
pDeter->AttRateTarRe[1] = 0.00873f;
|
||
pDeter->AttRateTarRe[2] = - 0.00873f;
|
||
pDeter->AttTarRe_Vld = POSE_OK;
|
||
|
||
//轨控参考系姿态
|
||
memset(pDeter->AttAngOrbCtrl,0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttRateOrbCtrl,0,3*sizeof(TYPE_CAL));
|
||
pDeter->AttOrbCtrl_Vld = POSE_OK;
|
||
|
||
//闭环姿态角
|
||
memset(pDeter->CLPAttAng,0,3*sizeof(TYPE_CAL));
|
||
pDeter->CLPAttValid = POSE_OK;
|
||
#endif
|
||
}
|
||
|
||
/***********************************************
|
||
说明：陀螺选择
|
||
***********************************************/
|
||
void Gyro_AttChoice(void)
|
||
{
|
||
UINT8 i =0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//AttCtrlConst_t *tmpConst;
|
||
GyroPara_t *PGyro[GYRO_NUM];
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
|
||
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
|
||
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
//if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0x55)//单陀螺可用
|
||
if( ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
|
||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
|
||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))//单陀螺可用
|
||
{
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if(POSE_OK == PGyro[i]->Gyro_AvailableFlg)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[i]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->Wi_Vld=POSE_OK;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | (0x01<<i) ;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
//else if((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg)==0xAA)//两陀螺可用
|
||
else if(((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_NO == PGyro[2]->Gyro_AvailableFlg))||
|
||
((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_NO == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))||
|
||
((POSE_NO == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg)))
|
||
{
|
||
pDeter->Wi_Vld=POSE_OK;
|
||
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg))
|
||
{
|
||
if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
|
||
}
|
||
}
|
||
else if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
|
||
{
|
||
if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
|
||
}
|
||
}
|
||
}
|
||
else if((POSE_OK == PGyro[0]->Gyro_AvailableFlg)&&(POSE_OK == PGyro[1]->Gyro_AvailableFlg )&&(POSE_OK == PGyro[2]->Gyro_AvailableFlg))//三陀螺可用
|
||
{
|
||
pDeter->Wi_Vld=POSE_OK;
|
||
if ((PGyro[0]->GyroPrior == PGyro[1]->GyroPrior)&&(PGyro[0]->GyroPrior == PGyro[2]->GyroPrior))
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
|
||
}
|
||
else
|
||
{
|
||
if (PGyro[0]->GyroPrior >= PGyro[1]->GyroPrior)
|
||
{
|
||
if (PGyro[0]->GyroPrior >= PGyro[2]->GyroPrior)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[0]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x01;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (PGyro[1]->GyroPrior >= PGyro[2]->GyroPrior)
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[1]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x02;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Wi, PGyro[2]->Gyro_Wi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x04;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else{;}
|
||
for(i=0;i<3;i++)
|
||
{
|
||
tmpAtt->sPerPara.Gyro_ATTWi[i] = pDeter->Wi[i];
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：只用陀螺定姿
|
||
***********************************************/
|
||
void Gyro_AttDeter(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
GyroPara_t *PGyro[GYRO_NUM];
|
||
sAttDeterPara_t *pDeter =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];
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
|
||
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
|
||
{
|
||
Gyro_AttChoice();
|
||
}
|
||
else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
|
||
{
|
||
memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->Wi_Vld=POSE_OK;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
|
||
}else{;}
|
||
|
||
if(POSE_OK == pDeter->QiPre_Vld)//前次姿态有效
|
||
{
|
||
AttLongKutaCal(pDeter->WiPre, pDeter->QiPre, pDeter->Qi);//积分算四元数
|
||
pDeter->Qi_Vld=POSE_OK;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
|
||
if (pDeter->GyroINT_Time<60000)
|
||
{
|
||
pDeter->GyroINT_Time++;
|
||
}
|
||
|
||
if (pDeter->GyroINT_Time > (tmpConst->GyroINT_TLimt*2) )
|
||
{
|
||
pDeter->Qi_Vld=POSE_NO;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
|
||
}
|
||
}
|
||
else//前次姿态无效
|
||
{
|
||
pDeter->Qi_Vld=POSE_NO;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
|
||
pDeter->GyroINT_Time= 0;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：星敏差分计算角速度
|
||
输入：星敏四元数，上周期四元数
|
||
输出：角速度
|
||
***********************************************/
|
||
void QuatDiffCal(TYPE_CAL * pQ_now, TYPE_CAL * pQ_pre, TYPE_CAL * pW)
|
||
{
|
||
TYPE_CAL tmpDeltQ[4] ={0.0};
|
||
TYPE_CAL tmpMatQuat[12] ={0.0};
|
||
TYPE_CAL tmpMatDeltQ[4] ={0.0};
|
||
UINT8 i =0;
|
||
|
||
if(CalPFlag(pQ_now[3])==CalPFlag(pQ_pre[3]))
|
||
{
|
||
for(i=0;i<4;i++)
|
||
{tmpDeltQ[i] = pQ_now[i] - pQ_pre[i];}
|
||
}
|
||
else//应该不会出现这种情况吧
|
||
{
|
||
for(i=0;i<4;i++)
|
||
{tmpDeltQ[i] = -pQ_now[i] - pQ_pre[i];}
|
||
}
|
||
|
||
tmpMatQuat[0] = -pQ_pre[0];
|
||
tmpMatQuat[1] = pQ_pre[3];
|
||
tmpMatQuat[2] = pQ_pre[2];
|
||
tmpMatQuat[3] = -pQ_pre[1];
|
||
|
||
tmpMatQuat[4] = -pQ_pre[1];
|
||
tmpMatQuat[5] = -pQ_pre[2];
|
||
tmpMatQuat[6] = pQ_pre[3];
|
||
tmpMatQuat[7] = pQ_pre[0];
|
||
|
||
tmpMatQuat[8] = -pQ_pre[2];
|
||
tmpMatQuat[9] = pQ_pre[1];
|
||
tmpMatQuat[10] = -pQ_pre[0];
|
||
tmpMatQuat[11] = pQ_pre[3];
|
||
|
||
tmpMatDeltQ[0] = tmpDeltQ[3] / ATTPERIOD_S;
|
||
tmpMatDeltQ[1] = tmpDeltQ[0] / ATTPERIOD_S;
|
||
tmpMatDeltQ[2] = tmpDeltQ[1] / ATTPERIOD_S;
|
||
tmpMatDeltQ[3] = tmpDeltQ[2] / ATTPERIOD_S;
|
||
|
||
MatrixPlusVectorN(tmpMatQuat, tmpMatDeltQ, pW, 3, 4);
|
||
for (i = 0; i < 3; ++i)
|
||
{
|
||
pW[i] *= 2.0f;
|
||
}
|
||
}
|
||
|
||
/***********************************************
|
||
说明：使用陀螺确定角度，陀螺不可用，使用星敏差分的方式计算角度
|
||
***********************************************/
|
||
void SsGyro_AngDeter(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
GyroPara_t *PGyro[GYRO_NUM];
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
|
||
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
|
||
PGyro[0] = &tmpAtt->sPerPara.GyroPara[0];
|
||
PGyro[1] = &tmpAtt->sPerPara.GyroPara[1];
|
||
PGyro[2] = &tmpAtt->sPerPara.GyroPara[2];
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55)
|
||
if ((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg)||(POSE_OK == PGyro[2]->Gyro_AvailableFlg))
|
||
{
|
||
Gyro_AttChoice();
|
||
}
|
||
else if (POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)
|
||
{
|
||
memcpy(pDeter->Wi, tmpAtt->sPerPara.GyroIn_CombWi, 3*sizeof(TYPE_CAL));//输出角速度
|
||
pDeter->Wi_Vld=POSE_OK;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x08;
|
||
}
|
||
else if(POSE_OK == pDeter->QiPre_Vld)//前次姿态有效
|
||
{
|
||
QuatDiffCal(pDeter->Qi, pDeter->QiPre, pDeter->Wi);//差分计算角速度
|
||
pDeter->Wi_Vld = POSE_OK;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type | 0x10;
|
||
}
|
||
else
|
||
{
|
||
//pDeter->Wi_Vld = POSE_NO;
|
||
//pDeter->AttDeter_Type= pDeter->AttDeter_Type & 0xEF;
|
||
;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：星敏陀螺定姿
|
||
输入：星敏,陀螺单元数据
|
||
输出：惯性系四元数/角速度，惯性系四元数/角速度有效标志
|
||
***********************************************/
|
||
void SsGyro_AttDeter(void)
|
||
{
|
||
UINT8 i =0;
|
||
TYPE_CAL tmpRbi[9] ={0.0};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
AttCtrlConst_t *tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
GyroPara_t *PGyro[GYRO_NUM];
|
||
SsPara_t *PStar[STAR_NUM];
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
AttIMPTPara_t *pImpt =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
PPUPara_t *PPPU =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];
|
||
|
||
PStar[0] = &tmpAtt->sPerPara.SsPara[0];
|
||
PStar[1] = &tmpAtt->sPerPara.SsPara[1];
|
||
PStar[2] = &tmpAtt->sPerPara.SsPara[2];
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
|
||
pWMod =&tmpAtt->sModePara;
|
||
|
||
PPPU = &tmpAtt->sPerPara.PPUPara;
|
||
//记录前次结果
|
||
pDeter->QiPre_Vld=pDeter->Qi_Vld;
|
||
memcpy(pDeter->QiPre,pDeter->Qi,4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->WiPre,pDeter->Wi,3*sizeof(TYPE_CAL));
|
||
|
||
//初始化
|
||
pDeter->Qi_Vld=0;
|
||
pDeter->Wi_Vld=0;
|
||
memset(pDeter->Qi, 0, 4*sizeof(TYPE_CAL));
|
||
memset(pDeter->Wi, 0, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type=0;
|
||
|
||
//获取星敏优先级
|
||
PStar[0]->ssPrior=tmpConst->SSUsePrior[0];
|
||
PStar[1]->ssPrior=tmpConst->SSUsePrior[1];
|
||
PStar[2]->ssPrior=tmpConst->SSUsePrior[2];
|
||
#ifdef MINMODULE_TYPE
|
||
//判断星敏ABC是否可用
|
||
for(i=0;i<STAR_NUM;i++)
|
||
{
|
||
PStar[i]->ssAvailableFlg=POSE_NO;
|
||
if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) )
|
||
{PStar[i]->ssAvailableFlg=POSE_OK;}
|
||
}
|
||
#else
|
||
//判断星敏ABC是否可用
|
||
for(i=0;i<STAR_NUM;i++)
|
||
{
|
||
PStar[i]->ssAvailableFlg=POSE_NO;
|
||
if((pImpt->SSUseState[i]==POSE_OK) && (PStar[i]->ssIn_AttVld==POSE_OK) && (PStar[i]->ssCrCheckFlg!=CC_ERROR))
|
||
{PStar[i]->ssAvailableFlg=POSE_OK;}
|
||
}
|
||
#endif
|
||
//获取陀螺优先级
|
||
PGyro[0]->GyroPrior=tmpConst->GyroUsePrior[0];
|
||
PGyro[1]->GyroPrior=tmpConst->GyroUsePrior[1];
|
||
PGyro[2]->GyroPrior=tmpConst->GyroUsePrior[2];
|
||
#ifdef MINMODULE_TYPE
|
||
//判断陀螺是否可用
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
PGyro[i]->Gyro_AvailableFlg=POSE_NO;
|
||
if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK))
|
||
{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}
|
||
}
|
||
#else
|
||
//判断陀螺是否可用
|
||
for(i=0;i<GYRO_NUM;i++)
|
||
{
|
||
PGyro[i]->Gyro_AvailableFlg=POSE_NO;
|
||
if((pImpt->GyroUseState[i]==POSE_OK)&&(PGyro[i]->GyroIn_AttVld==POSE_OK)&&(PGyro[i]->GyroCrCheckFlg!=CC_ERROR))
|
||
{PGyro[i]->Gyro_AvailableFlg=POSE_OK;}
|
||
}
|
||
#endif
|
||
//if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0)//无星敏可用
|
||
if((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))//无星敏可用
|
||
{
|
||
//if ((PGyro[0]->Gyro_AvailableFlg + PGyro[1]->Gyro_AvailableFlg + PGyro[2]->Gyro_AvailableFlg) >= 0x55 ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //陀螺可用
|
||
if (((POSE_OK == PGyro[0]->Gyro_AvailableFlg) || (POSE_OK == PGyro[1]->Gyro_AvailableFlg) || (POSE_OK == PGyro[2]->Gyro_AvailableFlg)) ||( POSE_OK == tmpAtt->sPerPara.GyroIn_Comb)) //陀螺可用
|
||
{
|
||
//只用陀螺定姿
|
||
Gyro_AttDeter();
|
||
}
|
||
else
|
||
{
|
||
//星敏陀螺均不可用
|
||
pDeter->Qi_Vld=0;
|
||
pDeter->Wi_Vld=0;
|
||
pDeter->AttDeter_Type=0;
|
||
}
|
||
}
|
||
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0x55)//单星敏可用
|
||
else if(((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
|
||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
|
||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//单星敏可用
|
||
{
|
||
pDeter->GyroINT_Time = 0;
|
||
//输出姿态四元数
|
||
for(i=0;i<3;i++)
|
||
{
|
||
if(POSE_OK == PStar[i]->ssAvailableFlg)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[i]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[i]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->Qi_Vld=0x55;
|
||
pDeter->Wi_Vld=0x55;
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |(0x20<<i) ;
|
||
break;
|
||
}
|
||
}
|
||
SsGyro_AngDeter();//确定角速度
|
||
}
|
||
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xAA)//两星敏可用
|
||
else if (((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_NO == PStar[2]->ssAvailableFlg))||
|
||
((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_NO == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))||
|
||
((POSE_NO == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg)))//两星敏可用
|
||
{
|
||
pDeter->GyroINT_Time = 0;
|
||
pDeter->Qi_Vld=0x55;
|
||
pDeter->Wi_Vld=0x55;
|
||
if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[1]->ssAvailableFlg))
|
||
{
|
||
if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
|
||
}
|
||
}
|
||
else if ((POSE_OK == PStar[0]->ssAvailableFlg)&&(POSE_OK == PStar[2]->ssAvailableFlg))
|
||
{
|
||
if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
|
||
}
|
||
}
|
||
|
||
SsGyro_AngDeter();//确定角速度
|
||
}
|
||
//else if((PStar[0]->ssAvailableFlg + PStar[1]->ssAvailableFlg + PStar[2]->ssAvailableFlg)==0xFF)//三星敏可用
|
||
else if((POSE_OK == PStar[0]->ssAvailableFlg)&& (POSE_OK == PStar[1]->ssAvailableFlg)&& (POSE_OK == PStar[2]->ssAvailableFlg))//三星敏可用
|
||
{
|
||
pDeter->GyroINT_Time = 0;
|
||
pDeter->Qi_Vld=0x55;
|
||
pDeter->Wi_Vld=0x55;
|
||
if ((PStar[0]->ssPrior == PStar[1]->ssPrior)&&(PStar[0]->ssPrior == PStar[2]->ssPrior))
|
||
{
|
||
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
|
||
}
|
||
else
|
||
{
|
||
if (PStar[0]->ssPrior >= PStar[1]->ssPrior)
|
||
{
|
||
if (PStar[0]->ssPrior >= PStar[2]->ssPrior)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[0]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[0]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x20 ;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (PStar[1]->ssPrior >= PStar[2]->ssPrior)
|
||
{
|
||
memcpy(pDeter->Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[1]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[1]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x40 ;
|
||
}
|
||
else
|
||
{
|
||
memcpy(pDeter->Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Qi, PStar[2]->ssExpose_Qbi, 4*sizeof(TYPE_CAL));
|
||
memcpy(tmpAtt->sPerPara.Ss_Wi, PStar[2]->ssExpose_Wbi, 3*sizeof(TYPE_CAL));
|
||
pDeter->AttDeter_Type= pDeter->AttDeter_Type |0x80 ;
|
||
}
|
||
}
|
||
}
|
||
SsGyro_AngDeter();//确定角速度
|
||
}else{;}
|
||
|
||
//#ifdef MINMODULE_TYPE
|
||
//;
|
||
//#else
|
||
#ifndef MINMODULE_TYPE
|
||
GetTime(pDeter->SatTime);
|
||
//计数轨控推力器加速度
|
||
if((POSE_OK == pDeter->Qi_Vld)&& (ATTMOD_ORBITCTL == pWMod->WorkMode ) &&(0x01 == PPPU->PPUIn_FIREOK))
|
||
{
|
||
Q2Att(pDeter->Qi, tmpRbi);
|
||
MatrixProductHL(tmpRbi, tmpConst->AccThru_Up, pDeter->Ai, 3, 3, 1);
|
||
pDeter->PPUVld = POSE_OK;
|
||
}
|
||
else
|
||
{
|
||
pDeter->Ai[0] = 0;
|
||
pDeter->Ai[1] = 0;
|
||
pDeter->Ai[2] = 0;
|
||
pDeter->PPUVld = POSE_NO;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：利用星敏+模拟太敏，计算卫星+Xb\+Yb\-Zb轴对日太阳角
|
||
***********************************************/
|
||
void Cal_AngToSun(void)
|
||
{
|
||
TYPE_CAL tmpRbi[9] ={0.0};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//AttCtrlConst_t *tmpConst;
|
||
sAttDeterPara_t *pDeter= NULL;
|
||
sAttModPara_t *pWMod= NULL;
|
||
sAttDataPrePara_t *pDataPre= NULL;
|
||
sAttOrbitGetPara_t *pOrbit = NULL;
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(tmpAtt==NULL)
|
||
{return;}
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
pDataPre=&tmpAtt->sPerPara ;
|
||
pOrbit = &tmpAtt->sOrbitPara;
|
||
//初始化
|
||
if((POSE_OK == pDeter->Qi_Vld)&&(POSE_OK == pOrbit->MJCLv)) //使用星敏且星敏可用
|
||
{
|
||
Q2Att(pDeter->Qi, tmpRbi);
|
||
MatrixProductHL(tmpRbi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVecb, 3, 3, 1);
|
||
pDeter->CalAngToSun_Source=0x55;
|
||
pDeter->AngToSun_ValidFlg=0x55;
|
||
}
|
||
else if ((POSE_OK == pDataPre->AssPara.Ass_SunVecVld)&&(CC_ERROR!= pDataPre->AssPara.AssCrCheckFlg))//太敏可用
|
||
{
|
||
memcpy(pDeter->SunVecb, pDataPre->AssPara.Ass_SunVecb, 3*sizeof(TYPE_CAL));
|
||
pDeter->CalAngToSun_Source=0xAA;
|
||
pDeter->AngToSun_ValidFlg=0x55;
|
||
}
|
||
else
|
||
{
|
||
pDeter->CalAngToSun_Source=0x00;
|
||
pDeter->AngToSun_ValidFlg=0;
|
||
}
|
||
|
||
if (ATTMOD_MagTOSUN == pWMod->WorkMode)
|
||
{
|
||
if ((POSE_OK !=pWMod->WModSadaOpen))
|
||
{
|
||
//计算-Z轴对日太阳角
|
||
if (POSE_OK == pDeter->AngToSun_ValidFlg)
|
||
{
|
||
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
else
|
||
{
|
||
pDeter->AngToSun[0]=0.0f;
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//计算+Y轴对日太阳角
|
||
if (POSE_OK == pDeter->AngToSun_ValidFlg)
|
||
{
|
||
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[1]);
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]= POSE_ATAN2F(pDeter->SunVecb[0],pDeter->SunVecb[1]);
|
||
}
|
||
else
|
||
{
|
||
pDeter->AngToSun[0]=0.0f;
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
}
|
||
}
|
||
else if (ATTMOD_WhlTOSUN == pWMod->WorkMode)
|
||
{
|
||
if ((POSE_OK!= pWMod->WModSadaOpen))
|
||
{
|
||
//计算-Z轴对日太阳角
|
||
if (POSE_OK ==pDeter->AngToSun_ValidFlg)
|
||
{
|
||
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
else
|
||
{
|
||
pDeter->AngToSun[0]=0.0f;
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
//计算X轴对日太阳角
|
||
if (POSE_OK == pDeter->AngToSun_ValidFlg)
|
||
{
|
||
pDeter->AngToSun[0]= 0.0f;
|
||
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[2],pDeter->SunVecb[0]);
|
||
pDeter->AngToSun[2]=- POSE_ATAN2F(pDeter->SunVecb[1],pDeter->SunVecb[0]);
|
||
}
|
||
else
|
||
{
|
||
pDeter->AngToSun[0]=0.0f;
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
}
|
||
}
|
||
else if (ATTMOD_UNCTL == pWMod->WorkMode)
|
||
{
|
||
//计算-Z轴对日太阳角
|
||
if (POSE_OK == pDeter->AngToSun_ValidFlg)
|
||
{
|
||
pDeter->AngToSun[0]= -POSE_ATAN2F(pDeter->SunVecb[1],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[1]= POSE_ATAN2F(pDeter->SunVecb[0],-pDeter->SunVecb[2]);
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
else
|
||
{
|
||
pDeter->AngToSun[0]=0.0f;
|
||
pDeter->AngToSun[1]=0.0f;
|
||
pDeter->AngToSun[2]=0.0f;
|
||
}
|
||
}else{;}
|
||
|
||
}
|
||
|
||
|
||
/***********************************************
|
||
说明：本体系磁场强度计算
|
||
***********************************************/
|
||
//磁场计算系磁场强度
|
||
void DertCalcBb(void)
|
||
{
|
||
UINT8 i =0;
|
||
UINT32 tmpCNT = 0;
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
//AttCtrlConst_t *tmpConst;
|
||
MagPara_t *pMag =NULL;
|
||
MtPara_t *pMt =NULL;
|
||
sAttEnvPara_t *pEnv =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
AttIMPTPara_t *pImpt =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(tmpAtt==NULL)
|
||
{return;}
|
||
//tmpConst=(AttCtrlConst_t *)ATTCLT_DATA2_ADDR;
|
||
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
pMt=&tmpAtt->sPerPara.MtPara;
|
||
pEnv = &tmpAtt->sEnvPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
pImpt=&tmpAtt->sIMPTPara;
|
||
|
||
//判断磁强计是否可用
|
||
for(i=0;i<MAG_NUM;i++)
|
||
{
|
||
pMag->MagAvailableFlg[i]=POSE_NO;
|
||
if((pImpt->MagUseState[i]==POSE_OK) && (pMag->MagIn_Valid[i] ==POSE_OK) && (pMag->MagCrCheckFlg[i]!=CC_ERROR))
|
||
{pMag->MagAvailableFlg[i]=POSE_OK;}
|
||
}
|
||
|
||
//tmpCNT = pMt-> MTCtrlTime % tmpConst->MTCtrlClc;
|
||
tmpCNT = pMt-> MTCtrlTime % 10;
|
||
if ((1 == tmpCNT)||(0x88 == pMag->MagUseSwich))
|
||
{
|
||
memcpy(pMag->Mag_BbPre, pMag->Mag_Bb, 3*sizeof(TYPE_CAL));
|
||
pMag->Mag_UsePre = pMag->Mag_Use;
|
||
}
|
||
|
||
#ifdef MINMODULE_TYPE
|
||
if ( POSE_OK == pMag->MagAvailableFlg[0])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x11;
|
||
}
|
||
else if (POSE_OK == pMag->MagAvailableFlg[1])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x22;
|
||
}
|
||
else if (POSE_OK == pMag->MagIn_Valid[2])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x44;
|
||
}
|
||
else
|
||
{
|
||
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
|
||
pMag->Mag_Use =POSE_NO;
|
||
pMag->MagUseSwich =POSE_NO;
|
||
}
|
||
#else
|
||
|
||
if ((ATTMOD_RATEDMP == pWMod->WorkMode)|| (ATTMOD_MagTOSUN == pWMod->WorkMode))
|
||
{
|
||
|
||
if ( POSE_OK == pMag->MagAvailableFlg[0])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x11;
|
||
}
|
||
else if (POSE_OK == pMag->MagAvailableFlg[1])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x22;
|
||
}
|
||
else if (POSE_OK==pMag->MagIn_Valid[2])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x44;
|
||
}
|
||
else if (POSE_OK == pEnv->fvIn_Valid)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x88;
|
||
}
|
||
else
|
||
{
|
||
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
|
||
pMag->Mag_Use =POSE_NO;
|
||
pMag->MagUseSwich =POSE_NO;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (POSE_OK == pEnv->fvIn_Valid)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pEnv->fvBb, 3*sizeof(TYPE_CAL));
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x88;
|
||
}
|
||
else if (POSE_OK == pMag->MagAvailableFlg[0])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[0], 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x11;
|
||
}
|
||
else if (POSE_OK == pMag->MagAvailableFlg[1])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaAB[1], 3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
pMag->Mag_Use =POSE_OK;
|
||
pMag->MagUseSwich = 0x22;
|
||
}
|
||
else if (POSE_OK==pMag->MagIn_Valid[2])
|
||
{
|
||
if (1 == tmpCNT)
|
||
{
|
||
memcpy(pMag->Mag_Bb, pMag->Mag_Bb_MeaZH, 3*sizeof(TYPE_CAL));
|
||
}
|
||
pMag->Mag_Use = POSE_OK;
|
||
pMag->MagUseSwich = 0x44;
|
||
}
|
||
else
|
||
{
|
||
memset(pMag->Mag_Bb,0.0,3*sizeof(TYPE_CAL));
|
||
pMag->Mag_Use =POSE_NO;
|
||
pMag->MagUseSwich =POSE_NO;
|
||
}
|
||
}
|
||
|
||
if (POSE_OK == pMag->Mag_Use)
|
||
{
|
||
for(i=0; i<3; i++)
|
||
{pMag->Mag_UBb[i] = pMag->Mag_Bb[i] ;}
|
||
unitary( &pMag->Mag_UBb[0], 3 ); // 归一化
|
||
}
|
||
#endif
|
||
}
|
||
|
||
/***********************************************
|
||
说明：计算轨道系下姿态、角速度、太阳矢量
|
||
***********************************************/
|
||
void AttDeterOrb(void)
|
||
{
|
||
TYPE_CAL tmpRbi[9] ={0.0}, tmpRoi[9] ={0.0}, tmpRio[9] ={0.0}, tmpRbo[9] ={0.0};
|
||
TYPE_CAL tmpWoi[3] ={0.0};
|
||
TYPE_CAL tmpWbo[3] ={0.0};
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttOrbitGetPara_t *pOrbitIn =NULL;
|
||
|
||
// tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(tmpAtt==NULL)
|
||
{return;}
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pOrbitIn=&tmpAtt->sOrbitPara;
|
||
|
||
if((0 == pDeter->Qi_Vld)||(0 == pOrbitIn->OrbitVld))
|
||
{
|
||
pDeter->AttOrb_Vld=0;
|
||
return;
|
||
}
|
||
pDeter->AttOrb_Vld=0x55;
|
||
//计算轨道系姿态角
|
||
Q2Att(pDeter->Qi, tmpRbi);
|
||
CalTransMatrix_IO(pOrbitIn->JPos, pOrbitIn->JVel, tmpRoi);
|
||
MatrixTransposeHL(tmpRoi, tmpRio, 3, 3);
|
||
MatrixProductHL(tmpRbi, tmpRio, tmpRbo, 3, 3, 3);
|
||
Matrix2Eul312A(tmpRbo, pDeter->AttAngOrb);
|
||
|
||
//计算卫星轨道系四元素
|
||
Eul312A2Quat(pDeter->AttAngOrb, pDeter->Qo);
|
||
|
||
//计算轨道系太阳矢量
|
||
MatrixProductHL(tmpRoi, tmpAtt->sEnvPara.SunVecI, pDeter->SunVeco, 3, 3, 1);
|
||
|
||
if(0 == pDeter->Wi_Vld)
|
||
{
|
||
return;
|
||
}
|
||
|
||
//计算姿态角速度
|
||
AglSpdOofICal(pOrbitIn->JPos, pOrbitIn->JVel, tmpWoi);
|
||
//pDeter->OrbAttRate_Wo = tmpWoi[1];
|
||
MatrixProductHL(tmpRbo, tmpWoi, tmpWbo, 3, 3, 1);
|
||
tmpWbo[0]=pDeter->Wi[0]-tmpWbo[0];
|
||
tmpWbo[1]=pDeter->Wi[1]-tmpWbo[1];
|
||
tmpWbo[2]=pDeter->Wi[2]-tmpWbo[2];
|
||
memcpy(pDeter->AttRateOrb,tmpWbo,3*sizeof(TYPE_CAL));
|
||
|
||
}
|
||
|
||
/***********************************************
|
||
说明：计算轨控参考系姿态
|
||
***********************************************/
|
||
void AttDeterOrbCtrl(void)
|
||
{
|
||
TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//轨道系到轨道系欧拉角,旋转矩阵
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttOrbitCtlInfo_t *pOrb =NULL;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL == tmpAtt)
|
||
{return;}
|
||
pOrb=&tmpAtt->sOrbitInp;
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
|
||
pDeter->AttOrbCtrl_Vld=0;
|
||
memset(pDeter->AttAngOrbCtrl,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttRateOrbCtrl,0.0,3*sizeof(TYPE_CAL));
|
||
//如果当前工作模式不为轨控调姿（0X66）或轨控（0X77）或轨控后（0X8），退出
|
||
//如卫星轨道系姿态有效标志为无效
|
||
if((ATTMOD_ATTAJUSTPRE!= tmpAtt->sModePara.WorkMode)&&(ATTMOD_ORBITCTL!= tmpAtt->sModePara.WorkMode))
|
||
{return;}
|
||
|
||
if (0x55 == pDeter->AttOrb_Vld)
|
||
{
|
||
pDeter->AttOrbCtrl_Vld=0x55;
|
||
}
|
||
else
|
||
{
|
||
pDeter->AttOrbCtrl_Vld = 0x00;
|
||
return;
|
||
}
|
||
|
||
//获取卫星目标调姿姿态
|
||
memcpy(pDeter->AttAngTar,pOrb->OrbCtrlPackCur.OrbCtrAng,3*sizeof(TYPE_CAL));
|
||
|
||
//由卫星轨道系四元数Qbo得到转换矩阵Rbo
|
||
Q2Att(pDeter->Qo, tmpRbo);
|
||
//轨道系到轨控调整角转移矩阵RCo
|
||
Att2M(pDeter->AttAngTar, tmpRCo);
|
||
|
||
MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
|
||
|
||
//轨控参考系转换矩阵
|
||
MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
|
||
|
||
//轨控参考系姿态角
|
||
Matrix2Eul312A(tmpRbc, pDeter->AttAngOrbCtrl);
|
||
//卫星轨控参考系角速度即为卫星轨道系角速度
|
||
memcpy(pDeter->AttRateOrbCtrl,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
/***********************************************
|
||
说明：计算目标参考系姿态
|
||
***********************************************/
|
||
void AttDeterTarRe(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
sAttTARCCtlInfo_t *pTAR =NULL;
|
||
//sAttOrbitCtlInfo_t *pOrb;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(tmpAtt==NULL)
|
||
{return;}
|
||
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
pTAR=&tmpAtt->sTARCInp;
|
||
//pOrb=&tmpAtt->sTARCInp;
|
||
|
||
TYPE_CAL tmpRbo[9] ={0.0},tmpRCo[9] ={0.0},tmpRoC[9] ={0.0},tmpRbc[9] ={0.0};//轨道系到目标系欧拉角,旋转矩阵
|
||
|
||
memset(pDeter->AttAngTarRe,0.0,3*sizeof(TYPE_CAL));
|
||
memset(pDeter->AttRateTarRe,0.0,3*sizeof(TYPE_CAL));
|
||
|
||
pDeter->AttTarRe_Vld = 0x00;
|
||
//如果当前工作模式为目标定向调姿（0X99）或目标定向（0XAA），进行以下计算；否则，退出。
|
||
if((pWMod->WorkMode!=ATTMOD_ATTAJUST)&&(pWMod->WorkMode!=ATTMOD_ONLTOTAR))
|
||
{return;}
|
||
//如卫星轨道系姿态有效标志为无效，置卫星轨控参考系姿态有效标志为无效，并退出；否则，置标志为有效
|
||
if (POSE_OK == pDeter->AttOrb_Vld)
|
||
{
|
||
pDeter->AttTarRe_Vld = 0x55;
|
||
}
|
||
else
|
||
{
|
||
pDeter->AttTarRe_Vld = 0x00;
|
||
return;
|
||
}
|
||
|
||
//获取目标调姿姿态
|
||
memcpy(pDeter->AttAngTar,pTAR->TARCtrlPackCur.TARCtrAng,3*sizeof(TYPE_CAL));
|
||
|
||
//由卫星轨道系四元数Qbo得到转换矩阵Rbo
|
||
Q2Att(pDeter->Qo, tmpRbo);
|
||
//轨道系目标角转移矩阵RCo
|
||
Att2M(pDeter->AttAngTar, tmpRCo);
|
||
|
||
MatrixTransposeHL(tmpRCo, tmpRoC, 3, 3);
|
||
|
||
//目标参考系转换矩阵
|
||
MatrixProductHL(tmpRbo, tmpRoC, tmpRbc, 3, 3, 3);
|
||
//目标参考系姿态角
|
||
Matrix2Eul312A(tmpRbc, pDeter->AttAngTarRe);
|
||
//卫星目标参考系角速度即为卫星轨道系角速度
|
||
memcpy(pDeter->AttRateTarRe,pDeter->AttRateOrb,3*sizeof(TYPE_CAL));
|
||
}
|
||
|
||
/***********************************************
|
||
说明：闭环姿态确定
|
||
输入：模式
|
||
输出：闭环姿态，闭环姿态角
|
||
***********************************************/
|
||
void LoopAttDeter(void)
|
||
{
|
||
sAttPriData *tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
sAttDeterPara_t *pDeter =NULL;
|
||
sAttModPara_t *pWMod =NULL;
|
||
MagPara_t *pMag =NULL;
|
||
|
||
//tmpAtt=(sAttPriData *)ATTCLT_DATA1_ADDR;
|
||
if(NULL ==tmpAtt)
|
||
{return;}
|
||
pDeter=&tmpAtt->sDeterPara;
|
||
pWMod=&tmpAtt->sModePara;
|
||
pMag=&tmpAtt->sPerPara.MagPara;
|
||
|
||
pDeter->CLPAttValid=0;
|
||
|
||
memset(pDeter->CLPAttAngRatPre,0.0,3*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->CLPAttAngRatPre, pDeter->CLPAttAngRat, 3*sizeof(TYPE_CAL));
|
||
memset(pDeter->CLPAttAng, 0.0, 3*sizeof(TYPE_CAL));
|
||
memset(pDeter->CLPAttAngRat, 0.0, 3*sizeof(TYPE_CAL));
|
||
#ifdef MINMODULE_TYPE
|
||
switch (pWMod->WorkMode)
|
||
{
|
||
case ATTMOD_RATEDMP:
|
||
case ATTMOD_MagTOSUN:
|
||
if (POSE_OK == pMag->Mag_Use )
|
||
{
|
||
pDeter->CLPAttValid= 0x55;
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
#else
|
||
switch (pWMod->WorkMode)
|
||
{
|
||
case ATTMOD_UNCTL:
|
||
case ATTMOD_NOCOLL:
|
||
if (0x55 == pDeter->Wi_Vld )
|
||
{
|
||
pDeter->CLPAttValid= 0x55;
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_RATEDMP: //zmx20231007
|
||
case ATTMOD_MagTOSUN:
|
||
if (POSE_OK == pMag->Mag_Use)
|
||
{
|
||
pDeter->CLPAttValid= 0x55;
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_WhlTOSUN:
|
||
if(0x55 == pDeter->Wi_Vld)
|
||
{
|
||
pDeter->CLPAttValid=0x55; //zmx20231009
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->Wi, 3*sizeof(TYPE_CAL));
|
||
if (pDeter->AngToSun_ValidFlg==0x55)
|
||
{
|
||
//pDeter->CLPAttValid=0x55;
|
||
pDeter->CLPAttAng[0]=pDeter->AngToSun[0];
|
||
pDeter->CLPAttAng[1]=pDeter->AngToSun[1];
|
||
pDeter->CLPAttAng[2]=pDeter->AngToSun[2];
|
||
}
|
||
}
|
||
break;
|
||
|
||
case ATTMOD_ONLTOEARTH:
|
||
case ATTMOD_WHEELTOEARTH:
|
||
if(0x55 == pDeter->AttOrb_Vld)
|
||
{
|
||
pDeter->CLPAttValid=0x55;
|
||
memcpy(pDeter->CLPAttAng, pDeter->AttAngOrb, 3*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrb, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
case ATTMOD_ATTAJUST:
|
||
case ATTMOD_ONLTOTAR:
|
||
if(0x55 == pDeter->AttTarRe_Vld)
|
||
{
|
||
pDeter->CLPAttValid=0x55;
|
||
memcpy(pDeter->CLPAttAng, pDeter->AttAngTarRe, 3*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateTarRe, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
case ATTMOD_ATTAJUSTPRE:
|
||
case ATTMOD_ORBITCTL:
|
||
if(0x55 == pDeter->AttOrbCtrl_Vld)
|
||
{
|
||
pDeter->CLPAttValid=0x55;
|
||
memcpy(pDeter->CLPAttAng, pDeter->AttAngOrbCtrl, 3*sizeof(TYPE_CAL));
|
||
memcpy(pDeter->CLPAttAngRat, pDeter->AttRateOrbCtrl, 3*sizeof(TYPE_CAL));
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
/************************************************
|
||
说明：定姿算法接口，每周期调用一次定姿处理,包括数据预处理以及定姿计算
|
||
输入：wbi:陀螺惯性系姿态角速度 （rad/s）
|
||
startFlag:定姿算法启动标志
|
||
返回 :定姿模式1算法惯性系角速度 （rad/s）
|
||
************************************************/
|
||
void ZKPGAct(void)
|
||
{
|
||
#ifdef MINMODULE_TYPE
|
||
//姿态变量初始化
|
||
ZKPGInit();
|
||
//星敏陀螺定姿
|
||
SsGyro_AttDeter();
|
||
//磁场计算系磁场强度
|
||
DertCalcBb();
|
||
//计算卫星-Zb轴对日太阳角
|
||
Cal_AngToSun();
|
||
//闭环姿态确定
|
||
LoopAttDeter();
|
||
#else
|
||
//姿态变量初始化
|
||
ZKPGInit();
|
||
//星敏陀螺定姿
|
||
SsGyro_AttDeter();
|
||
//磁场计算系磁场强度
|
||
DertCalcBb();
|
||
//计算卫星-Zb轴对日太阳角
|
||
Cal_AngToSun();
|
||
//printf("ZL0\n");
|
||
//计算轨道系姿态
|
||
AttDeterOrb();
|
||
//printf("ZL1\n");
|
||
//目标参考系姿态
|
||
AttDeterTarRe();
|
||
//printf("ZL2\n");
|
||
//计算轨控惯性系姿态
|
||
AttDeterOrbCtrl();
|
||
//printf("ZL3\n");
|
||
//闭环姿态确定
|
||
LoopAttDeter();
|
||
#endif
|
||
}
|