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;
|
||
|
||
}
|