xb/build
0
0
Fork 0
Code Issues Activity

Initial commit from DCSP - 2026/1/15 15:11:58

Browse Source
This commit is contained in:
xb
2026-01-15 15:11:58 +08:00
commit c93536b187
100 changed files with 59765 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

253
sensor/source/sensor_star.cpp Normal file
View File

@@ -0,0 +1,253 @@
#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;
}