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

110
sensor/source/sensor_gnss.cpp Normal file
View File

@@ -0,0 +1,110 @@
#include "sensor_gnss.h"
#include "orbit_info.h"
#include "libconvert.h"
#include "liblog.h"
#include <arpa/inet.h>//linux下大小端转换
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "TyxkDynamics.h"
#include "GNCFunction.h"
using namespace Dynamics;
GNSS_Base_t gnss_info;
uint8_t telemetry_cnt = 0;
void GetGNSSMeas(double PosVelECIMeas[6], double PosVelWGS84[6], double* GNSSTime, double PosVelECIReal[6], double mjd, double Epoch_J2000);
/// @brief 生成GNSS遥测帧
/// @param frame:生成的遥测帧数据
/// @return: 无
uint8_t generate_gnss_frame(uint8_t *frame, double PosVelECIReal[6], double mjd, double Epoch_J2000)
{
uint8_t gnss_frame[GPS_Hk_FRAME_LENGTH] = {0};
double PosVelECIMeas[6], PosVelWGS84[6], GNSSTime;
GetGNSSMeas(PosVelECIMeas, PosVelWGS84, &GNSSTime, PosVelECIReal, mjd, Epoch_J2000);
//printf("PosVelECIMeas is %10.3f \t %10.3f \t %10.3f\r\n", PosVelECIMeas[0], PosVelECIMeas[1], PosVelECIMeas[2]);
//printf("PosVelWGS84 is %10.3f \t %10.3f \t %10.3f\r\n", PosVelWGS84[0], PosVelWGS84[1], PosVelWGS84[1]);
spdlog::info("函数{0}计算位置为:{1:.5f},{2:.5f},{3:.5f},速度为:{4:.2f},{5:.2f},{6:.2f},GNSSTime为:{7:.5f},Epoch_J2000为{8:.5f}",__FUNCTION__,
PosVelECIMeas[0], PosVelECIMeas[1], PosVelECIMeas[2],
PosVelECIMeas[3], PosVelECIMeas[4], PosVelECIMeas[5],
GNSSTime, Epoch_J2000);
uint16_t checksum = 0;
gnss_info.header[0] = 0xEB;
gnss_info.header[1] = 0x90;
gnss_info.length = 0x6C01;
gnss_info.datatype = 0x12;
gnss_info.telemetry_cnt = telemetry_cnt++;
gnss_info.GNSS_X_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[0]*10.0));
gnss_info.GNSS_Y_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[1]*10.0));
gnss_info.GNSS_Z_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[2]*10.0));
gnss_info.GNSS_X_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[3]*100.0));
gnss_info.GNSS_Y_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[4]*100.0));
gnss_info.GNSS_Z_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[5]*100.0));
gnss_info.GNSS_X_POS_inWGS84 = htonl((int32_t)(PosVelWGS84[0]*10.0));
gnss_info.GNSS_Y_POS_inWGS84 = htonl((int32_t)(PosVelWGS84[1]*10.0));
gnss_info.GNSS_Z_POS_inWGS84 = htonl((int32_t)(PosVelWGS84[2]*10.0));
gnss_info.GNSS_X_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[3]*100.0));
gnss_info.GNSS_Y_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[4]*100.0));
gnss_info.GNSS_Z_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[5]*100.0));
gnss_info.posi_status = 0xE4;
gnss_info.utc_time = htonl(GNSSTime);
memcpy(gnss_frame,(uint8_t *)&gnss_info.header[0],GPS_Hk_FRAME_LENGTH);
for(int kc= 2;kc<GPS_Hk_FRAME_LENGTH-4;kc++)
{
checksum+=gnss_frame[kc];
}
gnss_info.checksum = htons(checksum);
gnss_info.tail = 0xD709;
memcpy(frame,(uint8_t *)&gnss_info,GPS_Hk_FRAME_LENGTH);
return 1;
}
/* GNSS模拟器 */
void GetGNSSMeas(double PosVelECIMeas[6], double PosVelWGS84[6],double* GNSSTime, double PosVelECIReal[6], double mjd, double Epoch_J2000) {
/* 近距离激光测距模拟模块 */
/* 输入量 */
/* PosVelECIReal J2000坐标系下位置速度真实值 */
/* mjd 简约儒略日，单位：天 */
/* 输出量 */
/* PosVelECIMeas J2000坐标系下位置速度测量值 */
/* PosVelWGS84 WGS84坐标系下位置速度测量值 */
double ECIPos[3], ECIVel[3];
ECIPos[0] = PosVelECIReal[0] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
ECIPos[1] = PosVelECIReal[1] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
ECIPos[2] = PosVelECIReal[2] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
ECIVel[0] = PosVelECIReal[3] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
ECIVel[1] = PosVelECIReal[4] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
ECIVel[2] = PosVelECIReal[5] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
PosVelECIMeas[0] = ECIPos[0]; PosVelECIMeas[1] = ECIPos[1]; PosVelECIMeas[2] = ECIPos[2];
PosVelECIMeas[3] = ECIVel[0]; PosVelECIMeas[4] = ECIVel[1]; PosVelECIMeas[5] = ECIVel[2];
J2000_WGS84(mjd, PosVelECIMeas, PosVelWGS84);
*GNSSTime = Epoch_J2000 - (86400 * 365 * 2);
}