254 lines
9.3 KiB
C++
254 lines
9.3 KiB
C++
|
#include "sensor_star.h"
|
|||
|
|
#include "orbit_info.h"
|
|||
|
|
#include "GNCFunction.h"
|
|||
|
|
|
|||
|
|
#include "libconvert.h"
|
|||
|
|
#include "liblog.h"
|
|||
|
|
|
|||
|
|
#include <math.h>
|
|||
|
|
// #include <time.h>
|
|||
|
|
// #include <stdio.h>
|
|||
|
|
// #include <stdlib.h>
|
|||
|
|
|
|||
|
|
double Star1QuatInstall[4] = { 0.454717549603582 ,0.541041969225232 ,0.454793640506779 ,-0.541911692228529 };
|
|||
|
|
double Star2QuatInstall[4] = { 0.454717962112787 ,-0.541835255189012 ,0.453848239109489 ,0.541911346092208 };
|
|||
|
|
|
|||
|
|
void GetStar1Meas(double MeasStar1[4], int* Star1Flag, double Star1QuatInstall[4], double QuatECI[4], double AttiVelECI[3]);
|
|||
|
|
void GetStar2Meas(double MeasStar2[4], int* Star2Flag, double Star2QuatInstall[4], double QuatECI[4], double AttiVelECI[3]);
|
|||
|
|
|
|||
|
|
/// @brief 生成星敏1遥测帧
|
|||
|
|
/// @param frame:生成的遥测帧数据
|
|||
|
|
/// @return
|
|||
|
|
uint8_t generate_star_1_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3])
|
|||
|
|
{
|
|||
|
|
CAK_TLM_1_STRUCT star;
|
|||
|
|
memset(frame,0,CAK_TLM_1_Info_Length);
|
|||
|
|
memset((uint8_t *)&star.FRAME_HEAD,0,CAK_TLM_1_Info_Length);
|
|||
|
|
|
|||
|
|
|
|||
|
|
double MeasStar1[4]; int Star1Flag = 0;
|
|||
|
|
GetStar1Meas(MeasStar1, &Star1Flag, Star1QuatInstall, QuatECI, AttiVelECI);
|
|||
|
|
|
|||
|
|
//printf("MeasStar1 is %10.3f \t %10.3f \t %10.3f \t %10.3f\r\n", MeasStar1[0], MeasStar1[1], MeasStar1[2], MeasStar1[3]);
|
|||
|
|
//printf("Star1Flag is %10.3d\r\n", Star1Flag);
|
|||
|
|
spdlog::info("MeasStar1为:{1:10.6f},{2:10.6f},{3:10.6f},{4:10.6f},星敏有效标志为{5:d}",__FUNCTION__,
|
|||
|
|
MeasStar1[0], MeasStar1[1], MeasStar1[2], MeasStar1[3],Star1Flag);
|
|||
|
|
|
|||
|
|
star.FRAME_HEAD = 0xA18A;
|
|||
|
|
star.TLM_FRAME_ID = 0;
|
|||
|
|
star.q0 = htonl((int32_t)(MeasStar1[0] *2147483647.5));
|
|||
|
|
star.q1 = htonl((int32_t)(MeasStar1[1] *2147483647.5));
|
|||
|
|
star.q2 = htonl((int32_t)(MeasStar1[2] *2147483647.5));
|
|||
|
|
star.q3 = htonl((int32_t)(MeasStar1[3] *2147483647.5));
|
|||
|
|
|
|||
|
|
star.STAR_NUM_D = 100;
|
|||
|
|
star.WD_CNT = 12;
|
|||
|
|
star.STAR_NUM_T = 23;
|
|||
|
|
star.para1 = 0x91;
|
|||
|
|
|
|||
|
|
uint8_t checksum = 0;
|
|||
|
|
uint8_t *buffer = (uint8_t *)&star.FRAME_HEAD;
|
|||
|
|
for(int kc = 0;kc<CAK_TLM_1_Info_Length-1;kc++)
|
|||
|
|
checksum += buffer[kc];
|
|||
|
|
star.SUM = checksum;
|
|||
|
|
memcpy(frame,(uint8_t *)&star.FRAME_HEAD,CAK_TLM_1_Info_Length);
|
|||
|
|
|
|||
|
|
spdlog::info("函数{0}STAR数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
|
|||
|
|
sizeof(CAK_TLM_1_STRUCT),frame[CAK_TLM_1_Info_Length-1]);
|
|||
|
|
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// @brief 生成星敏2遥测帧
|
|||
|
|
/// @param frame:生成的遥测帧数据
|
|||
|
|
/// @return
|
|||
|
|
uint8_t generate_star_2_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3])
|
|||
|
|
{
|
|||
|
|
CAK_TLM_1_STRUCT star;
|
|||
|
|
memset(frame,0,CAK_TLM_1_Info_Length);
|
|||
|
|
memset((uint8_t *)&star.FRAME_HEAD,0,CAK_TLM_1_Info_Length);
|
|||
|
|
|
|||
|
|
|
|||
|
|
double MeasStar2[4]; int Star2Flag = 0;
|
|||
|
|
GetStar2Meas(MeasStar2, &Star2Flag, Star2QuatInstall, QuatECI, AttiVelECI);
|
|||
|
|
|
|||
|
|
// printf("MeasStar2 is %10.3f \t %10.3f \t %10.3f \t %10.3f\r\n", MeasStar2[0], MeasStar2[1], MeasStar2[2], MeasStar2[3]);
|
|||
|
|
// printf("Star2Flag is %10.3d\r\n", Star2Flag);
|
|||
|
|
|
|||
|
|
spdlog::info("MeasStar2为:{1:10.6f},{2:10.6f},{3:10.6f},{4:10.6f},星敏有效标志为{5:d}",__FUNCTION__,
|
|||
|
|
MeasStar2[0], MeasStar2[1], MeasStar2[2], MeasStar2[3],Star2Flag);
|
|||
|
|
|
|||
|
|
star.FRAME_HEAD = 0xA18A;
|
|||
|
|
star.TLM_FRAME_ID = 0;
|
|||
|
|
star.q0 = htonl((int32_t)(MeasStar2[0] *2147483647.5));
|
|||
|
|
star.q1 = htonl((int32_t)(MeasStar2[1] *2147483647.5));
|
|||
|
|
star.q2 = htonl((int32_t)(MeasStar2[2] *2147483647.5));
|
|||
|
|
star.q3 = htonl((int32_t)(MeasStar2[3] *2147483647.5));
|
|||
|
|
|
|||
|
|
star.STAR_NUM_D = 100;
|
|||
|
|
star.WD_CNT = 12;
|
|||
|
|
star.STAR_NUM_T = 23;
|
|||
|
|
star.para1 = 0x91;
|
|||
|
|
|
|||
|
|
uint8_t checksum = 0;
|
|||
|
|
uint8_t *buffer = (uint8_t *)&star.FRAME_HEAD;
|
|||
|
|
for(int kc = 0;kc<CAK_TLM_1_Info_Length-1;kc++)
|
|||
|
|
checksum += buffer[kc];
|
|||
|
|
star.SUM = checksum;
|
|||
|
|
memcpy(frame,(uint8_t *)&star.FRAME_HEAD,CAK_TLM_1_Info_Length);
|
|||
|
|
|
|||
|
|
spdlog::info("函数{0}STAR数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
|
|||
|
|
sizeof(CAK_TLM_1_STRUCT),frame[CAK_TLM_1_Info_Length-1]);
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 星敏感器1测量模拟 */
|
|||
|
|
void GetStar1Meas(double MeasStar1[4], int* Star1Flag, double Star1QuatInstall[4], double QuatECI[4], double AttiVelECI[3]) {
|
|||
|
|
|
|||
|
|
/* 星敏感器1测量模拟模块 */
|
|||
|
|
/* 输入量 */
|
|||
|
|
/* QuatECI 惯性系下姿态四元数qbi */
|
|||
|
|
/* AttiVelECI 角速度Wbi */
|
|||
|
|
/* Star1QuatInstall 星敏感器1总安装四元数（从本体到真实） */
|
|||
|
|
/* 输出量 */
|
|||
|
|
/* MeasStar1 星敏感器1输出值Wgi_meas */
|
|||
|
|
/* Star1Flag 星敏感器1有效标志位 */
|
|||
|
|
|
|||
|
|
double StarBias_axis, StarBias_plane, StarNoise_axis, StarNoise_plane, AttiVelECI_norm, ii;
|
|||
|
|
double AttiVelECI_deg[3], MeasErr[3], q_sr_i_true_1[4], e_sr_i_err_1[3], q_sr_i_err_1[4], q_sr_i_meas_1[4], CrossMatrixQuatECI[4][4], CrossMatrixQ_sr_i_true_1[4][4];
|
|||
|
|
double StarMeasErr[3][4] = { {60.0,5.0,0.0,0.0},{170.0,28.0,0.0,0.0},{250.0,50.0,0.0,0.0} };
|
|||
|
|
|
|||
|
|
AttiVelECI_deg[0] = AttiVelECI[0] * 180.0 / 3.1415926535;
|
|||
|
|
AttiVelECI_deg[1] = AttiVelECI[1] * 180.0 / 3.1415926535;
|
|||
|
|
AttiVelECI_deg[2] = AttiVelECI[2] * 180.0 / 3.1415926535;
|
|||
|
|
|
|||
|
|
AttiVelECI_norm = sqrt(pow(AttiVelECI_deg[0], 2) + pow(AttiVelECI_deg[1], 2) + pow(AttiVelECI_deg[2], 2));
|
|||
|
|
if (AttiVelECI_norm == 0)
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[0][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[0][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[0][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[0][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
else if ((0 < AttiVelECI_norm) && (AttiVelECI_norm < 0.5))
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[1][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[1][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[1][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[1][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
else if (0.5 <= AttiVelECI_norm)
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[2][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[2][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[2][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[2][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
//生成随机误差量
|
|||
|
|
for (int i = 0; i < 3; i++)
|
|||
|
|
{
|
|||
|
|
MeasErr[i] = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//计算q_sr_i_true_1
|
|||
|
|
qleft(CrossMatrixQuatECI, QuatECI);
|
|||
|
|
mtxMtp((double*)q_sr_i_true_1, (double*)CrossMatrixQuatECI, 4, 4, (double*)Star1QuatInstall, 4, 1);
|
|||
|
|
ii = -1.0;
|
|||
|
|
if (q_sr_i_true_1[0] < 0) {
|
|||
|
|
mtxMtp((double*)q_sr_i_true_1, (double*)q_sr_i_true_1, 4, 1, &ii, 1, 1);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
e_sr_i_err_1[0] = StarBias_axis + StarNoise_axis * MeasErr[0];
|
|||
|
|
e_sr_i_err_1[1] = StarBias_plane + StarNoise_plane * MeasErr[1];
|
|||
|
|
e_sr_i_err_1[2] = StarBias_plane + StarNoise_plane * MeasErr[2];
|
|||
|
|
euler2qua(q_sr_i_err_1, e_sr_i_err_1);
|
|||
|
|
qleft(CrossMatrixQ_sr_i_true_1, q_sr_i_true_1);
|
|||
|
|
mtxMtp((double*)q_sr_i_meas_1, (double*)CrossMatrixQ_sr_i_true_1, 4, 4, (double*)q_sr_i_err_1, 4, 1);
|
|||
|
|
if (q_sr_i_meas_1[0] < 0) {
|
|||
|
|
mtxMtp((double*)q_sr_i_meas_1, (double*)q_sr_i_meas_1, 4, 1, &ii, 1, 1);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MeasStar1[0] = q_sr_i_meas_1[0];
|
|||
|
|
MeasStar1[1] = q_sr_i_meas_1[1];
|
|||
|
|
MeasStar1[2] = q_sr_i_meas_1[2];
|
|||
|
|
MeasStar1[3] = q_sr_i_meas_1[3];
|
|||
|
|
*Star1Flag = 1;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/* 星敏感器2测量模拟 */
|
|||
|
|
void GetStar2Meas(double MeasStar2[4], int* Star2Flag, double Star2QuatInstall[4], double QuatECI[4], double AttiVelECI[3]) {
|
|||
|
|
|
|||
|
|
/* 星敏感器1测量模拟模块 */
|
|||
|
|
/* 输入量 */
|
|||
|
|
/* QuatECI 惯性系下姿态四元数qbi */
|
|||
|
|
/* AttiVelECI 角速度Wbi */
|
|||
|
|
/* Star2QuatInstall 星敏感器2总安装四元数（从本体到真实） */
|
|||
|
|
/* 输出量 */
|
|||
|
|
/* MeasStar2 星敏感器2输出值Wgi_meas */
|
|||
|
|
/* Star2Flag 星敏感器2有效标志位 */
|
|||
|
|
|
|||
|
|
double StarBias_axis, StarBias_plane, StarNoise_axis, StarNoise_plane, AttiVelECI_norm, ii;
|
|||
|
|
double AttiVelECI_deg[3], MeasErr[3], q_sr_i_true_2[4], e_sr_i_err_2[3], q_sr_i_err_2[4], q_sr_i_meas_2[4], CrossMatrixQuatECI[4][4], CrossMatrixQ_sr_i_true_2[4][4];
|
|||
|
|
double StarMeasErr[3][4] = { {60.0,5.0,0.0,0.0},{170.0,28.0,0.0,0.0},{250.0,50.0,0.0,0.0} };
|
|||
|
|
|
|||
|
|
AttiVelECI_deg[0] = AttiVelECI[0] * 180.0 / 3.1415926535;
|
|||
|
|
AttiVelECI_deg[1] = AttiVelECI[1] * 180.0 / 3.1415926535;
|
|||
|
|
AttiVelECI_deg[2] = AttiVelECI[2] * 180.0 / 3.1415926535;
|
|||
|
|
|
|||
|
|
AttiVelECI_norm = sqrt(pow(AttiVelECI_deg[0], 2) + pow(AttiVelECI_deg[1], 2) + pow(AttiVelECI_deg[2], 2));
|
|||
|
|
if (AttiVelECI_norm == 0)
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[0][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[0][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[0][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[0][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
else if ((0 < AttiVelECI_norm) && (AttiVelECI_norm < 0.5))
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[1][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[1][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[1][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[1][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
else if (0.5 <= AttiVelECI_norm)
|
|||
|
|
{
|
|||
|
|
StarNoise_axis = StarMeasErr[2][0] / 3600.0;
|
|||
|
|
StarNoise_plane = StarMeasErr[2][1] / 3600.0;
|
|||
|
|
StarBias_axis = StarMeasErr[2][2] / 3600.0;
|
|||
|
|
StarBias_plane = StarMeasErr[2][3] / 3600.0;
|
|||
|
|
}
|
|||
|
|
//生成随机误差量
|
|||
|
|
for (int i = 0; i < 3; i++)
|
|||
|
|
{
|
|||
|
|
MeasErr[i] = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//计算q_sr_i_true_1
|
|||
|
|
qleft(CrossMatrixQuatECI, QuatECI);
|
|||
|
|
mtxMtp((double*)q_sr_i_true_2, (double*)CrossMatrixQuatECI, 4, 4, (double*)Star2QuatInstall, 4, 1);
|
|||
|
|
ii = -1.0;
|
|||
|
|
if (q_sr_i_true_2[0] < 0) {
|
|||
|
|
mtxMtp((double*)q_sr_i_true_2, (double*)q_sr_i_true_2, 4, 1, &ii, 1, 1);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
e_sr_i_err_2[0] = StarBias_axis + StarNoise_axis * MeasErr[0];
|
|||
|
|
e_sr_i_err_2[1] = StarBias_plane + StarNoise_plane * MeasErr[1];
|
|||
|
|
e_sr_i_err_2[2] = StarBias_plane + StarNoise_plane * MeasErr[2];
|
|||
|
|
euler2qua(q_sr_i_err_2, e_sr_i_err_2);
|
|||
|
|
qleft(CrossMatrixQ_sr_i_true_2, q_sr_i_true_2);
|
|||
|
|
mtxMtp((double*)q_sr_i_meas_2, (double*)CrossMatrixQ_sr_i_true_2, 4, 4, (double*)q_sr_i_err_2, 4, 1);
|
|||
|
|
if (q_sr_i_meas_2[0] < 0) {
|
|||
|
|
mtxMtp((double*)q_sr_i_meas_2, (double*)q_sr_i_meas_2, 4, 1, &ii, 1, 1);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MeasStar2[0] = q_sr_i_meas_2[0];
|
|||
|
|
MeasStar2[1] = q_sr_i_meas_2[1];
|
|||
|
|
MeasStar2[2] = q_sr_i_meas_2[2];
|
|||
|
|
MeasStar2[3] = q_sr_i_meas_2[3];
|
|||
|
|
*Star2Flag = 1;
|
|||
|
|
|
|||
|
|
}
|