140 lines
5.2 KiB
C++
140 lines
5.2 KiB
C++
#include "sensor_mems.h"
|
||
#include "orbit_info.h"
|
||
|
||
#include "libconvert.h"
|
||
|
||
#include "liblog.h"
|
||
|
||
#include <math.h>
|
||
#include <time.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include "GNCFunction.h"
|
||
|
||
int32_t Delta_angle_int[3];
|
||
uint16_t Integ_Cnt_int[3];
|
||
double Gyro2MtxInstall[3][3] = { {-0.000872664182949154, -0.999999237792072, 0.000873425726015331},
|
||
{0.999999238456644, -0.000873425726015331, -0.000871902307307135},
|
||
{0.000872664515235150, 0.000872664182949154, 0.999999238456644} };
|
||
double MemsOutputPeriod = 1.0;
|
||
|
||
void GetGyro2Meas(double MeasGyro2[3], double AttiVelECI[3], double Gyro2MtxInstall[3][3], double MemsOutputPeriod);
|
||
|
||
/// @brief 生成MEMS遥测帧
|
||
/// @param frame:生成的遥测帧数据
|
||
/// @return
|
||
uint8_t generate_mems_frame(uint8_t *frame, double AttiVelECI[3])
|
||
{
|
||
MEMS_Frame mems;
|
||
memset(frame,0,MEMS_FRAME_LENGTH);
|
||
memset( (uint8_t *)&mems.header,0,MEMS_FRAME_LENGTH);
|
||
|
||
|
||
double MeasGyro2[3];
|
||
GetGyro2Meas(MeasGyro2, AttiVelECI, Gyro2MtxInstall, MemsOutputPeriod);
|
||
|
||
//printf("MeasGyro2 is %10.3f \t %10.3f \t %10.3f\r\n", MeasGyro2[0], MeasGyro2[1], MeasGyro2[2]);
|
||
spdlog::info("函数{0}计算MEMS输出为:{1:10.3f},{2:10.3f},{3:10.3f}",__FUNCTION__,
|
||
MeasGyro2[0], MeasGyro2[1], MeasGyro2[2]);
|
||
|
||
mems.header = 0x97;
|
||
|
||
mems.x_angle_delta[0] = (uint8_t)((Delta_angle_int[0] & 0x00FF0000) >> 16);
|
||
mems.x_angle_delta[1] = (uint8_t)((Delta_angle_int[0] & 0x0000FF00) >> 8);
|
||
mems.x_angle_delta[2] = (uint8_t)((Delta_angle_int[0] & 0x000000FF));
|
||
mems.x_mems_integral_Cnt = htons(Integ_Cnt_int[0]);
|
||
mems.y_angle_delta[0] = (uint8_t)((Delta_angle_int[1] & 0x00FF0000) >> 16);
|
||
mems.y_angle_delta[1] = (uint8_t)((Delta_angle_int[1] & 0x0000FF00) >> 8);
|
||
mems.y_angle_delta[2] = (uint8_t)((Delta_angle_int[1] & 0x000000FF));
|
||
mems.y_mems_integral_Cnt = htons(Integ_Cnt_int[1]);
|
||
mems.z_angle_delta[0] = (uint8_t)((Delta_angle_int[2] & 0x00FF0000) >> 16);
|
||
mems.z_angle_delta[1] = (uint8_t)((Delta_angle_int[2] & 0x0000FF00) >> 8);
|
||
mems.z_angle_delta[2] = (uint8_t)((Delta_angle_int[2] & 0x000000FF));
|
||
mems.z_mems_integral_Cnt = htons(Integ_Cnt_int[2]);
|
||
|
||
|
||
uint8_t checksum = 0;
|
||
uint8_t *buffer = (uint8_t *)&mems.header;
|
||
for(int kc = 0;kc<MEMS_FRAME_LENGTH-1;kc++)
|
||
checksum +=buffer[kc];
|
||
mems.checksum = checksum;
|
||
memcpy(frame,(uint8_t *)&mems.header,MEMS_FRAME_LENGTH);
|
||
|
||
spdlog::info("函数{0}MEMS数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
|
||
sizeof(MEMS_Frame),frame[MEMS_FRAME_LENGTH-1]);
|
||
|
||
return 1;
|
||
}
|
||
|
||
//* 微机械惯组测量模拟 */
|
||
void GetGyro2Meas(double MeasGyro2[3], double AttiVelECI[3], double Gyro2MtxInstall[3][3], double MemsOutputPeriod) {
|
||
|
||
/* 微机械惯组测量模拟模块 */
|
||
/* 输入量 */
|
||
/* AttiVelECI 角速度Wbi */
|
||
/* Gyro2MtxInstall 微机械惯组总安装矩阵（从本体到真实） */
|
||
/* MemsOutputPeriod 循环周期*/
|
||
/* 输出量 */
|
||
/* MeasGyro2 微机械惯组输出值Wgi_meas */
|
||
|
||
double wgi_true[3];
|
||
double wgi_meas[3];
|
||
double Gyro2MeasErr[3] = { 1.39E-5 * 3.1415926535 / 180.0, 8.33E-5 * 3.1415926535 / 180.0, 1E-4 * 3.1415926535 / 180.0 };
|
||
static double b_true2[3] = { 2.4260e-07, 2.4260e-07, 2.4260e-07 };
|
||
static double b_true_last2[3] = { 2.4260e-07, 2.4260e-07, 2.4260e-07 };
|
||
double tSample = 0.05;
|
||
double GyroNoise_N = Gyro2MeasErr[1];
|
||
double GyroNoise_K = Gyro2MeasErr[2];
|
||
|
||
//计算wgi_true
|
||
mtxMtp((double*)wgi_true, (double*)Gyro2MtxInstall, 3, 3, (double*)AttiVelECI, 3, 1);
|
||
//计算wgi_meas
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
wgi_meas[i] = wgi_true[i] + 0.5 * (b_true2[i] + b_true_last2[i]) + sqrt(pow(GyroNoise_N, 2) / tSample + 0.0833 * pow(GyroNoise_K, 2) * tSample) * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0);
|
||
}
|
||
MeasGyro2[0] = wgi_meas[0];
|
||
MeasGyro2[1] = wgi_meas[1];
|
||
MeasGyro2[2] = wgi_meas[2];
|
||
|
||
//保存b_true_last2
|
||
b_true_last2[0] = b_true2[0];
|
||
b_true_last2[1] = b_true2[1];
|
||
b_true_last2[2] = b_true2[2];
|
||
|
||
//更新b_true2
|
||
double temp = MemsOutputPeriod / tSample;
|
||
for (int i = 0; i < temp; i++)
|
||
{
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
b_true2[j] += GyroNoise_K * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * sqrt(tSample);
|
||
}
|
||
}
|
||
|
||
/* 角速度结果按通讯协议输出 */
|
||
double Wx = MeasGyro2[0] * 180.0 / 3.1415926535; //角度制，单位°/s
|
||
double Wy = MeasGyro2[1] * 180.0 / 3.1415926535;
|
||
double Wz = MeasGyro2[2] * 180.0 / 3.1415926535;
|
||
|
||
double Ox = 1.465294; double Oy = 2.140157; double Oz = 1.470876; //%常值漂移，单位为°/h
|
||
double Tx = 20.0; double Ty = 20.0; double Tz = 20.0; //积分次数
|
||
double Kx = 319198.1; double Ky = 319182.2; double Kz = 319335.1; //%标度因子，单位为^/°/s
|
||
|
||
double Delta_angle_x = round(Tx * Kx * (Wx + Ox / 3600) / 1024); //X轴角度增量脉冲数，单位为^
|
||
double Delta_angle_y = round(Ty * Ky * (Wy + Oy / 3600) / 1024); //Y轴角度增量脉冲数，单位为^
|
||
double Delta_angle_z = round(Tz * Kz * (Wz + Oz / 3600) / 1024); //Z轴角度增量脉冲数，单位为^
|
||
double Integ_Cnt_x = Tx; //积分次数
|
||
double Integ_Cnt_y = Ty;
|
||
double Integ_Cnt_z = Tz;
|
||
|
||
|
||
Delta_angle_int[0] = (int32_t)Delta_angle_x; //将double型变量转换为整数
|
||
Delta_angle_int[1] = (int32_t)Delta_angle_y;
|
||
Delta_angle_int[2] = (int32_t)Delta_angle_z;
|
||
Integ_Cnt_int[0] = (uint16_t)Integ_Cnt_x;
|
||
Integ_Cnt_int[1] = (uint16_t)Integ_Cnt_y;
|
||
Integ_Cnt_int[2] = (uint16_t)Integ_Cnt_z;
|
||
}
|
||
|