142 lines
5.3 KiB
C++
142 lines
5.3 KiB
C++
#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;
|
||
}
|