851 lines
27 KiB
C++
851 lines
27 KiB
C++
|
||
#include "orbit_info.h"
|
||
// #include "TyxkDynamics.h"
|
||
#include "GNCFunction.h"
|
||
|
||
#include "liblog.h"
|
||
#include "json.hpp"
|
||
|
||
|
||
#include <string>
|
||
#include <thread>
|
||
|
||
#include <fstream>
|
||
|
||
#include <math.h>
|
||
#include <time.h>
|
||
// #include <stdio.h>
|
||
#include <stdlib.h>
|
||
|
||
using namespace nlohmann;
|
||
// using namespace Dynamics;
|
||
|
||
|
||
typedef struct __attribute__((packed))
|
||
{
|
||
double Epoch; //历元时间
|
||
uint8_t pkg_Cnt;
|
||
BASIC_CMDER CJB_cmder; //CJB轨道
|
||
BASIC_CMDER Target_cmder; //目标星轨道
|
||
} ORBIT_CMDER;
|
||
|
||
|
||
ORBIT_CMDER orbit_cmder;
|
||
|
||
|
||
|
||
STEP_CMDER_RES s_rescmd = {0};
|
||
|
||
uint8_t set_orbit_flag = 0; //轨道初值赋值标志位
|
||
uint8_t orbit_source_flag = 0; //轨道来源标志:0--使用主星传入轨道;1--使用程序默认轨道
|
||
|
||
|
||
double Epoch_J2000 = 233881200; //历元信息，累计秒
|
||
double posi_J2000[3] = {-41966956.6938834, 4089556.26746777, 92419.0202929388}; //J2000位置信息,m
|
||
double velo_J2000[3] ={-298.1140306366,-3059.51295367263, 54.487742185087}; //J2000速度信息,m/s
|
||
double Q_J2000[4] = {1.0,0.0,0.0,0.0}; //惯性系四元数信息
|
||
|
||
double QuatVVLH[4]= {1.0,0.0,0.0,0.0}; //姿态四元数信息
|
||
|
||
double Tar_posi_J2000[3] = {-41972144.5417670,4040287.04196300, 92381.2956310000};//目标性初始位置
|
||
double Tar_velo_J2000[3] = {-294.436520000000,-3059.96985700000,54.3873050000000};//目标性初始速度
|
||
|
||
double AngularVelocity[3]; //惯性系角速度信息
|
||
|
||
double mjd_2017 = 57754; //2017.1.1 0:0:0 北京时
|
||
//double mjd = 0;
|
||
|
||
double StepSize = 0.05; //动力学递推步长，s
|
||
|
||
|
||
double ThrustForceBody[3], ChaSatMass;
|
||
|
||
void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI);
|
||
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI);
|
||
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval);
|
||
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]);
|
||
void Fun_orbit(double dots[6], double RV[6]);
|
||
void Orbit_Runge_Kutta(double* PosVel, double h);
|
||
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]);
|
||
|
||
/// @brief 轨道信息初始化
|
||
/// @param
|
||
/// @return 0:设置失败; 1:成功
|
||
uint8_t orbit_info_initz(string path)
|
||
{
|
||
json sync_j;
|
||
ifstream fin(path);
|
||
|
||
if(!fin)
|
||
{
|
||
cout<<"udp_sever_init配置文件打开失败,采用默认状态!"<<endl;
|
||
spdlog::warn("函数{0}配置文件打开失败,采用默认状态!",__FUNCTION__);
|
||
|
||
return 0;
|
||
}
|
||
|
||
if(fin.good())
|
||
{
|
||
fin>>sync_j;
|
||
}
|
||
else
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
orbit_source_flag = sync_j["orbit_source_flag"].get<uint8_t>();
|
||
|
||
// printf("orbit_source_flag is:%d",orbit_source_flag);
|
||
StepSize = sync_j["StepSize"].get<double>();
|
||
cout<<"StepSize1 is:"<<StepSize<<endl;
|
||
|
||
Epoch_J2000 = sync_j["Epoch_J2000"].get<double>();
|
||
posi_J2000[0] = sync_j["posi_J2000_X"].get<double>();
|
||
posi_J2000[1] = sync_j["posi_J2000_Y"].get<double>();
|
||
posi_J2000[2] = sync_j["posi_J2000_Z"].get<double>();
|
||
velo_J2000[0] = sync_j["velo_J2000_X"].get<double>();
|
||
velo_J2000[1] = sync_j["velo_J2000_Y"].get<double>();
|
||
velo_J2000[2] = sync_j["velo_J2000_Z"].get<double>();
|
||
|
||
Tar_posi_J2000[0] = sync_j["Tar_posi_J2000_X"].get<double>();
|
||
Tar_posi_J2000[1] = sync_j["Tar_posi_J2000_Y"].get<double>();
|
||
Tar_posi_J2000[2] = sync_j["Tar_posi_J2000_Z"].get<double>();
|
||
Tar_velo_J2000[0] = sync_j["Tar_velo_J2000_X"].get<double>();
|
||
Tar_velo_J2000[1] = sync_j["Tar_velo_J2000_Y"].get<double>();
|
||
Tar_velo_J2000[2] = sync_j["Tar_velo_J2000_Z"].get<double>();
|
||
|
||
fin.close();
|
||
|
||
return 1;
|
||
}
|
||
/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
|
||
/// 仅第一次赋值
|
||
/// @param frame:输入量,轨道信息数组
|
||
// 输出:frame
|
||
// AttiVelECI[3]
|
||
// EulerVVLH[3]
|
||
// AttiMtxVVLH[3][3]
|
||
// MtxECI2VVLH[3][3]
|
||
/// @return 0:设置失败; 1:成功
|
||
uint8_t set_orbit_info(uint8_t *frame,double AttiVelECI[3], double EulerVVLH[3], double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3])
|
||
{
|
||
|
||
double orbInfo[6];
|
||
double QuatECI[4],QuatECI2VVLH[4], QoiX[4][4];
|
||
|
||
if(orbit_source_flag==1) //如果使用自定义轨道则给定定值
|
||
{
|
||
if(!set_orbit_flag)
|
||
{
|
||
spdlog::info("轨道初始化成功！");
|
||
//本体轨道信息
|
||
// Epoch_J2000 = 261130015;// 233881200;
|
||
// posi_J2000[0] = 23580586.0537251;//-41966956.6938834;//s_rescmd.resCmder.posi_x_J2000;
|
||
// posi_J2000[1] = -34621080.4670552;//4089556.26746777;//s_rescmd.resCmder.posi_y_J2000;
|
||
// posi_J2000[2] = 3658341.81666531;//92419.0202929388;//s_rescmd.resCmder.posi_z_J2000;
|
||
// velo_J2000[0] = 2550.44489430168;//-298.1140306366;//s_rescmd.resCmder.velo_x_J2000;
|
||
// velo_J2000[1] = 1733.51288095298;//-3059.51295367263;//s_rescmd.resCmder.velo_y_J2000;
|
||
// velo_J2000[2] = -5.46126675290179;//54.487742185087;//s_rescmd.resCmder.velo_z_J2000;
|
||
/* 七公里 */
|
||
/*Epoch_J2000 = 261151615;
|
||
posi_J2000[0] = 34676281.2753897;
|
||
posi_J2000[1] = 24076270.7556943;
|
||
posi_J2000[2] = -109962.795723338;
|
||
velo_J2000[0] = -1737.82925849319;
|
||
velo_J2000[1] = 2518.86637867070;
|
||
velo_J2000[2] = -267.003526384092;*/
|
||
/* 1公里 */
|
||
/*Epoch_J2000 = 261162415;
|
||
posi_J2000[0] = 7615206.74657380;
|
||
posi_J2000[1] = 41520735.9749386;
|
||
posi_J2000[2] = -2677030.64312120;
|
||
velo_J2000[0] = -3011.20121409852;
|
||
velo_J2000[1] = 545.623103173396;
|
||
velo_J2000[2] = -183.096712053141;*/
|
||
/* 150m */
|
||
/*Epoch_J2000 = 261162415;
|
||
posi_J2000[0] = 7615354.12442575;
|
||
posi_J2000[1] = 41520709.5213564;
|
||
posi_J2000[2] = -2677021.70014955;
|
||
velo_J2000[0] = -3011.19925678085;
|
||
velo_J2000[1] = 545.633775145026;
|
||
velo_J2000[2] = -183.097400123677;*/
|
||
|
||
|
||
//目标星轨道参数
|
||
// Tar_posi_J2000[0] = 23621367.9190000;//-41972144.5417670;
|
||
// Tar_posi_J2000[1] = -34592141.8670000;// 4040287.04196300;
|
||
// Tar_posi_J2000[2] = 3657159.91500000;// 92381.2956310000;
|
||
// Tar_velo_J2000[0] = 2548.34575500000;// -294.436520000000;
|
||
// Tar_velo_J2000[1] = 1736.43994900000;// -3059.96985700000;
|
||
// Tar_velo_J2000[2] = -5.86735000000000;// 54.3873050000000;
|
||
/* 七公里 */
|
||
/*Tar_posi_J2000[0] = 34672232.3889290;
|
||
Tar_posi_J2000[1] = 24081737.4807088;
|
||
Tar_posi_J2000[2] = -112563.071870978;
|
||
Tar_velo_J2000[0] = -1738.23933197031;
|
||
Tar_velo_J2000[1] = 2518.56390864701;
|
||
Tar_velo_J2000[2] = -267.000903391630;*/
|
||
/* 1公里 */
|
||
/*Tar_posi_J2000[0] = 7616189.26558680;
|
||
Tar_posi_J2000[1] = 41520559.6177237;
|
||
Tar_posi_J2000[2] = -2676971.02331015;
|
||
Tar_velo_J2000[0] = -3011.18816531403;
|
||
Tar_velo_J2000[1] = 545.694249650931;
|
||
Tar_velo_J2000[2] = -183.101299190051;*/
|
||
/* 150m */
|
||
/*Tar_posi_J2000[0] = 7615206.74657380;
|
||
Tar_posi_J2000[1] = 41520735.9749386;
|
||
Tar_posi_J2000[2] = -2677030.64312120;
|
||
Tar_velo_J2000[0] = -3011.20121409852;
|
||
Tar_velo_J2000[1] = 545.623103173396;
|
||
Tar_velo_J2000[2] = -183.096712053141;*/
|
||
|
||
|
||
//姿态角初值
|
||
EulerVVLH[0] = 0.0;
|
||
EulerVVLH[1] = 0.0;
|
||
EulerVVLH[2] = 0.0;
|
||
|
||
AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
|
||
AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
|
||
AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
|
||
|
||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||
|
||
MtxJtoOGet(MtxECI2VVLH, orbInfo);
|
||
|
||
|
||
A2q(QuatECI2VVLH, MtxECI2VVLH);
|
||
qleft(QoiX, QuatECI2VVLH);
|
||
mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
|
||
Q_J2000[0] = QuatECI[0];
|
||
Q_J2000[1] = QuatECI[1];
|
||
Q_J2000[2] = QuatECI[2];
|
||
Q_J2000[3] = QuatECI[3];
|
||
|
||
//角速度初值
|
||
AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||
AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||
AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||
AttiVelECI[0] = AngularVelocity[0];
|
||
AttiVelECI[1] = AngularVelocity[1];
|
||
AttiVelECI[2] = AngularVelocity[2];
|
||
|
||
set_orbit_flag = 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
//否则使用主星输入轨道
|
||
else
|
||
{
|
||
if(!set_orbit_flag)
|
||
{
|
||
memcpy((uint8_t *)&orbit_cmder.Epoch,frame,sizeof(ORBIT_CMDER));
|
||
set_orbit_flag = 1;
|
||
|
||
Epoch_J2000 = orbit_cmder.Epoch;
|
||
posi_J2000[0] = orbit_cmder.CJB_cmder.posi_x_J2000;
|
||
posi_J2000[1] = orbit_cmder.CJB_cmder.posi_y_J2000;
|
||
posi_J2000[2] = orbit_cmder.CJB_cmder.posi_z_J2000;
|
||
|
||
velo_J2000[0] = orbit_cmder.CJB_cmder.velo_x_J2000;
|
||
velo_J2000[1] = orbit_cmder.CJB_cmder.velo_y_J2000;
|
||
velo_J2000[2] = orbit_cmder.CJB_cmder.velo_z_J2000;
|
||
|
||
Q_J2000[0] = orbit_cmder.CJB_cmder.q0_J2000;
|
||
Q_J2000[1] = orbit_cmder.CJB_cmder.q1_J2000;
|
||
Q_J2000[2] = orbit_cmder.CJB_cmder.q2_J2000;
|
||
Q_J2000[3] = orbit_cmder.CJB_cmder.q3_J2000;
|
||
|
||
//目标性轨道参数
|
||
Tar_posi_J2000[0] = orbit_cmder.Target_cmder.posi_x_J2000; //-41972144.5417670;
|
||
Tar_posi_J2000[1] = orbit_cmder.Target_cmder.posi_y_J2000;//4040287.04196300;
|
||
Tar_posi_J2000[2] = orbit_cmder.Target_cmder.posi_z_J2000;//92381.2956310000;
|
||
Tar_velo_J2000[0] = orbit_cmder.Target_cmder.velo_x_J2000;//-294.436520000000;
|
||
Tar_velo_J2000[1] = orbit_cmder.Target_cmder.velo_y_J2000;//-3059.96985700000;
|
||
Tar_velo_J2000[2] = orbit_cmder.Target_cmder.velo_z_J2000;//54.3873050000000;
|
||
|
||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||
|
||
MtxJtoOGet(MtxECI2VVLH, orbInfo);
|
||
|
||
EulerVVLH[0] = 0.0;
|
||
EulerVVLH[1] = 0.0;
|
||
EulerVVLH[2] = 0.0;
|
||
|
||
AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
|
||
AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
|
||
AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
|
||
|
||
|
||
AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;
|
||
AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;
|
||
AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;
|
||
AttiVelECI[0] = AngularVelocity[0];
|
||
AttiVelECI[1] = AngularVelocity[1];
|
||
AttiVelECI[2] = AngularVelocity[2];
|
||
|
||
spdlog::info("函数{0}接收到的轨道历元为:{1:.3f},节点号为:{2:2d}, \
|
||
位置为:{3:.3f},{4:.3f},{5:.3f}, \
|
||
速度为:{6:.3f},{7:.3f},{8:.3f}, \
|
||
四元数为:{9:.3f},{10:.3f},{11:.3f},{12:.3f}"
|
||
,__FUNCTION__,Epoch_J2000,
|
||
orbit_cmder.CJB_cmder.pack_id,
|
||
posi_J2000[0],posi_J2000[1],posi_J2000[2],
|
||
velo_J2000[0],velo_J2000[1],velo_J2000[2],
|
||
Q_J2000[0],Q_J2000[1],Q_J2000[2],Q_J2000[3]);
|
||
|
||
spdlog::info("函数{0}接收到的目标星轨道历元为:{1:.3f},节点号为:{2:2d}, \
|
||
位置为:{3:.3f},{4:.3f},{5:.3f}, \
|
||
速度为:{6:.3f},{7:.3f},{8:.3f}"
|
||
,__FUNCTION__,Epoch_J2000,
|
||
orbit_cmder.Target_cmder.pack_id,
|
||
Tar_posi_J2000[0],Tar_posi_J2000[1],Tar_posi_J2000[2],
|
||
Tar_velo_J2000[0],Tar_velo_J2000[1],Tar_velo_J2000[2]);
|
||
|
||
}
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
|
||
/// 仅第一次赋值,后续根据初始赋值递推
|
||
/// @param frame:输入量,轨道信息数组
|
||
/// @return 0:设置失败; 1:成功
|
||
/// /* 输入量 */
|
||
/* SatMtxI 卫星转动惯量矩阵 */
|
||
/* Tq 总力矩 */
|
||
/* WheelAM 动量轮角动量 */
|
||
//SatMass 卫星质量 从微推模型获取
|
||
//SatMtxI 转动惯量 从微推模型获取
|
||
/* 输出量 */
|
||
/* AttiVelECI 角速度W^bi */
|
||
/*orbInfo[6]*/
|
||
/* double Tar_orbInfo[6] */
|
||
// EulerVVLH[3] 欧拉角
|
||
// AttiMtxVVLH[3][3] 姿态转移矩阵
|
||
// MtxECI2VVLH 旋转矩阵
|
||
|
||
uint8_t generate_orbit_info(double orbInfo[6], double Tar_orbInfo[6], double AttiVelECI[3], double EulerVVLH[3],
|
||
double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3],
|
||
double* mjd, double* Epoch2017,
|
||
double SatMass, double SatMtxI[3][3],
|
||
double ThrustForce[3], double Tq[3], double WheelAM[3])
|
||
{
|
||
double dt_Orb;
|
||
double h, AttiVelVVLH[3], QuatECI[4], OrbitVel[3], tInterval, QuatECI2VVLH[4], QoiX[4][4], PosNorm, VelNorm, W_Orbit;
|
||
double mjd0, pv0[6], Tar_pv0[6];
|
||
double pv[6], Tar_pv[6];
|
||
int NUM = 10;
|
||
|
||
|
||
dt_Orb = StepSize;
|
||
h = StepSize / NUM;
|
||
tInterval = StepSize;
|
||
|
||
mtxCpy((double*)ThrustForceBody, (double*)ThrustForce, 3, 1);
|
||
ChaSatMass = SatMass;
|
||
|
||
mjd0 = Epoch_J2000 / 86400.0 - 8.0/24.0 + mjd_2017; //Epoch_J2000更名为Epoch_BJT
|
||
pv0[0] = posi_J2000[0];
|
||
pv0[1] = posi_J2000[1];
|
||
pv0[2] = posi_J2000[2];
|
||
pv0[3] = velo_J2000[0];
|
||
pv0[4] = velo_J2000[1];
|
||
pv0[5] = velo_J2000[2];
|
||
*mjd = mjd0 + dt_Orb / 86400.0;
|
||
//追踪星轨道动力学递推
|
||
ORBIT_J6(dt_Orb, h, pv0, pv);
|
||
MtxJtoOGet(MtxECI2VVLH, pv);
|
||
mtxCpy((double*)orbInfo, (double*)pv, 6, 1);
|
||
|
||
//目标星轨道动力学递推
|
||
ThrustForceBody[0] = 0.0; ThrustForceBody[1] = 0.0; ThrustForceBody[2] = 0.0;
|
||
Tar_pv0[0] = Tar_posi_J2000[0];
|
||
Tar_pv0[1] = Tar_posi_J2000[1];
|
||
Tar_pv0[2] = Tar_posi_J2000[2];
|
||
Tar_pv0[3] = Tar_velo_J2000[0];
|
||
Tar_pv0[4] = Tar_velo_J2000[1];
|
||
Tar_pv0[5] = Tar_velo_J2000[2];
|
||
ORBIT_J6(dt_Orb, h, Tar_pv0, Tar_pv);
|
||
mtxCpy((double*)Tar_orbInfo, (double*)Tar_pv, 6, 1);
|
||
|
||
//相对位置
|
||
double DeltaPos[3],DeltaPosVVLH[3];
|
||
DeltaPos[0] = pv0[0] - Tar_pv0[0];
|
||
DeltaPos[1] = pv0[1] - Tar_pv0[1];
|
||
DeltaPos[2] = pv0[2] - Tar_pv0[2];
|
||
mtxMtp((double*)DeltaPosVVLH, (double*)MtxECI2VVLH, 3, 3, (double*)DeltaPos, 3, 1);
|
||
|
||
// 姿态动力学运动学递推
|
||
PosNorm = norm(pv, 3);
|
||
VelNorm = norm(&pv[3], 3);
|
||
W_Orbit = VelNorm / PosNorm;
|
||
OrbitVel[0] = 0.0;
|
||
OrbitVel[1] = -1 * W_Orbit;
|
||
OrbitVel[2] = 0.0;
|
||
for (int i = 0; i < NUM; ++i) {
|
||
|
||
Runge_Kutta((double*)AngularVelocity, (double)h, (double*)Tq, (double*)WheelAM, (double*)SatMtxI);
|
||
mtxCpy((double*)AttiVelECI, (double*)AngularVelocity, 3, 1);
|
||
}
|
||
KinenaticModel(AttiVelVVLH, AttiMtxVVLH, EulerVVLH, QuatVVLH, AttiVelECI, OrbitVel, tInterval);
|
||
|
||
Epoch_J2000 = Epoch_J2000 + dt_Orb;
|
||
posi_J2000[0] = pv[0];
|
||
posi_J2000[1] = pv[1];
|
||
posi_J2000[2] = pv[2];
|
||
velo_J2000[0] = pv[3];
|
||
velo_J2000[1] = pv[4];
|
||
velo_J2000[2] = pv[5];
|
||
|
||
Tar_posi_J2000[0] = Tar_pv[0];
|
||
Tar_posi_J2000[1] = Tar_pv[1];
|
||
Tar_posi_J2000[2] = Tar_pv[2];
|
||
Tar_velo_J2000[0] = Tar_pv[3];
|
||
Tar_velo_J2000[1] = Tar_pv[4];
|
||
Tar_velo_J2000[2] = Tar_pv[5];
|
||
|
||
A2q(QuatECI2VVLH, MtxECI2VVLH);
|
||
qleft(QoiX, QuatECI2VVLH);
|
||
mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
|
||
Q_J2000[0] = QuatECI[0];
|
||
Q_J2000[1] = QuatECI[1];
|
||
Q_J2000[2] = QuatECI[2];
|
||
Q_J2000[3] = QuatECI[3];
|
||
|
||
*Epoch2017 =Epoch_J2000;
|
||
|
||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||
|
||
Tar_orbInfo[0] = Tar_posi_J2000[0]; Tar_orbInfo[1] = Tar_posi_J2000[1]; Tar_orbInfo[2] = Tar_posi_J2000[2];
|
||
Tar_orbInfo[3] = Tar_velo_J2000[0]; Tar_orbInfo[4] = Tar_velo_J2000[1]; Tar_orbInfo[5] = Tar_velo_J2000[2];
|
||
|
||
spdlog::info("函数{0}的QuatECI为:{1:.1f},{2:.5f},{3:.5f},{4:.5f},AttiVelECI为:{5:.5f},{6:.5f},{7:.5f},{8:.5f},{9:.5f},{10:.5f}",
|
||
__FUNCTION__,
|
||
QuatECI[0], QuatECI[1], QuatECI[2], QuatECI[3],
|
||
AttiVelECI[0], AttiVelECI[1], AttiVelECI[2],
|
||
DeltaPosVVLH[0],DeltaPosVVLH[1],DeltaPosVVLH[2]);
|
||
|
||
spdlog::info("函数{0}轨道信息:{1:.1f},{2:.5f},{3:.5f},{4:.5f},{5:.5f},{6:.5f},{7:.5f},\
|
||
{8:.5f},{9:.5f},{10:.5f},{11:.5f},{12:.5f},{13:.5f},{14:.5f},{15:.5f}",
|
||
__FUNCTION__,
|
||
*mjd,
|
||
posi_J2000[0],
|
||
posi_J2000[1],
|
||
posi_J2000[2],
|
||
velo_J2000[0],
|
||
velo_J2000[1],
|
||
velo_J2000[2],
|
||
Q_J2000[0],
|
||
Q_J2000[1],
|
||
Q_J2000[2],
|
||
Q_J2000[3],
|
||
AttiVelECI[0],
|
||
AttiVelECI[1],
|
||
AttiVelECI[2],
|
||
Epoch_J2000);
|
||
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取轨道信息;该函数供通信模块调用
|
||
/// @param frame:输出量,轨道信息数组
|
||
/// @return 1:成功
|
||
uint8_t get_orbit_info(uint8_t *frame,uint8_t id)
|
||
{
|
||
|
||
s_rescmd.Epoch = Epoch_J2000 ;
|
||
s_rescmd.resCmder.pack_id = id;
|
||
s_rescmd.resCmder.posi_x_J2000 = posi_J2000[0] ;
|
||
s_rescmd.resCmder.posi_y_J2000 = posi_J2000[1] ;
|
||
s_rescmd.resCmder.posi_z_J2000 = posi_J2000[2] ;
|
||
|
||
s_rescmd.resCmder.velo_x_J2000 = velo_J2000[0] ;
|
||
s_rescmd.resCmder.velo_y_J2000 = velo_J2000[1] ;
|
||
s_rescmd.resCmder.velo_z_J2000 = velo_J2000[2] ;
|
||
|
||
s_rescmd.resCmder.q0_J2000 = Q_J2000[0] ;
|
||
s_rescmd.resCmder.q1_J2000 = Q_J2000[1] ;
|
||
s_rescmd.resCmder.q2_J2000 = Q_J2000[2] ;
|
||
s_rescmd.resCmder.q3_J2000 = Q_J2000[3] ;
|
||
|
||
spdlog::info("函数{0}反馈的轨道历元为:{1:.3f},节点号为:{2:2d},位置为:{3:.3f},速度为:{4:.3f},姿态为:{5:.3f}"
|
||
,__FUNCTION__,Epoch_J2000,
|
||
s_rescmd.resCmder.pack_id,
|
||
posi_J2000[0],
|
||
velo_J2000[0],
|
||
Q_J2000[0]);
|
||
|
||
memcpy(frame,(uint8_t *)&s_rescmd.Epoch,sizeof(STEP_CMDER_RES));
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取J2000系历元信息
|
||
/// @param Epoch:返回的历元数据
|
||
/// @return 0:获取失败; 1:成功
|
||
uint8_t get_orbit_epoic(double *Epoch)
|
||
{
|
||
*Epoch = Epoch_J2000;
|
||
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取mjd
|
||
/// @param Epoch:返回的位置数据
|
||
/// @return 0:获取失败; 1:成功
|
||
//double get_orbit_mjd(void)
|
||
//{
|
||
// return mjd;
|
||
//}
|
||
|
||
|
||
/// @brief 获取J2000系位置信息
|
||
/// @param Epoch:返回的位置数据
|
||
/// @return 0:获取失败; 1:成功
|
||
uint8_t get_orbit_posi(double *posi)
|
||
{
|
||
posi[0] = posi_J2000[0];
|
||
posi[1] = posi_J2000[1];
|
||
posi[2] = posi_J2000[2];
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取J2000系速度信息
|
||
/// @param Epoch:返回的速度数据
|
||
/// @return 0:获取失败; 1:成功
|
||
uint8_t get_orbit_velo(double *velo)
|
||
{
|
||
velo[0] = velo_J2000[0];
|
||
velo[1] = velo_J2000[1];
|
||
velo[2] = velo_J2000[2];
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取J2000系姿态四元数信息
|
||
/// @param Epoch:返回的姿态四元数数据
|
||
/// @return 0:获取失败; 1:成功
|
||
uint8_t get_orbit_Q(double *Q)
|
||
{
|
||
Q[0] = Q_J2000[0];
|
||
Q[1] = Q_J2000[1];
|
||
Q[2] = Q_J2000[2];
|
||
Q[3] = Q_J2000[3];
|
||
|
||
return 1;
|
||
}
|
||
|
||
/// @brief 获取轨道赋值标志位
|
||
/// @param
|
||
/// @return 轨道赋值标志位
|
||
uint8_t get_orbit_flag(void)
|
||
{
|
||
return set_orbit_flag;
|
||
}
|
||
|
||
/* 动力学模型 */
|
||
void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI) {
|
||
|
||
/* 动力学模型模块 */
|
||
/* 输入量 */
|
||
/* w 角速度初值 */
|
||
/* Tq 力矩 */
|
||
/* WheelAM 动量轮角动量 */
|
||
/* 输出量 */
|
||
/* AttiVelDotECI 角加速度 */
|
||
|
||
double CrossMatrixW[3][3], IW[3], Hs[3];
|
||
double temp1[3], temp2[3], temp3[3][3], temp4[3];
|
||
|
||
mtxMtp((double*)IW, (double*)SatMtxI, 3, 3, w, 3, 1);
|
||
mtxAdd((double*)Hs, (double*)IW, (double*)WheelAM, 3, 1);
|
||
CrossMatrixGet(CrossMatrixW, w);
|
||
mtxMtp((double*)temp1, (double*)CrossMatrixW, 3, 3, (double*)Hs, 3, 1);
|
||
mtxSub((double*)temp2, (double*)Tq, (double*)temp1, 3, 1);
|
||
mtxInv((double*)temp3, (double*)SatMtxI, 3);
|
||
mtxMtp((double*)AttiVelDotECI, (double*)temp3, 3, 3, (double*)temp2, 3, 1);
|
||
}
|
||
|
||
|
||
/* 四阶龙格库塔算法 */
|
||
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI) {
|
||
|
||
/* 四阶龙格库塔算法模块 */
|
||
/* 输入量 */
|
||
/* AngularVelocity 角速度初值 */
|
||
/* h 迭代步长 */
|
||
/* Tq 力矩 */
|
||
/* WheelAM 动量轮角动量 */
|
||
/* 输出量 */
|
||
/* AngularVelocity 角速度终值 */
|
||
|
||
double work[3], AttiVelDotECI[3], k0[3], k1[3], k2[3], k3[3];
|
||
int j;
|
||
|
||
Dynamic_Model((double*)AttiVelDotECI, AngularVelocity, Tq, WheelAM, SatMtxI);//计算k1
|
||
for (j = 0; j < 3; ++j)
|
||
k0[j] = h * AttiVelDotECI[j];
|
||
for (j = 0; j < 3; ++j)
|
||
work[j] = *(AngularVelocity + j) + 0.5 * k0[j];//用数组work存储w的变化量
|
||
|
||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k2
|
||
for (j = 0; j < 3; ++j)
|
||
k1[j] = h * AttiVelDotECI[j];
|
||
for (j = 0; j < 3; ++j)
|
||
work[j] = *(AngularVelocity + j) + 0.5 * k1[j];//用数组work存储w的变化量
|
||
|
||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k3
|
||
for (j = 0; j < 3; ++j)
|
||
k2[j] = h * AttiVelDotECI[j];
|
||
for (j = 0; j < 3; ++j)
|
||
work[j] = *(AngularVelocity + j) + k2[j];//用数组work存储w的变化量
|
||
|
||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k4
|
||
for (j = 0; j < 3; ++j)
|
||
k3[j] = h * AttiVelDotECI[j];
|
||
for (j = 0; j < 3; ++j)
|
||
work[j] = *(AngularVelocity + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6;
|
||
mtxCpy((double*)AngularVelocity, (double*)work, 3, 1);
|
||
|
||
}
|
||
|
||
|
||
/* 运动学更新模拟 */
|
||
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval) {
|
||
|
||
/* 运动学更新模拟模块 */
|
||
/* 输入量 */
|
||
/* AttiVelECI 姿态角速度Wbi */
|
||
/* OrbitVel 轨道角速度Woi */
|
||
/* 输出量 */
|
||
/* AttiVelVVLH 姿态角速度Wbo */
|
||
/* AttiMtxVVLH 姿态矩阵Abo */
|
||
/* QuatVVLH 姿态四元数qbo */
|
||
|
||
double Abo[3][3], Wo[3], woi[3], CrossVelVVLH[4][4], temp[4][4], qbo[4], Rc, ii;
|
||
|
||
TransQ(Abo, QuatVVLH);
|
||
mtxMtp((double*)woi, (double*)Abo, 3, 3, (double*)OrbitVel, 3, 1);
|
||
AttiVelVVLH[0] = AttiVelECI[0] - woi[0];
|
||
AttiVelVVLH[1] = AttiVelECI[1] - woi[1];
|
||
AttiVelVVLH[2] = AttiVelECI[2] - woi[2];
|
||
|
||
CrossVelVVLH[0][0] = 0.0; CrossVelVVLH[0][1] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[0][2] = -0.5 * AttiVelVVLH[1]; CrossVelVVLH[0][3] = -0.5 * AttiVelVVLH[2];
|
||
CrossVelVVLH[1][0] = 0.5 * AttiVelVVLH[0]; CrossVelVVLH[1][1] = 0.0; CrossVelVVLH[1][2] = 0.5 * AttiVelVVLH[2]; CrossVelVVLH[1][3] = -0.5 * AttiVelVVLH[1];
|
||
CrossVelVVLH[2][0] = 0.5 * AttiVelVVLH[1]; CrossVelVVLH[2][1] = -0.5 * AttiVelVVLH[2]; CrossVelVVLH[2][2] = 0.0; CrossVelVVLH[2][3] = 0.5 * AttiVelVVLH[0];
|
||
CrossVelVVLH[3][0] = 0.5 * AttiVelVVLH[2]; CrossVelVVLH[3][1] = 0.5 * AttiVelVVLH[1]; CrossVelVVLH[3][2] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[3][3] = 0.0;
|
||
mtxMtp((double*)temp, (double*)CrossVelVVLH, 4, 4, &tInterval, 1, 1);
|
||
for (int i = 0; i < 4; i++) {
|
||
temp[i][i] += 1.0; /* temp = eye(4,4)+tInterval*0.5*(wbo叉乘矩阵） */
|
||
}
|
||
mtxCpy((double*)qbo, (double*)QuatVVLH, 4, 1);
|
||
mtxMtp((double*)QuatVVLH, (double*)temp, 4, 4, (double*)qbo, 4, 1); /* qbo=qbo(上一时刻)+tInterval*0.5*(wbo叉乘矩阵）*qbo 运动学方程 */
|
||
Rc = 1.0 / norm(QuatVVLH, 4);
|
||
mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &Rc, 1, 1);
|
||
ii = -1.0;
|
||
if (QuatVVLH[0] < 0) {
|
||
mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &ii, 1, 1);
|
||
}
|
||
|
||
TransQ(AttiMtxVVLH, QuatVVLH);
|
||
qua2euler(EulerVVLH, QuatVVLH, qbo);
|
||
}
|
||
|
||
|
||
/* 计算地球引力 */
|
||
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]) {
|
||
/*
|
||
输入:地固系下的卫星位置矢量,m
|
||
输出:地固系下的卫星加速度矢量，m/s2
|
||
*/
|
||
double GM = 398600.4418e9; //单位：m3/s3
|
||
double Re = 6378.137e3; //单位：m
|
||
double J2 = 1.08262617385222e-3;
|
||
double J3 = -2.53241051856772e-6;
|
||
double J4 = -1.61989759991697e-6;
|
||
double J5 = -2.27753590730836e-7;
|
||
double J6 = 5.40666576283813e-7;
|
||
|
||
double rx = R_ECEF[0];
|
||
double ry = R_ECEF[1];
|
||
double rz = R_ECEF[2];
|
||
double r = norm(R_ECEF, 3);
|
||
|
||
// 中心引力加速度
|
||
double Acc_TB[3];
|
||
double r3 = pow(r, 3);
|
||
for (int i = 0; i < 3; i++) {
|
||
Acc_TB[i] = -1.0 * (GM / r3) * R_ECEF[i];
|
||
}
|
||
|
||
// J2摄动加速度
|
||
double r5 = pow(r, 5);
|
||
double Re2 = pow(Re, 2);
|
||
double k0 = -1.0 * (3.0 * GM * J2 * Re2) / (2 * r5);
|
||
double k1 = 5.0 * rz * rz / (r * r);
|
||
double Acc_J2[3];
|
||
Acc_J2[0] = k0 * (1 - k1) * rx;
|
||
Acc_J2[1] = k0 * (1 - k1) * ry;
|
||
Acc_J2[2] = k0 * (3 - k1) * rz;
|
||
|
||
// J3摄动加速度
|
||
double r7 = pow(r, 7);
|
||
double Re3 = pow(Re, 3);
|
||
double k01 = -1.0 * (5.0 * J3 * GM * Re3) / (2.0 * r7);
|
||
double k11 = 3.0 * rz - 7.0 * rz * rz * rz / (r * r);
|
||
double k21 = 6.0 * rz * rz - 7.0 * rz * rz * rz * rz / (r * r) - 3.0 / 5.0 * r * r;
|
||
double Acc_J3[3];
|
||
Acc_J3[0] = k01 * rx * k11;
|
||
Acc_J3[1] = k01 * ry * k11;
|
||
Acc_J3[2] = k01 * k21;
|
||
|
||
// J4摄动加速度
|
||
//double r7 = pow(r, 7);
|
||
double Re4 = pow(Re, 4);
|
||
|
||
double k02 = (15.0 * J4 * GM * Re4) / (8 * r7);
|
||
double k12 = 1.0 - 14.0 * rz * rz / (r * r);
|
||
double k22 = 5.0 - 70.0 * rz * rz / (3 * r * r);
|
||
double k32 = 21.0 * rz * rz * rz * rz / (r * r * r * r);
|
||
double Acc_J4[3];
|
||
Acc_J4[0] = k02 * (k12 + k32) * rx;
|
||
Acc_J4[1] = k02 * (k12 + k32) * ry;
|
||
Acc_J4[2] = k02 * (k22 + k32) * rz;
|
||
|
||
// J5摄动加速度
|
||
double r9 = pow(r, 9);
|
||
double Re5 = pow(Re, 5);
|
||
double k03 = (3.0 * J5 * GM * Re5 * rz) / (8 * r9);
|
||
double k001 = rz * rz / (r * r);
|
||
double k002 = pow(k001, 2);
|
||
double k003 = pow(k001, 3);
|
||
double k13 = 35.0 - 210.0 * k001 + 231.0 * k002;
|
||
double k23 = 105.0 - 315.0 * k001 + 231.0 * k002;
|
||
double Acc_J5[3];
|
||
Acc_J5[0] = k03 * k13 * rx;
|
||
Acc_J5[1] = k03 * k13 * ry;
|
||
Acc_J5[2] = k03 * k23 * rz;
|
||
Acc_J5[2] = Acc_J5[2] - 15.0 * J5 * GM * Re5 / (8 * r7);
|
||
|
||
// J6摄动加速度
|
||
double Re6 = pow(Re, 6);
|
||
double k04 = -1.0 * (J6 * GM * Re6) / (16.0 * r9);
|
||
double k14 = 35.0 - 945.0 * k001 + 3465.0 * k002 - 3003.0 * k003;
|
||
double k24 = 245.0 - 2205.0 * k001 + 4851.0 * k002 - 3003.0 * k003;
|
||
double Acc_J6[3];
|
||
Acc_J6[0] = k04 * k14 * rx;
|
||
Acc_J6[1] = k04 * k14 * rz;//确定是rz吗?
|
||
Acc_J6[2] = k04 * k24 * rz;
|
||
|
||
// 总加速度
|
||
/*for (int i = 0; i < 3; i++) {
|
||
Acc_Earth[i] = Acc_TB[i] + Acc_J2[i] + Acc_J3[i] + Acc_J4[i] + Acc_J5[i] + Acc_J6[i];
|
||
}
|
||
*/
|
||
Acc_Earth[0] = Acc_TB[0] + Acc_J2[0] + Acc_J3[0] + Acc_J4[0] + Acc_J5[0] + Acc_J6[0];
|
||
Acc_Earth[1] = Acc_TB[1] + Acc_J2[1] + Acc_J3[1] + Acc_J4[1] + Acc_J5[1] + Acc_J6[1];
|
||
Acc_Earth[2] = Acc_TB[2] + Acc_J2[2] + Acc_J3[2] + Acc_J4[2] + Acc_J5[2] + Acc_J6[2];
|
||
|
||
}
|
||
|
||
|
||
/* 轨道动力学 */
|
||
void Fun_orbit(double dots[6], double RV[6]) {
|
||
|
||
double Acc_Earth[3], SatPosECI[3], MtxECI2Body[3][3], MtxBody2ECI[3][3], ThrustForceECI[3], Acc_Thrust_ECI[3];
|
||
|
||
SatPosECI[0] = RV[0];
|
||
SatPosECI[1] = RV[1];
|
||
SatPosECI[2] = RV[2];
|
||
Fun_AccEarth(Acc_Earth, SatPosECI);
|
||
|
||
// Acc_Thrust_ECI = MtxBody2ECI* 本体系下三轴推力 / 整星质量
|
||
TransQ(MtxECI2Body, Q_J2000);
|
||
mtxInv((double*)MtxBody2ECI, (double*)MtxECI2Body, 3);
|
||
mtxMtp((double*)ThrustForceECI, (double*)MtxBody2ECI, 3, 3, (double*)ThrustForceBody, 3, 1);
|
||
|
||
dots[0] = RV[3];
|
||
dots[1] = RV[4];
|
||
dots[2] = RV[5];
|
||
dots[3] = Acc_Earth[0] + ThrustForceECI[0] / ChaSatMass;
|
||
dots[4] = Acc_Earth[1] + ThrustForceECI[1] / ChaSatMass;
|
||
dots[5] = Acc_Earth[2] + ThrustForceECI[2] / ChaSatMass;
|
||
|
||
}
|
||
|
||
|
||
/* 轨道的四阶龙格库塔算法 */
|
||
void Orbit_Runge_Kutta(double* PosVel, double h) {
|
||
/* 输入量 */
|
||
/* RV 惯性系下位置速度初始值 */
|
||
/* h 迭代步长 */
|
||
|
||
/* 输出量 */
|
||
/* RV 迭代出的位置速度*/
|
||
|
||
double work[6], dots[6], k0[6], k1[6], k2[6], k3[6];
|
||
int j;
|
||
|
||
Fun_orbit((double*)dots, PosVel);//计算k1
|
||
for (j = 0; j < 6; ++j)
|
||
k0[j] = h * dots[j];
|
||
for (j = 0; j < 6; ++j)
|
||
work[j] = *(PosVel + j) + 0.5 * k0[j];//用数组work存储w的变化量
|
||
|
||
Fun_orbit((double*)dots, (double*)work);//计算k2
|
||
for (j = 0; j < 6; ++j)
|
||
k1[j] = h * dots[j];
|
||
for (j = 0; j < 6; ++j)
|
||
work[j] = *(PosVel + j) + 0.5 * k1[j];//用数组work存储w的变化量
|
||
|
||
Fun_orbit((double*)dots, (double*)work);//计算k3
|
||
for (j = 0; j < 6; ++j)
|
||
k2[j] = h * dots[j];
|
||
for (j = 0; j < 6; ++j)
|
||
work[j] = *(PosVel + j) + k2[j];//用数组work存储w的变化量
|
||
|
||
Fun_orbit((double*)dots, (double*)work);//计算k4
|
||
for (j = 0; j < 6; ++j)
|
||
k3[j] = h * dots[j];
|
||
for (j = 0; j < 6; ++j)
|
||
work[j] = *(PosVel + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6.0;
|
||
mtxCpy((double*)PosVel, (double*)work, 6, 1);
|
||
}
|
||
|
||
|
||
/* J6摄动轨道递推 */
|
||
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]) {
|
||
|
||
double state[6];
|
||
double t0 = 0;
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
state[i] = RV0[i];
|
||
}
|
||
|
||
int steps = (int)((tf - t0) / dt) + 1;
|
||
|
||
for (int i = 1; i < steps; i++) {
|
||
Orbit_Runge_Kutta((double*)state, dt);
|
||
}
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
RV[i] = state[i];
|
||
}
|
||
}
|
||
|
||
|
||
uint8_t get_orbit_source_flag(void)
|
||
{
|
||
return orbit_source_flag;
|
||
}
|
||
|
||
|
||
|