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

141
sensor/source/sensor_gyro.cpp Normal file
View File

@@ -0,0 +1,141 @@
#include "sensor_gyro.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>
int32_t Delta_Gyro_angle_int[3];
uint16_t Integ_time_int[3];
double Gyro1MtxInstall[3][3] = { {-0.000872664182949154, 0.000873425726015331, 0.999999237792072},
{0.999999238456644, -0.000871902307307135, 0.000873425726015331},
{0.000872664515235150, 0.999999238456644, -0.000872664182949154} };
double GyroOutputPeriod = 1.0;
double Ox = 0.017; double Oy = 0.205; double Oz = 0.369; //%常值漂移，单位为°/h
double Tx = 20.0; double Ty = 20.0; double Tz = 20.0; //积分时间，单位为ms
double Kx = 289752.5; double Ky = 299114.7; double Kz = 282454.6; //%标度因子，单位为^/°/s
void GetGyro1Meas(double MeasGyro1[3], double AttiVelECI[3], double Gyro1MtxInstall[3][3], double GyroOutputPeriod);
/// @brief 生成GYRO遥测帧
/// @param frame:生成的遥测帧数据
/// @return
uint8_t generate_gyro_frame(uint8_t *frame, double AttiVelECI[3])
{
Gyro_Frame gyro;
memset(frame,0,GYRO_FRAME_LENGTH);
memset( (uint8_t *)&gyro.header,0,GYRO_FRAME_LENGTH);
double MeasGyro1[3];
GetGyro1Meas( MeasGyro1, AttiVelECI, Gyro1MtxInstall, GyroOutputPeriod);
// printf("MeasGyro1 is %10.3f \t %10.3f \t %10.3f\r\n", MeasGyro1[0], MeasGyro1[1], MeasGyro1[2]);
spdlog::info("MeasGyro1为:{1:10.6f},{2:10.6f},{3:10.6f}",__FUNCTION__,
MeasGyro1[0], MeasGyro1[1], MeasGyro1[2]);
gyro.header = 0x97;
gyro.x_angle_delta[0] = (uint8_t)((Delta_Gyro_angle_int[0] & 0x00FF0000) >> 16);
gyro.x_angle_delta[1] = (uint8_t)((Delta_Gyro_angle_int[0] & 0x0000FF00)>>8);
gyro.x_angle_delta[2] = (uint8_t)((Delta_Gyro_angle_int[0] & 0x000000FF));
gyro.x_integral_time = htons(Integ_time_int[0]);
gyro.y_angle_delta[0] = (uint8_t)((Delta_Gyro_angle_int[1] & 0x00FF0000) >> 16);
gyro.y_angle_delta[1] = (uint8_t)((Delta_Gyro_angle_int[1] & 0x0000FF00)>>8);
gyro.y_angle_delta[2] = (uint8_t)((Delta_Gyro_angle_int[1] & 0x000000FF));
gyro.y_integral_time = htons(Integ_time_int[1]);
gyro.z_angle_delta[0] = (uint8_t)((Delta_Gyro_angle_int[2] & 0x00FF0000)>>16);
gyro.z_angle_delta[1] = (uint8_t)((Delta_Gyro_angle_int[2] & 0x0000FF00)>>8);
gyro.z_angle_delta[2] = (uint8_t)((Delta_Gyro_angle_int[2] & 0x000000FF));
gyro.z_integral_time = htons(Integ_time_int[2]);
uint8_t checksum = 0;
uint8_t *buffer = (uint8_t *)&gyro.header;
for(int kc = 0;kc<GYRO_FRAME_LENGTH-1;kc++)
checksum +=buffer[kc];
gyro.checksum = checksum;
memcpy(frame,(uint8_t *)&gyro.header,GYRO_FRAME_LENGTH);
spdlog::info("函数{0}微光纤数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
sizeof(Gyro_Frame),frame[GYRO_FRAME_LENGTH-1]);
return 1;
}
/* 微光纤陀螺测量模拟 */
void GetGyro1Meas(double MeasGyro1[3], double AttiVelECI[3], double Gyro1MtxInstall[3][3], double GyroOutputPeriod) {
/* 微光纤陀螺测量模拟模块 */
/* 输入量 */
/* AttiVelECI 角速度Wbi */
/* Gyro1MtxInstall 微光纤陀螺总安装矩阵（从本体到真实） */
/* GyroOutputPeriod 循环周期*/
/* 输出量 */
/* MeasGyro1 微光纤陀螺输出值Wgi_meas */
double wgi_true[3];
double wgi_meas[3];
double Gyro1MeasErr[3] = { 4.17E-5 * 3.1415926535 / 180.0, 3.33E-4 * 3.1415926535 / 180.0, 1E-4 * 3.1415926535 / 180.0 };
static double b_true1[3] = { 7.2780e-07, 7.2780e-07, 7.2780e-07 };
static double b_true_last1[3] = { 7.2780e-07, 7.2780e-07, 7.2780e-07 };
double tSample = 0.05;
double GyroNoise_N = Gyro1MeasErr[1];
double GyroNoise_K = Gyro1MeasErr[2];
//计算wgi_true
mtxMtp((double*)wgi_true, (double*)Gyro1MtxInstall, 3, 3, (double*)AttiVelECI, 3, 1);
//计算wgi_meas
for (int i = 0; i < 3; i++)
{
wgi_meas[i] = wgi_true[i] + 0.5 * (b_true1[i] + b_true_last1[i]) + sqrt(pow(GyroNoise_N, 2) / tSample + 0.0833 * pow(GyroNoise_K, 2) * tSample) * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0);
}
MeasGyro1[0] = wgi_meas[0];
MeasGyro1[1] = wgi_meas[1];
MeasGyro1[2] = wgi_meas[2];
//保存b_true_last1
b_true_last1[0] = b_true1[0];
b_true_last1[1] = b_true1[1];
b_true_last1[2] = b_true1[2];
//更新b_true1
double temp = GyroOutputPeriod / tSample;
for (int i = 0; i < temp; i++)
{
for (int j = 0; j < 3; j++)
{
b_true1[j] += GyroNoise_K * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * sqrt(tSample);
}
}
/* 角速度结果按通讯协议输出 */
double Wx = MeasGyro1[0] * 180.0 / 3.1415926535; //角度制，单位°/s
double Wy = MeasGyro1[1] * 180.0 / 3.1415926535;
double Wz = MeasGyro1[2] * 180.0 / 3.1415926535;
double Delta_angle_x = round(Tx * Kx * (Wx + Ox / 3600) / 1000); //X轴角度增量脉冲数，单位为^
double Delta_angle_y = round(Ty * Ky * (Wy + Oy / 3600) / 1000); //Y轴角度增量脉冲数，单位为^
double Delta_angle_z = round(Tz * Kz * (Wz + Oz / 3600) / 1000); //Z轴角度增量脉冲数，单位为^
double Integ_time_x = Tx; //积分时间，单位为ms
double Integ_time_y = Ty;
double Integ_time_z = Tz;
Delta_Gyro_angle_int[0] = (int32_t)Delta_angle_x; //将double型变量转换为整数
Delta_Gyro_angle_int[1] = (int32_t)Delta_angle_y;
Delta_Gyro_angle_int[2] = (int32_t)Delta_angle_z;
Integ_time_int[0] = (uint16_t)Integ_time_x;
Integ_time_int[1] = (uint16_t)Integ_time_y;
Integ_time_int[2] = (uint16_t)Integ_time_z;
}