236 lines
13 KiB
C++
236 lines
13 KiB
C++
#include "sensor_thrust.h"
|
||
|
||
#include "liblog.h"
|
||
|
||
#include <math.h>
|
||
#include <time.h>
|
||
#include <stdlib.h>
|
||
#include "GNCFunction.h"
|
||
|
||
//static double ImpCost = 0.0; /* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||
|
||
void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12],
|
||
double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);
|
||
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod);
|
||
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]);
|
||
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);
|
||
|
||
|
||
|
||
//输出:ThrustForce[3], ThrustTorque[3],
|
||
// SatMass, SatMtxI[3][3],
|
||
//输入: JetPWMPlan[12]
|
||
void sensor_ThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12])
|
||
{
|
||
double JetPWMPeriod, JetPos[3][12], JetDir[3][12], ThrustIsp, SatMassTemp1;
|
||
|
||
/*喷口PWM调制周期，即GNC控制周期，与控制程序周期相同*/
|
||
JetPWMPeriod = 1;
|
||
/*比例系数*/
|
||
ThrustIsp = 343.0;
|
||
/*本体系下的喷口安装位置，列向量 * 12，m*/
|
||
JetPos[0][0] = -0.233; JetPos[0][1] = -0.233; JetPos[0][2] = 0.383; JetPos[0][3] = 0.383; JetPos[0][4] = 0.18; JetPos[0][5] = -0.18; JetPos[0][6] = 0.18; JetPos[0][7] = -0.18; JetPos[0][8] = 0; JetPos[0][9] = 0; JetPos[0][10] = 0; JetPos[0][11] = 0;
|
||
JetPos[1][0] = 0; JetPos[1][1] = 0; JetPos[1][2] = 0; JetPos[1][3] = 0; JetPos[1][4] = -0.183; JetPos[1][5] = -0.183; JetPos[1][6] = 0.183; JetPos[1][7] = 0.183; JetPos[1][8] = 0.156; JetPos[1][9] = -0.156; JetPos[1][10] = 0.156; JetPos[1][11] = -0.156;
|
||
JetPos[2][0] = 0.147; JetPos[2][1] = -0.147; JetPos[2][2] = 0.147; JetPos[2][3] = -0.147; JetPos[2][4] = 0; JetPos[2][5] = 0; JetPos[2][6] = 0; JetPos[2][7] = 0; JetPos[2][8] = -0.323; JetPos[2][9] = -0.323; JetPos[2][10] = 0.323; JetPos[2][11] = 0.323;
|
||
/*本体系下的推力方向，列向量 * 12，单位方向矢量*/
|
||
JetDir[0][0] = 1; JetDir[0][1] = 1; JetDir[0][2] = -1; JetDir[0][3] = -1; JetDir[0][4] = 0; JetDir[0][5] = 0; JetDir[0][6] = 0; JetDir[0][7] = 0; JetDir[0][8] = 0; JetDir[0][9] = 0; JetDir[0][10] = 0; JetDir[0][11] = 0;
|
||
JetDir[1][0] = 0; JetDir[1][1] = 0; JetDir[1][2] = 0; JetDir[1][3] = 0; JetDir[1][4] = 1; JetDir[1][5] = 1; JetDir[1][6] = -1; JetDir[1][7] = -1; JetDir[1][8] = 0; JetDir[1][9] = 0; JetDir[1][10] = 0; JetDir[1][11] = 0;
|
||
JetDir[2][0] = 0; JetDir[2][1] = 0; JetDir[2][2] = 0; JetDir[2][3] = 0; JetDir[2][4] = 0; JetDir[2][5] = 0; JetDir[2][6] = 0; JetDir[2][7] = 0; JetDir[2][8] = 1; JetDir[2][9] = 1; JetDir[2][10] = -1; JetDir[2][11] = -1;
|
||
/*平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
|
||
int Cmd = 1;
|
||
/*初始推进剂质量，kg*/
|
||
double InitGasMass = 3.55;//6.5 * 0.7692;
|
||
/*平台质量，kg，5种工况，1 * 5*/
|
||
double DryMassData[5] = { 48.381, 48.381, 46.013, 45.898, 43.53 };//{ 34.99881, 34.99881, 32.312115, 32.22625, 29.535285 };
|
||
/*推进剂质心卫星位置，m，推力器布局坐标系*/
|
||
double GasMassPos[3] = { -25 * 1e-3, 0.0, -201.44 * 1e-3 };
|
||
/*平台质心位置，m，推力器布局坐标系，5种工况，3 * 5*/
|
||
double DryMassPosData[3][5] = { {8.39 * 0.001, 8.03 * 0.001, 10.29 * 0.001, 10.19 * 0.001, 12.96 * 0.001},
|
||
{ -8.92 * 0.001, -7.94 * 0.001, -15.98 * 0.001, 0.19 * 0.001, -8.00 * 0.001},
|
||
{ -4.72 * 0.001, -4.72 * 0.001, 0.25 * 0.001, 1.30 * 0.001, 7.50 * 0.001} };
|
||
/*初始推进剂转动惯量，kg·m3，取决于推进剂质心且对齐到推力器布局坐标系*/
|
||
double InitGasMtxI[3][3] = { {0.0571059330000000, 0, -0.00222918000000000},
|
||
{0, 0.0319816850000000, 0},
|
||
{-0.00222918000000000, 0, 0.0678860230000000} };
|
||
/*平台转动惯量，kg·m3，取决于平台质心且对齐到推力器布局坐标系，5种工况，3* (3 * 5)*/
|
||
double DryMtxIData[3][15] = { {1.86687640893600, 0.0286157968140000, -0.439427130912000, 1.89572816175600, 0.0409105154700000, -0.437885017878000, 1.87455028889500, 0.0482311009420000, -0.445634256610000, 1.90556951219600, 0.0362650204340000, -0.452313634868000, 1.89121192802100, 0.0438251188440000, -0.462662149911000},
|
||
{0.0286157968140000, 2.3271403853280, -0.0114310573740000, 0.0409105154700000, 2.31908819868000, -0.0163531346220000, 0.0482311009420000, 2.29461077938400, -0.00115876103900000, 0.0362650204340000, 2.32061908341600, -0.0323862321540000, 0.0438251188440000, 2.29928834970900, -0.0167186100900000},
|
||
{-0.439427130912000, -0.0114310573740000, 1.20621470673000, -0.437885017878000, -0.0163531346220000, 1.22701427290200, -0.445634256610000, -0.00115876103900000, 1.17827923915600, -0.452313634868000, -0.0323862321540000, 1.19281841776200, -0.462662149911000, -0.0167186100900000, 1.14977779874100} };
|
||
|
||
GetThrustMeas(ThrustForce, ThrustTorque, &SatMassTemp1, SatMtxI, JetPWMPlan, JetPWMPeriod, JetPos, JetDir,
|
||
ThrustIsp, InitGasMass, DryMassData, GasMassPos, DryMassPosData, Cmd, InitGasMtxI, DryMtxIData);
|
||
*SatMass = SatMassTemp1;
|
||
|
||
//printf("ThrustForce is %10.5f \t %10.5f \t %10.5f\r\n", ThrustForce[0], ThrustForce[1], ThrustForce[2]);
|
||
//printf("ThrustTorque is %10.5f \t %10.5f \t %10.5f\r\n", ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
|
||
//spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},ThrustTorque为:{4:.6f}, {5:.6f}, {6:.6f}",__FUNCTION__,
|
||
// ThrustForce[0], ThrustForce[1], ThrustForce[2],
|
||
// ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
|
||
spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},{4:.6f}, {5:.6f}, {6:.6f}, {7:.6f}", __FUNCTION__,
|
||
ThrustForce[0], ThrustForce[1], ThrustForce[2],
|
||
ThrustTorque[0], ThrustTorque[1], ThrustTorque[2],
|
||
*SatMass);
|
||
}
|
||
|
||
|
||
/* 获得微推力器三轴推力、力矩以及气体消耗 */
|
||
void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12],
|
||
double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {
|
||
|
||
/* 微推力器模拟模块 */
|
||
/* 输入量 */
|
||
/* FullTankForceStd 充气后推力器推力标称值 */
|
||
/* NullTankForceStd 触发充气推力器推力标称值 */
|
||
/* TankForceDscPara 推力下降系数，N/(N*s) */
|
||
/* TankGasTime 二级储箱充气时长 */
|
||
/* JetPWMPlan 喷口PWM调制指令值，即GNC输出的推力器指令 */
|
||
/* JetPWMPeriod 喷口PWM调制周期，即GNC控制周期 */
|
||
/* tNowSec 仿真时刻，s */
|
||
/* JetPos 本体系下的喷口安装位置，列向量*12，m */
|
||
/* JetDir 本体系下的推力方向，列向量*12，单位方向矢量 */
|
||
/* SatMassPos 二级储箱充气时长 */
|
||
/* ThrustIsp 比例系数 */
|
||
/* 输出量 */
|
||
/* ThrustForce 推力器合力 */
|
||
/* ThrustTorque 合力矩*/
|
||
/* GasCost 储气消耗*/
|
||
|
||
static double ImpCost = 0.0; /* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||
double JetForce[12], SatMassPos[3], GasCost, SatMassTemp;
|
||
|
||
GetJetForce(JetForce, JetPWMPlan, JetPWMPeriod);
|
||
for (int i = 0; i < 12; i++)
|
||
{
|
||
ImpCost += JetForce[i];
|
||
}
|
||
GasCost = ImpCost / ThrustIsp;
|
||
|
||
SatMassProperty(&SatMassTemp, SatMassPos, SatMtxI, InitGasMass, DryMassData, GasMassPos, DryMassPosData, GasCost, Cmd, InitGasMtxI, DryMtxIData);
|
||
*SatMass = SatMassTemp;
|
||
Jet2Thrust(ThrustForce, ThrustTorque, JetForce, JetPos, JetDir, SatMassPos);
|
||
}
|
||
|
||
|
||
/* 推力器12喷口推力输出模拟 */
|
||
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod) {
|
||
|
||
/* 推力器12喷口推力输出模拟模块 */
|
||
/* 输入量 */
|
||
/* FullTankForceStd 充气后推力器推力标称值 */
|
||
/* NullTankForceStd 触发充气推力器推力标称值 */
|
||
/* TankForceDscPara 推力下降系数，N/(N*s) */
|
||
/* TankGasTime 二级储箱充气时长 */
|
||
/* JetPWMPlan 喷口PWM调制指令值，即GNC输出的推力器指令 */
|
||
/* JetPWMPeriod 喷口PWM调制周期，即GNC控制周期 */
|
||
/* tNowSec 仿真时刻，s */
|
||
/* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||
/* 输出量 */
|
||
/* JetForce 推力器12喷口推力输出值（1s内平均推力） */
|
||
|
||
/*充气后推力器推力标称值*/
|
||
double FullTankForceStd = 40 * 1e-3; //45 * 1e-3;
|
||
// 当前二级储箱压力（推力）
|
||
double TankForce = FullTankForceStd;
|
||
|
||
for (int ii = 0; ii < 12; ii++) {
|
||
JetForce[ii] = TankForce * (JetPWMPlan[ii] / JetPWMPeriod);
|
||
}
|
||
}
|
||
|
||
|
||
/* 获得本体系三轴推力和力矩 */
|
||
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]) {
|
||
|
||
/* 推力器合力和合力矩模拟模块 */
|
||
/* 输入量 */
|
||
/* JetForce 推力器12喷口推力输出值（1s内平均推力） */
|
||
/* JetPos 本体系下的喷口安装位置，列向量*12，m */
|
||
/* JetDir 本体系下的推力方向，列向量*12，单位方向矢量 */
|
||
/* SatMassPos 二级储箱充气时长 */
|
||
/* 输出量 */
|
||
/* ThrustForce 推力器合力 */
|
||
/* ThrustTorque 合力矩*/
|
||
|
||
double iiJetForceVec[3], iiJetPosVec[3], iiJetTorque[3], ThrustForceTemp[3], ThrustTorqueTemp[3];
|
||
|
||
ThrustForceTemp[0] = 0.0; ThrustForceTemp[1] = 0.0; ThrustForceTemp[2] = 0.0;
|
||
ThrustTorqueTemp[0] = 0.0; ThrustTorqueTemp[1] = 0.0; ThrustTorqueTemp[2] = 0.0;
|
||
for (int ii = 0; ii < 12; ii++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
iiJetForceVec[j] = JetForce[ii] * JetDir[j][ii];
|
||
}
|
||
for (int j = 0; j < 3; j++) {
|
||
iiJetPosVec[j] = JetPos[j][ii] - SatMassPos[j];
|
||
}
|
||
//iiJetTorque = cross( iiJetPosVec,iiJetForceVec );
|
||
iiJetTorque[0] = iiJetPosVec[1] * iiJetForceVec[2] - iiJetPosVec[2] * iiJetForceVec[1];
|
||
iiJetTorque[1] = iiJetPosVec[2] * iiJetForceVec[0] - iiJetPosVec[0] * iiJetForceVec[2];
|
||
iiJetTorque[2] = iiJetPosVec[0] * iiJetForceVec[1] - iiJetPosVec[1] * iiJetForceVec[0];
|
||
|
||
/*mtxAdd((double*)ThrustForceTemp, (double*)ThrustForceTemp, (double*)iiJetForceVec, 3, 1);
|
||
mtxAdd((double*)ThrustTorqueTemp, (double*)ThrustTorqueTemp, (double*)iiJetTorque, 3, 1);*/
|
||
for (int j = 0; j < 3; j++) {
|
||
ThrustForceTemp[j] += iiJetForceVec[j];
|
||
ThrustTorqueTemp[j] += iiJetTorque[j];
|
||
}
|
||
}
|
||
mtxCpy((double*)ThrustForce, (double*)ThrustForceTemp, 3, 1);
|
||
mtxCpy((double*)ThrustTorque, (double*)ThrustTorqueTemp, 3, 1);
|
||
}
|
||
|
||
|
||
/* 追踪星质量特性 */
|
||
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {
|
||
|
||
/* 推力器合力和合力矩模拟模块 */
|
||
/* 输入量 */
|
||
/* InitGasMass 初始推进剂质量，kg */
|
||
/* DryMassData 平台质量，kg，5种工况，1*5； */
|
||
/* GasMassPos 推进剂质心卫星位置，m，推力器布局坐标系 */
|
||
/* DryMassPosData 平台质心位置，m，推力器布局坐标系，5种工况，3*5 */
|
||
/* GasCost 累积冲量消耗质量，kg*/
|
||
/* Cmd 平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
|
||
/* 输出量 */
|
||
/* SatMassPos 整星质心位置，m,推力器布局坐标系 */
|
||
|
||
//推进剂剩余质量
|
||
double GasMass = InitGasMass - GasCost;
|
||
//卫星平台质量，根据指令选择
|
||
int ii = Cmd - 1;
|
||
double DryMass = DryMassData[ii];
|
||
//整星质量
|
||
*SatMass = DryMass + GasMass;
|
||
//卫星平台质心位置，根据指令选择
|
||
double DryMassPos[3] = { DryMassPosData[0][ii], DryMassPosData[1][ii], DryMassPosData[2][ii] };
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
SatMassPos[i] = (GasMass * GasMassPos[i] + DryMass * DryMassPos[i]) / *SatMass;
|
||
}
|
||
//推进剂转动惯量，与剩余质量成正比
|
||
double GasMtxI[3][3], DryMtxI[3][3], dGasPos[3], dDryPos[3], dGasPosMtx[3][3], dDryPosMtx[3][3], temp1[3][3], temp2[3][3];
|
||
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
GasMtxI[i][j] = (GasMass / InitGasMass) * InitGasMtxI[i][j];
|
||
}
|
||
}
|
||
//卫星平台转动惯量，根据指令选择
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
DryMtxI[i][j] = DryMtxIData[i][3 * Cmd - 3 + j];
|
||
}
|
||
}
|
||
//整星转动惯量计算
|
||
mtxSub((double*)dGasPos, GasMassPos, (double*)SatMassPos, 3, 1);
|
||
mtxSub((double*)dDryPos, DryMassPos, (double*)SatMassPos, 3, 1);
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
dGasPosMtx[i][j] = dGasPos[i] * dGasPos[j] * GasMass;
|
||
dDryPosMtx[i][j] = dDryPos[i] * dDryPos[j] * GasMass;
|
||
}
|
||
}
|
||
mtxAdd((double*)temp1, (double*)GasMtxI, (double*)dGasPosMtx, 3, 3);
|
||
mtxAdd((double*)temp2, (double*)DryMtxI, (double*)dDryPosMtx, 3, 3);
|
||
mtxAdd((double*)SatMtxI, (double*)temp1, (double*)temp2, 3, 3);
|
||
}
|