Initial commit from DCSP - 2026/1/15 15:11:58
This commit is contained in:
xb
49
sensor/include/orbit_info.h
Normal file
49
sensor/include/orbit_info.h
Normal file
@@ -0,0 +1,49 @@
|
||||
#ifndef _SENSOR_ORBIT_INFO_H_
|
||||
#define _SENSOR_ORBIT_INFO_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
/// @brief 步进指令和步进反馈指令中的公共部分
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t pack_id; //节点编号
|
||||
double posi_x_J2000; //X轴位置
|
||||
double posi_y_J2000; //Y轴位置
|
||||
double posi_z_J2000; //Z轴位置
|
||||
double velo_x_J2000; //X轴速度
|
||||
double velo_y_J2000; //Y轴速度
|
||||
double velo_z_J2000; //Z轴速度
|
||||
double q0_J2000; //惯性系四元数
|
||||
double q1_J2000;
|
||||
double q2_J2000;
|
||||
double q3_J2000;
|
||||
} BASIC_CMDER;
|
||||
|
||||
/// @brief 步进反馈指令
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
double Epoch; //历元时间
|
||||
BASIC_CMDER resCmder;
|
||||
} STEP_CMDER_RES;
|
||||
|
||||
uint8_t orbit_info_initz(string path);
|
||||
uint8_t set_orbit_info(uint8_t *frame,double AttiVelECI[3], double EulerVVLH[3], double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3]);
|
||||
uint8_t get_orbit_info(uint8_t *frame,uint8_t id);
|
||||
|
||||
uint8_t generate_orbit_info(double orbInfo[6], double Tar_orbInfo[6], double AttiVelECI[3],
|
||||
double EulerVVLH[3], double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3],
|
||||
double* mjd, double* Epoch2017,
|
||||
double SatMass, double SatMtxI[3][3], double ThrustForce[3], double Tq[3], double WheelAM[3]);
|
||||
|
||||
uint8_t get_orbit_epoic(double *Epoch);
|
||||
uint8_t get_orbit_posi(double *posi);
|
||||
uint8_t get_orbit_velo(double *velo);
|
||||
uint8_t get_orbit_Q(double *Q);
|
||||
uint8_t get_orbit_flag(void);
|
||||
//double get_orbit_mjd(void);
|
||||
|
||||
uint8_t get_orbit_source_flag(void);
|
||||
#endif
|
||||
369
sensor/include/sensor_gnss.h
Normal file
369
sensor/include/sensor_gnss.h
Normal file
@@ -0,0 +1,369 @@
|
||||
#ifndef _SENSOR_GNSS_H_
|
||||
#define _SENSOR_GNSS_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define GPS_Hk_FRAME_LENGTH 372
|
||||
|
||||
typedef union {
|
||||
uint8_t data;
|
||||
struct{
|
||||
uint8_t post_staus : 1;//定位状态
|
||||
uint8_t orbit_staus : 2;//定轨状态
|
||||
uint8_t resolve_staus:3;//解算状态
|
||||
uint8_t work_staus:2; //工作状态
|
||||
};
|
||||
} PostionStatus;//定位状态
|
||||
|
||||
typedef union {
|
||||
uint16_t data;
|
||||
struct{
|
||||
uint8_t gps_available_num:4; //GPS可用星数
|
||||
uint8_t gps_catch_num:4; // GPS捕获星数
|
||||
uint8_t bd_available_num:4; //bd可用星数
|
||||
uint8_t bd_catch_num:4; // bd捕获星数
|
||||
};
|
||||
}GNSS_Star_Num;//星星
|
||||
|
||||
typedef union {
|
||||
uint8_t data;
|
||||
struct{
|
||||
uint8_t A_B_flag:1; //A/B机标识
|
||||
uint8_t master_slave_flag:2; //主从核通信状态
|
||||
uint8_t Tmark_flag:1; //Tmark状态
|
||||
uint8_t Orbit_enable_flag:2; //定轨输出使能标记
|
||||
uint8_t pulse_enable_flag:2; //秒脉冲输出使能标记
|
||||
|
||||
};
|
||||
}GNSSSReceiverStatus;//监控状态
|
||||
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t rs422_init_couse:4; //422总线初始化原因
|
||||
uint8_t rs422_init_num:4; //422总线初始化计数(总线需求类型1)，初始化一次+1
|
||||
};
|
||||
}RS422_Status;//RS422状态
|
||||
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint16_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t gps_carrier_noise_46:4; //载噪比大于46的GPS星数；
|
||||
uint8_t gps_carrier_noise_less40:4; //载噪比40~46的GPS星数
|
||||
uint8_t bd_carrier_noise_46:4; //载噪比大于46的bd星数；
|
||||
uint8_t bd_carrier_noise_less40:4; //载噪比40~46的bd星数
|
||||
};
|
||||
}GNSS_CarrierNoise;//载噪比分布
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t cpusoft_reset_num:4; //导航信息处理软件复位次数
|
||||
uint8_t orbitsoft_reset_num:4; //定轨接口软件复位次数
|
||||
};
|
||||
}GNSS_Reset;//GNSS接收机复位计数
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t not_posi_reason:4; //非定位原因
|
||||
uint8_t reset_reason_core:4; //从核复位原因
|
||||
};
|
||||
}GNSS_CPUSoft_Status;//航信息处理软件状态
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data[12];
|
||||
struct
|
||||
{
|
||||
uint8_t thoroughfare0:4; //通道0
|
||||
uint8_t thoroughfare1:4; //通道1
|
||||
uint8_t thoroughfare2:4; //通道2
|
||||
uint8_t thoroughfare3:4; //通道3
|
||||
uint8_t thoroughfare4:4; //通道4
|
||||
uint8_t thoroughfare5:4; //通道5
|
||||
uint8_t thoroughfare6:4; //通道6
|
||||
uint8_t thoroughfare7:4; //通道7
|
||||
uint8_t thoroughfare8:4; //通道8
|
||||
uint8_t thoroughfare9:4; //通道9
|
||||
uint8_t thoroughfare10:4; //通道10
|
||||
uint8_t thoroughfare11:4; //通道11
|
||||
uint8_t thoroughfare12:4; //通道12
|
||||
uint8_t thoroughfare13:4; //通道13
|
||||
uint8_t thoroughfare14:4; //通道14
|
||||
uint8_t thoroughfare15:4; //通道15
|
||||
uint8_t thoroughfare16:4; //通道16
|
||||
uint8_t thoroughfare17:4; //通道17
|
||||
uint8_t thoroughfare18:4; //通道18
|
||||
uint8_t thoroughfare19:4; //通道19
|
||||
uint8_t thoroughfare20:4; //通道20
|
||||
uint8_t thoroughfare21:4; //通道21
|
||||
uint8_t thoroughfare22:4; //通道22
|
||||
uint8_t thoroughfare23:4; //通道23
|
||||
};
|
||||
}GNSS_Orbit_Reject_Reason;//定轨剔除原因
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t filter_conver_flag:2; //滤波器收敛标志
|
||||
uint8_t Orbit_init_flag:2; //定轨初始化标记
|
||||
uint8_t reset_reason_master_core:4; //主核复位原因
|
||||
};
|
||||
}GNSS_OrbitSoft_Status;//定轨接口软件状态
|
||||
|
||||
|
||||
//该结构体中使用2字节的增量数据长度导致memcpy时数据丢失,暂不使用
|
||||
typedef union
|
||||
{
|
||||
uint32_t data;
|
||||
struct
|
||||
{
|
||||
uint16_t data_length:15;//增量数据长度
|
||||
uint8_t soft_version:8;//程序版本号
|
||||
uint8_t inject_cmd:2;//上注命令
|
||||
uint8_t crc_return_value:3;//校验返回值
|
||||
uint8_t program_inject_logo:3; //程序上注标识
|
||||
uint8_t boot:1; //boot引导区
|
||||
};
|
||||
}GNSS_Soft_Injection_Status;//上注状态
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t param_type:2; //上注参数类型
|
||||
uint8_t param_flash_flag:2; //上注参数写FLASH标识
|
||||
uint8_t track_parameter_source:2; //轨道参数来源
|
||||
uint8_t eop_param_source:2; //EOP参数来源
|
||||
|
||||
};
|
||||
}GNSS_Param_Injection_Status;//参数上注信息
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint16_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t channel_reject:8; //通道剔除原因
|
||||
uint8_t reserve:2; //保留
|
||||
uint8_t track_param:4; //跟踪参数
|
||||
uint8_t track_status:2; //跟踪状态
|
||||
|
||||
};
|
||||
}Satellite_Channel_Status;//通道卫星状态
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t satellite_num; //卫星号
|
||||
uint8_t Signal_noise_ratio;//信噪比
|
||||
Satellite_Channel_Status sat_channel_status;//通道卫星状态
|
||||
uint8_t channel_orbit_status;//通道定轨状态
|
||||
uint32_t pseudo_range_integer; //伪距整数部分
|
||||
uint16_t pseudo_range_decimal; //伪距小数部分
|
||||
uint32_t signal_carrier_integer; //载波相位整数部分
|
||||
uint32_t signal_carrier_decimal; //载波相位小数部分
|
||||
}GPS_Channel_Observe_with_pseu; //带伪距伪距率输出的GPS卫星信息
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t satellite_num; //卫星号
|
||||
uint8_t Signal_noise_ratio;//信噪比
|
||||
Satellite_Channel_Status sat_channel_status;//通道卫星状态
|
||||
uint8_t channel_orbit_status;//通道定轨状态
|
||||
}GPS_Channel_Observe_without_pseu; //带伪距伪距率输出的卫星信息
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t satellite_num; //卫星号
|
||||
uint8_t Signal_noise_ratio;//信噪比
|
||||
Satellite_Channel_Status satellite_channel_status;//通道卫星状态
|
||||
uint8_t channel_orbit_status;//通道定轨状态
|
||||
uint32_t pseudo_range_integer; //伪距整数部分
|
||||
uint16_t pseudo_range_decimal; //伪距小数部分
|
||||
}BD_Channel_Observe_with_pseu; //带伪距伪距率输出的BD卫星信息
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t satellite_num; //卫星号
|
||||
uint8_t Signal_noise_ratio;//信噪比
|
||||
Satellite_Channel_Status satellite_channel_status;//通道卫星状态
|
||||
uint8_t channel_orbit_status;//通道定轨状态
|
||||
}BD_Channel_Observe_without_pseu; //带伪距伪距率输出的BD卫星信息
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t data;
|
||||
struct
|
||||
{
|
||||
uint8_t time_top_param_type:1; //上注参数类型：(time参数上注状态)
|
||||
uint8_t time_param_source:1; //参数来源(time参数上注状态)
|
||||
uint8_t time_write_flash_flag:2; //写FLASH标识(time参数上注状态)
|
||||
uint8_t bd_top_param_type:1; //上注参数类型：(BD参数上注状态)
|
||||
uint8_t bd_param_source:1; //参数标识(BD参数上注状态)
|
||||
uint8_t bd_write_flash_flag:2; //写FLASH标识(BD参数上注状态)
|
||||
|
||||
};
|
||||
}BD_Time_Param_Status; //BD及时间基准参数上注状态
|
||||
|
||||
|
||||
|
||||
/******基本遥测数据******/
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t header[2];
|
||||
uint16_t length;
|
||||
uint8_t datatype;
|
||||
uint8_t telemetry_cnt; //遥测请求计数
|
||||
uint8_t rec_cmd_cnt; //接收指令计数
|
||||
uint8_t error_cmd_cnt; //错误指令计数
|
||||
uint8_t recent_cmd; //最近执行指令码
|
||||
|
||||
|
||||
//PostionStatus posi_status; //状态
|
||||
uint8_t posi_status;
|
||||
uint16_t bds_week; //BDS周
|
||||
uint8_t bds_second[3]; //BDS秒
|
||||
uint32_t bds_microsecond; //bds纳秒
|
||||
|
||||
uint16_t gps_week; //gps周
|
||||
uint8_t gps_second[3]; //gps秒
|
||||
uint32_t gps_microsecond; //gps纳秒
|
||||
|
||||
|
||||
uint32_t rx_star_broadcast_second; //收到星上秒值
|
||||
uint32_t rx_star_broadcast_microsecond; //收到星上毫秒值
|
||||
int8_t diff_seconds; //GNSS与星务时间差秒
|
||||
int16_t diff_m_seconds; //GNSS与星务时间差毫秒
|
||||
|
||||
int32_t GNSS_X_POS_inJ2000; //J2000坐标系，X轴位置；不定位时输出AAH
|
||||
int32_t GNSS_Y_POS_inJ2000; //J2000坐标系，Y轴位置；不定位时输出AAH
|
||||
int32_t GNSS_Z_POS_inJ2000; //J2000坐标系，Z轴位置；不定位时输出AAH
|
||||
int32_t GNSS_X_SPEED_inJ2000; //J2000坐标系，X轴速度；不定位时输出AAH
|
||||
int32_t GNSS_Y_SPEED_inJ2000; //J2000坐标系，Y轴速度；不定位时输出AAH
|
||||
int32_t GNSS_Z_SPEED_inJ2000; //J2000坐标系，Z轴速度；不定位时输出AAH
|
||||
|
||||
int32_t GNSS_X_POS_inWGS84; //WGS-84坐标系，X轴位置；不定位时输出AAH
|
||||
int32_t GNSS_Y_POS_inWGS84; //WGS-84坐标系，Y轴位置；不定位时输出AAH
|
||||
int32_t GNSS_Z_POS_inWGS84; //WGS-84坐标系，Z轴位置；不定位时输出AAH
|
||||
int32_t GNSS_X_SPEED_inWGS84; //WGS-84坐标系，X轴速度；不定位时输出AAH
|
||||
int32_t GNSS_Y_SPEED_inWGS84; //WGS-84坐标系，Y轴速度；不定位时输出AAH
|
||||
int32_t GNSS_Z_SPEED_inWGS84; //WGS-84坐标系，Z轴速度；不定位时输出AAH
|
||||
|
||||
int32_t GNSS_X_ORBIT_POS_inJ2000; //GNSS定轨X位置,J2000坐标系
|
||||
int32_t GNSS_Y_ORBIT_POS_inJ2000; //GNSS定轨Y位置,J2000坐标系
|
||||
int32_t GNSS_Z_ORBIT_POS_inJ2000; //GNSS定轨Z位置,J2000坐标系
|
||||
int32_t GNSS_X_ORBIT_SPEED_inJ2000; //GNSS定轨X速度,J2000坐标系
|
||||
int32_t GNSS_Y_ORBIT_SPEED_inJ2000; //GNSS定轨Y速度,J2000坐标系
|
||||
int32_t GNSS_Z_ORBIT_SPEED_inJ2000; //GNSS定轨Z速度,J2000坐标系
|
||||
|
||||
int32_t GNSS_X_ORBIT_POS_inWGS84; //GNSS定轨X位置,WGS-84坐标系
|
||||
int32_t GNSS_Y_ORBIT_POS_inWGS84; //GNSS定轨Y位置,WGS-84坐标系
|
||||
int32_t GNSS_Z_ORBIT_POS_inWGS84; //GNSS定轨Z位置,WGS-84坐标系
|
||||
int32_t GNSS_X_ORBIT_SPEED_inWGS84; //GNSS定轨X速度,WGS-84坐标系
|
||||
int32_t GNSS_Y_ORBIT_SPEED_inWGS84; //GNSS定轨Y速度,WGS-84坐标系
|
||||
int32_t GNSS_Z_ORBIT_SPEED_inWGS84; //GNSS定轨Z速度,WGS-84坐标系
|
||||
|
||||
GNSS_Star_Num gnss_star_num;
|
||||
|
||||
uint32_t utc_time; //UTC累计秒整数
|
||||
uint32_t tic_time; //tic计数
|
||||
|
||||
uint8_t gps_leap_seconds; //GPS定位计算结果当前使用的跳秒值
|
||||
uint8_t bd2_leap_seconds; //BDS定位计算结果当前使用的跳秒值
|
||||
|
||||
uint16_t GDOP_value; //GNSS定位计算结果GDOP值
|
||||
int32_t gps_solve_clock_diff; //GPS定位计算结果时钟偏差量
|
||||
int32_t BD_solve_clock_diff; //BDS定位计算结果时钟偏差量
|
||||
int32_t GNSS_drift; //GNSS定位计算结果时钟漂移值
|
||||
|
||||
GNSSSReceiverStatus receiver_status; //监控状态标识
|
||||
RS422_Status rs422_status; //422总线初始化原因及计数(总线需求类型1)
|
||||
uint8_t reserve; //保留
|
||||
|
||||
GNSS_CarrierNoise gnss_carrier_noise; //载噪比分布
|
||||
GNSS_Reset gnss_reset_num; //GNSS接收机复位计数
|
||||
|
||||
GNSS_CPUSoft_Status navigation_status; //导航信息处理软件状态
|
||||
GNSS_OrbitSoft_Status orbitsoft_status; //定轨接口软件状态
|
||||
|
||||
uint8_t slave_core_sram_err_num; //从核SRAM监控错误计数
|
||||
uint8_t mater_core_sram_err_num; //主核SRAM监控错误计数
|
||||
|
||||
uint8_t Single_point_use_star ; //单点定位使用星数
|
||||
uint8_t filter_point_use_star ; //滤波使用星数
|
||||
|
||||
uint8_t orbit_param_PDOP ; //定轨参数PDOP
|
||||
uint8_t orbit_param_pos_residual; //定轨参数位置残差
|
||||
uint8_t orbit_param_speed_residual; //定轨参数速度残差
|
||||
uint8_t orbit_convergence_value; //定轨收敛值
|
||||
uint8_t orbit_infor_standard_deviation; //定轨先验信息的标准差
|
||||
|
||||
uint8_t measure_update_standard_deviation; //量测更新后的标准差
|
||||
uint32_t orbit_module_run_flag; //定轨模块运行标记
|
||||
|
||||
GNSS_Orbit_Reject_Reason orbit_reject_reasion; //定轨剔除原因
|
||||
uint8_t orbit_health[3]; //当前定轨健康字
|
||||
|
||||
//GNSS_Soft_Injection_Status soft_injection_Status;//上注状态
|
||||
uint32_t soft_injection_Status;//上注状态
|
||||
GNSS_Param_Injection_Status param_injection_Status;//参数上注信息
|
||||
uint8_t reserve2; //保留
|
||||
|
||||
GPS_Channel_Observe_with_pseu gps_channel_1_observe_amount;//GPS第1通道卫星观测量
|
||||
GPS_Channel_Observe_with_pseu gps_channel_2_observe_amount;//GPS第2通道卫星观测量
|
||||
GPS_Channel_Observe_with_pseu gps_channel_3_observe_amount;//GPS第3通道卫星观测量
|
||||
GPS_Channel_Observe_without_pseu gps_channel_4_observe_amount;//GPS第4通道卫星观测量
|
||||
GPS_Channel_Observe_without_pseu gps_channel_5_observe_amount;//GPS第5通道卫星观测量
|
||||
GPS_Channel_Observe_without_pseu gps_channel_6_observe_amount;//GPS第6通道卫星观测量
|
||||
GPS_Channel_Observe_without_pseu gps_channel_7_observe_amount;//GPS第7通道卫星观测量
|
||||
GPS_Channel_Observe_without_pseu gps_channel_8_observe_amount;//GPS第8通道卫星观测量
|
||||
BD_Channel_Observe_with_pseu bd_channel_1_observe_amount;//bd第1通道卫星观测量
|
||||
BD_Channel_Observe_with_pseu bd_channel_2_observe_amount;//bd第2通道卫星观测量
|
||||
BD_Channel_Observe_with_pseu bd_channel_3_observe_amount;//bd第3通道卫星观测量
|
||||
BD_Channel_Observe_without_pseu bd_channel_4_observe_amount;//bd第4通道卫星观测量
|
||||
BD_Channel_Observe_without_pseu bd_channel_5_observe_amount;//bd第5通道卫星观测量
|
||||
BD_Channel_Observe_without_pseu bd_channel_6_observe_amount;//bd第6通道卫星观测量
|
||||
BD_Channel_Observe_without_pseu bd_channel_7_observe_amount;//bd第7通道卫星观测量
|
||||
BD_Channel_Observe_without_pseu bd_channel_8_observe_amount;//bd第8通道卫星观测量
|
||||
|
||||
BD_Time_Param_Status bd_time_param_status;
|
||||
uint8_t navigation_frame_number; //导航星历帧序号
|
||||
uint8_t navigation_frame_data[20]; //导航星历数据
|
||||
uint16_t navigation_frame_checksum; //导航星历校验和
|
||||
|
||||
uint16_t checksum; //数据长度和有效数据字段的校验和
|
||||
uint16_t tail; //桢尾
|
||||
|
||||
|
||||
} GNSS_Base_t;
|
||||
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint16_t header; //桢头
|
||||
uint16_t data_length; //数据长度
|
||||
uint8_t type; //数据类型
|
||||
uint8_t cmd[6];
|
||||
uint16_t checksum;//校验和
|
||||
uint16_t tail;//桢尾
|
||||
}GNSS_Cmd;
|
||||
|
||||
|
||||
|
||||
|
||||
uint8_t generate_gnss_frame(uint8_t *frame, double PosVelECIReal[6], double mjd, double Epoch_J2000);
|
||||
|
||||
#endif
|
||||
25
sensor/include/sensor_gyro.h
Normal file
25
sensor/include/sensor_gyro.h
Normal file
@@ -0,0 +1,25 @@
|
||||
#ifndef _SENSOR_GYRO_H_
|
||||
#define _SENSOR_GYRO_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define GYRO_FRAME_LENGTH 27
|
||||
|
||||
//光纤陀螺通信协议帧格式
|
||||
typedef struct __attribute__((packed)){
|
||||
uint8_t header;
|
||||
uint8_t x_angle_delta[3]; //陀螺X输出角度增量
|
||||
uint16_t x_integral_time; //陀螺X积分时间
|
||||
uint8_t y_angle_delta[3]; //陀螺Y输出角度增量
|
||||
uint16_t y_integral_time; //陀螺Y积分时间
|
||||
uint8_t z_angle_delta[3]; //陀螺Z输出角度增量
|
||||
uint16_t z_integral_time; //陀螺Z积分时间
|
||||
uint8_t rsvd[10];
|
||||
uint8_t checksum;
|
||||
}Gyro_Frame;
|
||||
|
||||
|
||||
uint8_t generate_gyro_frame(uint8_t *frame, double AttiVelECI[3]);
|
||||
|
||||
#endif
|
||||
33
sensor/include/sensor_mems.h
Normal file
33
sensor/include/sensor_mems.h
Normal file
@@ -0,0 +1,33 @@
|
||||
#ifndef _SENSOR_MEMS_H_
|
||||
#define _SENSOR_MEMS_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define MEMS_FRAME_LENGTH 44
|
||||
|
||||
typedef struct __attribute__((packed)){
|
||||
uint8_t header;
|
||||
uint8_t x_angle_delta[3];
|
||||
uint16_t x_mems_integral_Cnt;
|
||||
uint8_t y_angle_delta[3];
|
||||
uint16_t y_mems_integral_Cnt;
|
||||
uint8_t z_angle_delta[3];
|
||||
uint16_t z_mems_integral_Cnt;
|
||||
uint8_t x_acc_v_delta[3];
|
||||
uint16_t x_acc_integral_time;
|
||||
uint8_t y_acc_v_delta[3];
|
||||
uint16_t y_acc_integral_time;
|
||||
uint8_t z_acc_v_delta[3];
|
||||
uint16_t z_acc_integral_time;
|
||||
uint16_t x_gyro_temp;
|
||||
uint16_t y_gyro_temp;
|
||||
uint16_t z_gyro_temp;
|
||||
uint16_t x_acc_temp;
|
||||
uint16_t y_acc_temp;
|
||||
uint16_t z_acc_temp;
|
||||
uint8_t checksum;
|
||||
}MEMS_Frame;
|
||||
|
||||
uint8_t generate_mems_frame(uint8_t *frame, double AttiVelECI[3]);
|
||||
|
||||
#endif
|
||||
54
sensor/include/sensor_optical.h
Normal file
54
sensor/include/sensor_optical.h
Normal file
@@ -0,0 +1,54 @@
|
||||
#ifndef _SENSOR_OPTICAL_H_
|
||||
#define _SENSOR_OPTICAL_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define OPTIC_FRAME_LENGTH 60+164+4
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
uint16_t FRAME_HEAD ; //帧头
|
||||
uint8_t length;
|
||||
float AI01; //远距离测距（m），精度0.1m
|
||||
float AI02; //近距离测距(m)，精度0.02m
|
||||
int16_t AI03; //远距离摆镜水平角（角秒）
|
||||
int16_t AI04; //近距离摆镜俯仰角（角秒）
|
||||
uint8_t AI05; //远距离测距3.3V电源遥测
|
||||
uint8_t AI06; //近距离测距5V电源遥测
|
||||
uint16_t AI07; //远距离激光测量计数
|
||||
uint16_t AI08; //近距离激光测量计数
|
||||
uint8_t AI09; //接收指令计数
|
||||
uint8_t AI10; //遥测计数
|
||||
int32_t AI11; //遥测时间戳
|
||||
uint16_t AI12; //远距离相机工作状态
|
||||
int32_t AI13; //远距离相机曝光时间参数，单位：us
|
||||
uint8_t AI14; //远距离相机增益设置参数
|
||||
uint8_t AI15; //远距离相机输出图像帧计数
|
||||
uint8_t AI16; //远距离相机3.3V电压遥测1
|
||||
uint8_t AI17; //远距离相机5V电压遥测1
|
||||
uint16_t AI18; //近距离相机工作状态
|
||||
int32_t AI19; //近距离相机曝光时间参数，单位：us
|
||||
uint8_t AI20; //近距离相机增益设置参数
|
||||
uint8_t AI21; //近距离相机输出图像帧计数
|
||||
uint8_t AI22; //近距离相机3.3V电压遥测
|
||||
uint8_t AI23; //近距离相机5V电压遥测
|
||||
uint8_t AI_far; //远距激光测量有效标志
|
||||
uint8_t AI_close; //近距激光测量有效标志
|
||||
uint8_t AI25; //图像处理3.3V电压遥测
|
||||
uint8_t AI26; //图像处理5V电压遥测
|
||||
int32_t AI27; //综合控制板工作状态
|
||||
uint8_t AI28; //综合控制板3.3V电压遥测
|
||||
uint8_t AI29; //综合控制板5V电压遥测
|
||||
uint32_t AI_time_s;
|
||||
uint16_t AI_time_ms;
|
||||
uint8_t resvd[164];
|
||||
uint8_t Check_sum; //校验和
|
||||
} OPTICAL_TLM_STR; //OPTICAL遥测数据包
|
||||
|
||||
|
||||
|
||||
void generate_xgd_frame(uint8_t *frame, double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double LOS_Remote[9], double Euler[3], double R_Plane[2], int TargetFLAG);
|
||||
void generate_photo_frame(uint8_t *frame);
|
||||
|
||||
#endif
|
||||
89
sensor/include/sensor_star.h
Normal file
89
sensor/include/sensor_star.h
Normal file
@@ -0,0 +1,89 @@
|
||||
#ifndef _SENSOR_STAR_H_
|
||||
#define _SENSOR_STAR_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define CAK_TLM_1_Info_Length 64
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t ext;
|
||||
struct
|
||||
{
|
||||
unsigned int str_state1_data4 : 1; // 1（默认）
|
||||
unsigned int str_state1_data3 : 1; // 0：光信号激励（默认）1：电信号激励
|
||||
unsigned int str_state1_data2 : 2; //01：数据有效（姿态测量更新）
|
||||
//10：数据无效
|
||||
//00：数据有效（姿态滤波更新）
|
||||
unsigned int str_state1_data1 : 4; //0001:星跟踪模式
|
||||
//0010:全天识别模式
|
||||
//0100:测试模式
|
||||
//1000:星图成像及数据下传模式
|
||||
//1010:启动模式
|
||||
//1111:待机模式
|
||||
unsigned int str_state2_data5 : 1; //0:在轨标定关（默认）1:在轨标定开
|
||||
unsigned int str_state2_data4 : 1; //0：PPS校时使能开1：PPS信号使能关
|
||||
unsigned int str_state2_data3 : 1; //0:UTC校时有1:UTC校时无
|
||||
unsigned int str_state2_data2 : 2; //001:执行Flash第一份用户程序(默认)
|
||||
// 010:执行Flash第二份用户程序
|
||||
//011:执行Flash第三份用户程序
|
||||
unsigned int str_state2_data1 : 2; //01：执行第一份BOOT程序（默认）
|
||||
//10：执行第二份BOOT程序
|
||||
//11：执行第三份BOOT程序
|
||||
unsigned int pro_state_data8 : 1; // APP3程序校验状态。1：正确，0：错误
|
||||
unsigned int pro_state_data7 : 1; // APP2程序校验状态。1：正确，0：错误
|
||||
unsigned int pro_state_data6 : 1; // APP1程序校验状态。1：正确，0：错误
|
||||
unsigned int pro_state_data5 : 1; // 默认0
|
||||
unsigned int pro_state_data4 : 1; // BOOT3程序校验状态，1：正确，0：错误
|
||||
unsigned int pro_state_data3 : 1; // BOOT2程序校验状态，1：正确，0：错误
|
||||
unsigned int pro_state_data2 : 1; // BOOT1程序校验状态，1：正确，0：错误
|
||||
unsigned int pro_state_data1 : 1; // Loader程序校验状态，1：正确，0：错误
|
||||
|
||||
};
|
||||
|
||||
} Bitpara_State;
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
uint16_t FRAME_HEAD ; //帧头
|
||||
uint16_t TLM_FRAME_ID; //帧标识
|
||||
int32_t q0; //四元数q0
|
||||
int32_t q1; //四元数q1
|
||||
int32_t q2; //四元数q2
|
||||
int32_t q3; //四元数q3
|
||||
//Bitpara_State Bitpara_State_data; //星敏感器状态字
|
||||
uint8_t para1;
|
||||
uint8_t para2;
|
||||
|
||||
uint8_t UTC[5]; //曝光中点时刻UTC时间
|
||||
uint16_t GRAY_MEAN; // 图像灰度均值
|
||||
uint16_t PPS_CNT; // ETR脉冲计数
|
||||
int16_t PPS_DT; // 曝光时差
|
||||
uint8_t STAR_NUM_S; // 探测星数
|
||||
uint8_t STAR_NUM_G; // 导航星数
|
||||
uint8_t STAR_NUM_D; // 定姿星数
|
||||
uint8_t THR_OFST; // 星点提取阈值偏移量
|
||||
uint8_t TEMP_SEN; // 探测器温度
|
||||
uint8_t TEMP_SHL; // 处理器温度
|
||||
uint8_t EXP_TIME; // 曝光时间
|
||||
uint8_t CAK_BYTE[3]; // 命令应答字
|
||||
uint16_t STRLT_FLAG; // 杂散光标志字
|
||||
int16_t OMIGA_X; // 角速率ωx
|
||||
int16_t OMIGA_Y; // 角速率ωy
|
||||
int16_t OMIGA_Z; // 角速率ωz
|
||||
int16_t L3G_OMIG_X; //片上MEMS陀螺角速率X
|
||||
uint16_t FOCAL_L; // 焦距值
|
||||
uint8_t STAR_NUM_T; // 跟踪星数
|
||||
uint8_t REC_NUM_PRO; // 处理波门数
|
||||
int16_t L3G_OMIG_Y; // 片上MEMS陀螺角速率Y
|
||||
int16_t L3G_OMIG_Z; // 片上MEMS陀螺角速率Z
|
||||
uint8_t Bitpara_STATE; //软件健康状态字
|
||||
uint8_t WD_CNT; // 看门狗复位次数
|
||||
uint8_t SUM; // 累加和
|
||||
} CAK_TLM_1_STRUCT; //遥测数据包1（CAK_TLM_1）
|
||||
|
||||
|
||||
uint8_t generate_star_1_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3]);
|
||||
uint8_t generate_star_2_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3]);
|
||||
|
||||
#endif
|
||||
8
sensor/include/sensor_thrust.h
Normal file
8
sensor/include/sensor_thrust.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#ifndef _SENSOR_THRUST_H_
|
||||
#define _SENSOR_THRUST_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void sensor_ThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12]);
|
||||
//void sensor_ThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12]);
|
||||
#endif
|
||||
163
sensor/include/sensor_tx2.h
Normal file
163
sensor/include/sensor_tx2.h
Normal file
@@ -0,0 +1,163 @@
|
||||
#ifndef _SENSOR_TX2_H_
|
||||
#define _SENSOR_TX2_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
|
||||
#define VISION_Info_Length 164+6 //164字节数据+5字节头+1字节校验
|
||||
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint16_t ext;
|
||||
struct
|
||||
{
|
||||
uint8_t VISION_state1_data7 : 3; //输出单帧图像中点的检测数量，范围[0~5]。
|
||||
uint8_t VISION_state1_data6 : 3; //预留
|
||||
uint8_t VISION_state1_data5 : 2; //00：第一个簇是目标;
|
||||
//01：第一个簇识别错误;
|
||||
//10：第一个簇识别错误;
|
||||
//11：第一个簇是恒星
|
||||
uint8_t VISION_state1_data4 : 2; //00：第二个簇是目标;
|
||||
//01：第二个簇识别错误;
|
||||
//10：第二个簇识别错误;
|
||||
//11：第二个簇是恒星
|
||||
uint8_t VISION_state1_data3 : 2; //00：第三个簇是目标;
|
||||
//01：第三个簇识别错误;
|
||||
//10：第三个簇识别错误;
|
||||
//11：第三个簇是恒星
|
||||
uint8_t VISION_state1_data2 : 2; //00：第四个簇是目标;
|
||||
//01：第四个簇识别错误;
|
||||
//10：第四个簇识别错误;
|
||||
//11：第四个簇是恒星
|
||||
uint8_t VISION_state1_data1 : 2; //00：第五个簇是目标;
|
||||
//01：第五个簇识别错误;
|
||||
//10：第五个簇识别错误;
|
||||
//11：第五个簇是恒星
|
||||
};
|
||||
} Star_Removal_Brightness_Rec_State;
|
||||
|
||||
|
||||
typedef union
|
||||
{
|
||||
uint8_t ext;
|
||||
struct
|
||||
{
|
||||
uint8_t Flag0 : 4; //远距标志
|
||||
uint8_t Flag1 : 4; //近距标志
|
||||
};
|
||||
}TX2_Image_Flag;
|
||||
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
uint8_t recog_Valid; //识别结果,0b00：数据无效; 0b11：数据有效
|
||||
float recog_Alpha ; //计算俯仰角α1
|
||||
float recog_Beta ; //计算俯仰角α1
|
||||
}RecResult;
|
||||
|
||||
typedef struct __attribute__((__packed__))
|
||||
{
|
||||
uint16_t FRAME_HEAD; //帧头 0xEB90
|
||||
uint8_t Type_Code; //类型码 0xF0
|
||||
uint16_t Code_Length; //帧长度
|
||||
uint16_t FRAME_RESERVE1; //预留-1 0x8AA1
|
||||
uint8_t star_Q_flag;
|
||||
uint8_t Image_flag;
|
||||
uint8_t Telemetry_Counter; //遥测帧计数器
|
||||
Star_Removal_Brightness_Rec_State SRBRS_State_data; //远距恒星剔除和亮度识别算法检测情况
|
||||
|
||||
float TARGET_Horizontal; //图像中目标点的横向坐标
|
||||
float TARGET_Vertical ; //图像中目标点的横向坐标
|
||||
float TARGET_Brightness ; //图像中目标点的亮度信息
|
||||
uint8_t Target_Capture_Flag; //目标稳定捕获标志（0b01：已稳定捕获，进入跟踪阶段；0b00：未稳定捕获）
|
||||
uint8_t Image_Number; //目标识别阶段已识别到目标的图像数量
|
||||
float frame_noise_ratio ; //图像的信噪比
|
||||
float frame_contrast; //图像对比度
|
||||
uint8_t Area_Number_far; //远距相机检测到的区域数量
|
||||
uint8_t Area_Number_close; //近距相机检测到的区域数量
|
||||
uint8_t far_Core_Size_far; //远距相机存储开运算核的尺寸
|
||||
uint8_t far_Core_Size_close; //近距相机存储开运算核的尺寸
|
||||
uint8_t Area_Brightness_far; //远距相机图像平均亮度
|
||||
uint8_t Area_Brightness_close; //近距相机图像平均亮度
|
||||
uint8_t Target_Edge_Integrity_Flag_far; //远距相机完整性标志;0b00：完整；0b11：不完整
|
||||
uint8_t Target_Edge_Integrity_Flag_close; //近距相机完整性标志;0b00：完整；0b11：不完整
|
||||
|
||||
uint8_t Missed_Number; //跟踪目标脱靶次数
|
||||
uint8_t line_of_sight_flag_far; //远距相机视线距可靠标志位;0b00：不可靠;0b11：可靠
|
||||
uint8_t line_of_sight_flag_close; //近距相机视线距可靠标志位;0b00：不可靠;0b11：可靠
|
||||
|
||||
float Line_Distance_far; //远距相机视线距离
|
||||
float Line_Distance_close; //近距相机视线距离
|
||||
uint32_t Image_time_sec; //照片时间戳整秒
|
||||
uint16_t Image_time_ms; //照片时间戳毫秒
|
||||
|
||||
RecResult Left_Sail_far; //远距相机左帆板
|
||||
RecResult Right_Sail_far; //远距相机右帆板
|
||||
RecResult Self_plat_far; //远距相机本体
|
||||
|
||||
RecResult Left_Sail_close; //近距相机左帆板
|
||||
RecResult Right_Sail_close; //近距相机右帆板
|
||||
RecResult Self_plat_close; //近距相机本体
|
||||
|
||||
|
||||
uint8_t Corrosion_size_far_cmd; //远距相机腐蚀核尺寸指令执行结果
|
||||
uint8_t Corrosion_size_far; //远距相机腐蚀核尺寸
|
||||
|
||||
uint8_t Corrosion_size_close_cmd; //近距相机腐蚀核尺寸指令执行结果
|
||||
uint8_t Corrosion_size_close; //近距相机腐蚀核尺寸
|
||||
|
||||
uint8_t removal_match_radius_cmd; //恒星剔除匹配半径指令执行结果
|
||||
uint8_t removal_match_radius; //恒星剔除匹配半径
|
||||
|
||||
uint8_t Capture_flag_cmd; //捕获标志位指令执行结果
|
||||
uint8_t Capture_flag; //捕获标志位
|
||||
|
||||
uint8_t Target_Stable_Threshold_cmd; //目标稳定捕获判定阈值指令执行结果
|
||||
uint8_t Target_Stable_Threshold; //目标稳定捕获判定阈值
|
||||
|
||||
uint8_t alg_Threshold_far_cmd; //远距相机算法阈值分割系数指令执行结果
|
||||
float alg_Threshold_far; //远距相机算法阈值分割系数
|
||||
|
||||
uint8_t target_track_cmd; //追踪目标信息遥控指令接收结果
|
||||
uint8_t target_track_fan_flag; //追踪目标信息帆板单双侧标志
|
||||
float target_track_fan_coef; //追踪目标信息帆板比例系数
|
||||
float target_track_target_length; //追踪目标信息目标整体长度
|
||||
float target_track_target_width; //追踪目标信息目标帆板宽度
|
||||
|
||||
uint8_t far_alg_change_cmd; //远距相机远近距算法切换阈值指令
|
||||
uint8_t far_alg_change_Threshold; //远距相机远近距算法切换阈值
|
||||
|
||||
uint8_t far_camera_Threshold_cmd; //远距相机依据像素大小判断目标阈值指令
|
||||
uint8_t far_camera_Threshold; //远距相机依据像素大小判断目标阈值
|
||||
|
||||
|
||||
uint8_t moon_exist_cmd; //月亮存在标志指令
|
||||
uint8_t moon_exist; //月亮存在标志
|
||||
|
||||
uint8_t close_alg_change_cmd; //近距相机远近距算法切换阈值指令
|
||||
uint8_t close_alg_change_Threshold; //近距相机远近距算法切换阈值
|
||||
|
||||
uint8_t close_camera_Threshold_cmd; //近距相机依据像素大小判断目标阈值指令
|
||||
uint8_t close_camera_Threshold; //近距相机依据像素大小判断目标阈值
|
||||
|
||||
uint32_t Identification_time_sec; //识别结果时间整秒
|
||||
uint16_t Identification_time_ms; //识别结果时间毫秒
|
||||
|
||||
TX2_Image_Flag image_exist_flag; //图像无目标标志
|
||||
TX2_Image_Flag far_close_alg_change_flag; //远近距算法切换标志
|
||||
TX2_Image_Flag target_edge_flag; //目标边缘标志
|
||||
// uint8_t image_exist_flag;
|
||||
// uint8_t far_close_alg_change_flag; //远近距算法切换标志
|
||||
// uint8_t target_edge_flag; //目标边缘标志
|
||||
|
||||
float frame_noise_ratio_close ; //近距图像的信噪比
|
||||
float frame_contrast_close; //近距图像对比度
|
||||
uint8_t checksum;
|
||||
}VISION_TLM_STRUCT; //视觉遥测数据包
|
||||
|
||||
|
||||
uint8_t generate_tx2_frame(uint8_t *frame, double LOS_Remote[9], double R_Plane[2], double TarRVECI[6], double ChaRVECI[6], double ChaMtxVVLH2Body[3][3], double ChaMtxECI2VVLH[3][3], int TargetFLAG);
|
||||
|
||||
#endif
|
||||
28
sensor/include/sensor_wheel.h
Normal file
28
sensor/include/sensor_wheel.h
Normal file
@@ -0,0 +1,28 @@
|
||||
#ifndef _SENSOR_WHEEL_H_
|
||||
#define _SENSOR_WHEEL_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define WHEEL_FRAME_LENGTH 19
|
||||
|
||||
#define WHEEL_EXP 1000
|
||||
#define WHEEL_LIMIT4 4.294967296e9
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t header;
|
||||
uint8_t rotate_speed[4]; //飞轮转速
|
||||
uint8_t current[4]; //飞轮电流
|
||||
uint8_t control_state[4]; //控制状态
|
||||
uint8_t acc_torque[4]; //飞轮加速力矩指令
|
||||
uint8_t id; //飞轮ID
|
||||
uint8_t checksum;
|
||||
}FlyWheel_Frame;
|
||||
|
||||
//uint8_t generate_wheel_frame(uint8_t *frame);
|
||||
uint8_t generate_wheel_frame(uint8_t *frame,double CmdWheel);
|
||||
double cal_message_para(int s1, int s2, int s3, int s4, int exp);
|
||||
|
||||
void sensor_WheelMeas(double WheelTorqueBody[3], double WheelAM[3], double CmdWheel[3]);
|
||||
|
||||
#endif
|
||||
850
sensor/source/orbit_info.cpp
Normal file
850
sensor/source/orbit_info.cpp
Normal file
@@ -0,0 +1,850 @@
|
||||
|
||||
#include "orbit_info.h"
|
||||
// #include "TyxkDynamics.h"
|
||||
#include "GNCFunction.h"
|
||||
|
||||
#include "liblog.h"
|
||||
#include "json.hpp"
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include <fstream>
|
||||
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
// #include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
using namespace nlohmann;
|
||||
// using namespace Dynamics;
|
||||
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
double Epoch; //历元时间
|
||||
uint8_t pkg_Cnt;
|
||||
BASIC_CMDER CJB_cmder; //CJB轨道
|
||||
BASIC_CMDER Target_cmder; //目标星轨道
|
||||
} ORBIT_CMDER;
|
||||
|
||||
|
||||
ORBIT_CMDER orbit_cmder;
|
||||
|
||||
|
||||
|
||||
STEP_CMDER_RES s_rescmd = {0};
|
||||
|
||||
uint8_t set_orbit_flag = 0; //轨道初值赋值标志位
|
||||
uint8_t orbit_source_flag = 0; //轨道来源标志:0--使用主星传入轨道;1--使用程序默认轨道
|
||||
|
||||
|
||||
double Epoch_J2000 = 233881200; //历元信息，累计秒
|
||||
double posi_J2000[3] = {-41966956.6938834, 4089556.26746777, 92419.0202929388}; //J2000位置信息,m
|
||||
double velo_J2000[3] ={-298.1140306366,-3059.51295367263, 54.487742185087}; //J2000速度信息,m/s
|
||||
double Q_J2000[4] = {1.0,0.0,0.0,0.0}; //惯性系四元数信息
|
||||
|
||||
double QuatVVLH[4]= {1.0,0.0,0.0,0.0}; //姿态四元数信息
|
||||
|
||||
double Tar_posi_J2000[3] = {-41972144.5417670,4040287.04196300, 92381.2956310000};//目标性初始位置
|
||||
double Tar_velo_J2000[3] = {-294.436520000000,-3059.96985700000,54.3873050000000};//目标性初始速度
|
||||
|
||||
double AngularVelocity[3]; //惯性系角速度信息
|
||||
|
||||
double mjd_2017 = 57754; //2017.1.1 0:0:0 北京时
|
||||
//double mjd = 0;
|
||||
|
||||
double StepSize = 0.05; //动力学递推步长，s
|
||||
|
||||
|
||||
double ThrustForceBody[3], ChaSatMass;
|
||||
|
||||
void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI);
|
||||
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI);
|
||||
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval);
|
||||
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]);
|
||||
void Fun_orbit(double dots[6], double RV[6]);
|
||||
void Orbit_Runge_Kutta(double* PosVel, double h);
|
||||
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]);
|
||||
|
||||
/// @brief 轨道信息初始化
|
||||
/// @param
|
||||
/// @return 0:设置失败; 1:成功
|
||||
uint8_t orbit_info_initz(string path)
|
||||
{
|
||||
json sync_j;
|
||||
ifstream fin(path);
|
||||
|
||||
if(!fin)
|
||||
{
|
||||
cout<<"udp_sever_init配置文件打开失败,采用默认状态!"<<endl;
|
||||
spdlog::warn("函数{0}配置文件打开失败,采用默认状态!",__FUNCTION__);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(fin.good())
|
||||
{
|
||||
fin>>sync_j;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
orbit_source_flag = sync_j["orbit_source_flag"].get<uint8_t>();
|
||||
|
||||
// printf("orbit_source_flag is:%d",orbit_source_flag);
|
||||
StepSize = sync_j["StepSize"].get<double>();
|
||||
cout<<"StepSize1 is:"<<StepSize<<endl;
|
||||
|
||||
Epoch_J2000 = sync_j["Epoch_J2000"].get<double>();
|
||||
posi_J2000[0] = sync_j["posi_J2000_X"].get<double>();
|
||||
posi_J2000[1] = sync_j["posi_J2000_Y"].get<double>();
|
||||
posi_J2000[2] = sync_j["posi_J2000_Z"].get<double>();
|
||||
velo_J2000[0] = sync_j["velo_J2000_X"].get<double>();
|
||||
velo_J2000[1] = sync_j["velo_J2000_Y"].get<double>();
|
||||
velo_J2000[2] = sync_j["velo_J2000_Z"].get<double>();
|
||||
|
||||
Tar_posi_J2000[0] = sync_j["Tar_posi_J2000_X"].get<double>();
|
||||
Tar_posi_J2000[1] = sync_j["Tar_posi_J2000_Y"].get<double>();
|
||||
Tar_posi_J2000[2] = sync_j["Tar_posi_J2000_Z"].get<double>();
|
||||
Tar_velo_J2000[0] = sync_j["Tar_velo_J2000_X"].get<double>();
|
||||
Tar_velo_J2000[1] = sync_j["Tar_velo_J2000_Y"].get<double>();
|
||||
Tar_velo_J2000[2] = sync_j["Tar_velo_J2000_Z"].get<double>();
|
||||
|
||||
fin.close();
|
||||
|
||||
return 1;
|
||||
}
|
||||
/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
|
||||
/// 仅第一次赋值
|
||||
/// @param frame:输入量,轨道信息数组
|
||||
// 输出:frame
|
||||
// AttiVelECI[3]
|
||||
// EulerVVLH[3]
|
||||
// AttiMtxVVLH[3][3]
|
||||
// MtxECI2VVLH[3][3]
|
||||
/// @return 0:设置失败; 1:成功
|
||||
uint8_t set_orbit_info(uint8_t *frame,double AttiVelECI[3], double EulerVVLH[3], double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3])
|
||||
{
|
||||
|
||||
double orbInfo[6];
|
||||
double QuatECI[4],QuatECI2VVLH[4], QoiX[4][4];
|
||||
|
||||
if(orbit_source_flag==1) //如果使用自定义轨道则给定定值
|
||||
{
|
||||
if(!set_orbit_flag)
|
||||
{
|
||||
spdlog::info("轨道初始化成功！");
|
||||
//本体轨道信息
|
||||
// Epoch_J2000 = 261130015;// 233881200;
|
||||
// posi_J2000[0] = 23580586.0537251;//-41966956.6938834;//s_rescmd.resCmder.posi_x_J2000;
|
||||
// posi_J2000[1] = -34621080.4670552;//4089556.26746777;//s_rescmd.resCmder.posi_y_J2000;
|
||||
// posi_J2000[2] = 3658341.81666531;//92419.0202929388;//s_rescmd.resCmder.posi_z_J2000;
|
||||
// velo_J2000[0] = 2550.44489430168;//-298.1140306366;//s_rescmd.resCmder.velo_x_J2000;
|
||||
// velo_J2000[1] = 1733.51288095298;//-3059.51295367263;//s_rescmd.resCmder.velo_y_J2000;
|
||||
// velo_J2000[2] = -5.46126675290179;//54.487742185087;//s_rescmd.resCmder.velo_z_J2000;
|
||||
/* 七公里 */
|
||||
/*Epoch_J2000 = 261151615;
|
||||
posi_J2000[0] = 34676281.2753897;
|
||||
posi_J2000[1] = 24076270.7556943;
|
||||
posi_J2000[2] = -109962.795723338;
|
||||
velo_J2000[0] = -1737.82925849319;
|
||||
velo_J2000[1] = 2518.86637867070;
|
||||
velo_J2000[2] = -267.003526384092;*/
|
||||
/* 1公里 */
|
||||
/*Epoch_J2000 = 261162415;
|
||||
posi_J2000[0] = 7615206.74657380;
|
||||
posi_J2000[1] = 41520735.9749386;
|
||||
posi_J2000[2] = -2677030.64312120;
|
||||
velo_J2000[0] = -3011.20121409852;
|
||||
velo_J2000[1] = 545.623103173396;
|
||||
velo_J2000[2] = -183.096712053141;*/
|
||||
/* 150m */
|
||||
/*Epoch_J2000 = 261162415;
|
||||
posi_J2000[0] = 7615354.12442575;
|
||||
posi_J2000[1] = 41520709.5213564;
|
||||
posi_J2000[2] = -2677021.70014955;
|
||||
velo_J2000[0] = -3011.19925678085;
|
||||
velo_J2000[1] = 545.633775145026;
|
||||
velo_J2000[2] = -183.097400123677;*/
|
||||
|
||||
|
||||
//目标星轨道参数
|
||||
// Tar_posi_J2000[0] = 23621367.9190000;//-41972144.5417670;
|
||||
// Tar_posi_J2000[1] = -34592141.8670000;// 4040287.04196300;
|
||||
// Tar_posi_J2000[2] = 3657159.91500000;// 92381.2956310000;
|
||||
// Tar_velo_J2000[0] = 2548.34575500000;// -294.436520000000;
|
||||
// Tar_velo_J2000[1] = 1736.43994900000;// -3059.96985700000;
|
||||
// Tar_velo_J2000[2] = -5.86735000000000;// 54.3873050000000;
|
||||
/* 七公里 */
|
||||
/*Tar_posi_J2000[0] = 34672232.3889290;
|
||||
Tar_posi_J2000[1] = 24081737.4807088;
|
||||
Tar_posi_J2000[2] = -112563.071870978;
|
||||
Tar_velo_J2000[0] = -1738.23933197031;
|
||||
Tar_velo_J2000[1] = 2518.56390864701;
|
||||
Tar_velo_J2000[2] = -267.000903391630;*/
|
||||
/* 1公里 */
|
||||
/*Tar_posi_J2000[0] = 7616189.26558680;
|
||||
Tar_posi_J2000[1] = 41520559.6177237;
|
||||
Tar_posi_J2000[2] = -2676971.02331015;
|
||||
Tar_velo_J2000[0] = -3011.18816531403;
|
||||
Tar_velo_J2000[1] = 545.694249650931;
|
||||
Tar_velo_J2000[2] = -183.101299190051;*/
|
||||
/* 150m */
|
||||
/*Tar_posi_J2000[0] = 7615206.74657380;
|
||||
Tar_posi_J2000[1] = 41520735.9749386;
|
||||
Tar_posi_J2000[2] = -2677030.64312120;
|
||||
Tar_velo_J2000[0] = -3011.20121409852;
|
||||
Tar_velo_J2000[1] = 545.623103173396;
|
||||
Tar_velo_J2000[2] = -183.096712053141;*/
|
||||
|
||||
|
||||
//姿态角初值
|
||||
EulerVVLH[0] = 0.0;
|
||||
EulerVVLH[1] = 0.0;
|
||||
EulerVVLH[2] = 0.0;
|
||||
|
||||
AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
|
||||
AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
|
||||
AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
|
||||
|
||||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||||
|
||||
MtxJtoOGet(MtxECI2VVLH, orbInfo);
|
||||
|
||||
|
||||
A2q(QuatECI2VVLH, MtxECI2VVLH);
|
||||
qleft(QoiX, QuatECI2VVLH);
|
||||
mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
|
||||
Q_J2000[0] = QuatECI[0];
|
||||
Q_J2000[1] = QuatECI[1];
|
||||
Q_J2000[2] = QuatECI[2];
|
||||
Q_J2000[3] = QuatECI[3];
|
||||
|
||||
//角速度初值
|
||||
AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||||
AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||||
AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
|
||||
AttiVelECI[0] = AngularVelocity[0];
|
||||
AttiVelECI[1] = AngularVelocity[1];
|
||||
AttiVelECI[2] = AngularVelocity[2];
|
||||
|
||||
set_orbit_flag = 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//否则使用主星输入轨道
|
||||
else
|
||||
{
|
||||
if(!set_orbit_flag)
|
||||
{
|
||||
memcpy((uint8_t *)&orbit_cmder.Epoch,frame,sizeof(ORBIT_CMDER));
|
||||
set_orbit_flag = 1;
|
||||
|
||||
Epoch_J2000 = orbit_cmder.Epoch;
|
||||
posi_J2000[0] = orbit_cmder.CJB_cmder.posi_x_J2000;
|
||||
posi_J2000[1] = orbit_cmder.CJB_cmder.posi_y_J2000;
|
||||
posi_J2000[2] = orbit_cmder.CJB_cmder.posi_z_J2000;
|
||||
|
||||
velo_J2000[0] = orbit_cmder.CJB_cmder.velo_x_J2000;
|
||||
velo_J2000[1] = orbit_cmder.CJB_cmder.velo_y_J2000;
|
||||
velo_J2000[2] = orbit_cmder.CJB_cmder.velo_z_J2000;
|
||||
|
||||
Q_J2000[0] = orbit_cmder.CJB_cmder.q0_J2000;
|
||||
Q_J2000[1] = orbit_cmder.CJB_cmder.q1_J2000;
|
||||
Q_J2000[2] = orbit_cmder.CJB_cmder.q2_J2000;
|
||||
Q_J2000[3] = orbit_cmder.CJB_cmder.q3_J2000;
|
||||
|
||||
//目标性轨道参数
|
||||
Tar_posi_J2000[0] = orbit_cmder.Target_cmder.posi_x_J2000; //-41972144.5417670;
|
||||
Tar_posi_J2000[1] = orbit_cmder.Target_cmder.posi_y_J2000;//4040287.04196300;
|
||||
Tar_posi_J2000[2] = orbit_cmder.Target_cmder.posi_z_J2000;//92381.2956310000;
|
||||
Tar_velo_J2000[0] = orbit_cmder.Target_cmder.velo_x_J2000;//-294.436520000000;
|
||||
Tar_velo_J2000[1] = orbit_cmder.Target_cmder.velo_y_J2000;//-3059.96985700000;
|
||||
Tar_velo_J2000[2] = orbit_cmder.Target_cmder.velo_z_J2000;//54.3873050000000;
|
||||
|
||||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||||
|
||||
MtxJtoOGet(MtxECI2VVLH, orbInfo);
|
||||
|
||||
EulerVVLH[0] = 0.0;
|
||||
EulerVVLH[1] = 0.0;
|
||||
EulerVVLH[2] = 0.0;
|
||||
|
||||
AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
|
||||
AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
|
||||
AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
|
||||
|
||||
|
||||
AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;
|
||||
AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;
|
||||
AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;
|
||||
AttiVelECI[0] = AngularVelocity[0];
|
||||
AttiVelECI[1] = AngularVelocity[1];
|
||||
AttiVelECI[2] = AngularVelocity[2];
|
||||
|
||||
spdlog::info("函数{0}接收到的轨道历元为:{1:.3f},节点号为:{2:2d}, \
|
||||
位置为:{3:.3f},{4:.3f},{5:.3f}, \
|
||||
速度为:{6:.3f},{7:.3f},{8:.3f}, \
|
||||
四元数为:{9:.3f},{10:.3f},{11:.3f},{12:.3f}"
|
||||
,__FUNCTION__,Epoch_J2000,
|
||||
orbit_cmder.CJB_cmder.pack_id,
|
||||
posi_J2000[0],posi_J2000[1],posi_J2000[2],
|
||||
velo_J2000[0],velo_J2000[1],velo_J2000[2],
|
||||
Q_J2000[0],Q_J2000[1],Q_J2000[2],Q_J2000[3]);
|
||||
|
||||
spdlog::info("函数{0}接收到的目标星轨道历元为:{1:.3f},节点号为:{2:2d}, \
|
||||
位置为:{3:.3f},{4:.3f},{5:.3f}, \
|
||||
速度为:{6:.3f},{7:.3f},{8:.3f}"
|
||||
,__FUNCTION__,Epoch_J2000,
|
||||
orbit_cmder.Target_cmder.pack_id,
|
||||
Tar_posi_J2000[0],Tar_posi_J2000[1],Tar_posi_J2000[2],
|
||||
Tar_velo_J2000[0],Tar_velo_J2000[1],Tar_velo_J2000[2]);
|
||||
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
|
||||
/// 仅第一次赋值,后续根据初始赋值递推
|
||||
/// @param frame:输入量,轨道信息数组
|
||||
/// @return 0:设置失败; 1:成功
|
||||
/// /* 输入量 */
|
||||
/* SatMtxI 卫星转动惯量矩阵 */
|
||||
/* Tq 总力矩 */
|
||||
/* WheelAM 动量轮角动量 */
|
||||
//SatMass 卫星质量 从微推模型获取
|
||||
//SatMtxI 转动惯量 从微推模型获取
|
||||
/* 输出量 */
|
||||
/* AttiVelECI 角速度W^bi */
|
||||
/*orbInfo[6]*/
|
||||
/* double Tar_orbInfo[6] */
|
||||
// EulerVVLH[3] 欧拉角
|
||||
// AttiMtxVVLH[3][3] 姿态转移矩阵
|
||||
// MtxECI2VVLH 旋转矩阵
|
||||
|
||||
uint8_t generate_orbit_info(double orbInfo[6], double Tar_orbInfo[6], double AttiVelECI[3], double EulerVVLH[3],
|
||||
double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3],
|
||||
double* mjd, double* Epoch2017,
|
||||
double SatMass, double SatMtxI[3][3],
|
||||
double ThrustForce[3], double Tq[3], double WheelAM[3])
|
||||
{
|
||||
double dt_Orb;
|
||||
double h, AttiVelVVLH[3], QuatECI[4], OrbitVel[3], tInterval, QuatECI2VVLH[4], QoiX[4][4], PosNorm, VelNorm, W_Orbit;
|
||||
double mjd0, pv0[6], Tar_pv0[6];
|
||||
double pv[6], Tar_pv[6];
|
||||
int NUM = 10;
|
||||
|
||||
|
||||
dt_Orb = StepSize;
|
||||
h = StepSize / NUM;
|
||||
tInterval = StepSize;
|
||||
|
||||
mtxCpy((double*)ThrustForceBody, (double*)ThrustForce, 3, 1);
|
||||
ChaSatMass = SatMass;
|
||||
|
||||
mjd0 = Epoch_J2000 / 86400.0 - 8.0/24.0 + mjd_2017; //Epoch_J2000更名为Epoch_BJT
|
||||
pv0[0] = posi_J2000[0];
|
||||
pv0[1] = posi_J2000[1];
|
||||
pv0[2] = posi_J2000[2];
|
||||
pv0[3] = velo_J2000[0];
|
||||
pv0[4] = velo_J2000[1];
|
||||
pv0[5] = velo_J2000[2];
|
||||
*mjd = mjd0 + dt_Orb / 86400.0;
|
||||
//追踪星轨道动力学递推
|
||||
ORBIT_J6(dt_Orb, h, pv0, pv);
|
||||
MtxJtoOGet(MtxECI2VVLH, pv);
|
||||
mtxCpy((double*)orbInfo, (double*)pv, 6, 1);
|
||||
|
||||
//目标星轨道动力学递推
|
||||
ThrustForceBody[0] = 0.0; ThrustForceBody[1] = 0.0; ThrustForceBody[2] = 0.0;
|
||||
Tar_pv0[0] = Tar_posi_J2000[0];
|
||||
Tar_pv0[1] = Tar_posi_J2000[1];
|
||||
Tar_pv0[2] = Tar_posi_J2000[2];
|
||||
Tar_pv0[3] = Tar_velo_J2000[0];
|
||||
Tar_pv0[4] = Tar_velo_J2000[1];
|
||||
Tar_pv0[5] = Tar_velo_J2000[2];
|
||||
ORBIT_J6(dt_Orb, h, Tar_pv0, Tar_pv);
|
||||
mtxCpy((double*)Tar_orbInfo, (double*)Tar_pv, 6, 1);
|
||||
|
||||
//相对位置
|
||||
double DeltaPos[3],DeltaPosVVLH[3];
|
||||
DeltaPos[0] = pv0[0] - Tar_pv0[0];
|
||||
DeltaPos[1] = pv0[1] - Tar_pv0[1];
|
||||
DeltaPos[2] = pv0[2] - Tar_pv0[2];
|
||||
mtxMtp((double*)DeltaPosVVLH, (double*)MtxECI2VVLH, 3, 3, (double*)DeltaPos, 3, 1);
|
||||
|
||||
// 姿态动力学运动学递推
|
||||
PosNorm = norm(pv, 3);
|
||||
VelNorm = norm(&pv[3], 3);
|
||||
W_Orbit = VelNorm / PosNorm;
|
||||
OrbitVel[0] = 0.0;
|
||||
OrbitVel[1] = -1 * W_Orbit;
|
||||
OrbitVel[2] = 0.0;
|
||||
for (int i = 0; i < NUM; ++i) {
|
||||
|
||||
Runge_Kutta((double*)AngularVelocity, (double)h, (double*)Tq, (double*)WheelAM, (double*)SatMtxI);
|
||||
mtxCpy((double*)AttiVelECI, (double*)AngularVelocity, 3, 1);
|
||||
}
|
||||
KinenaticModel(AttiVelVVLH, AttiMtxVVLH, EulerVVLH, QuatVVLH, AttiVelECI, OrbitVel, tInterval);
|
||||
|
||||
Epoch_J2000 = Epoch_J2000 + dt_Orb;
|
||||
posi_J2000[0] = pv[0];
|
||||
posi_J2000[1] = pv[1];
|
||||
posi_J2000[2] = pv[2];
|
||||
velo_J2000[0] = pv[3];
|
||||
velo_J2000[1] = pv[4];
|
||||
velo_J2000[2] = pv[5];
|
||||
|
||||
Tar_posi_J2000[0] = Tar_pv[0];
|
||||
Tar_posi_J2000[1] = Tar_pv[1];
|
||||
Tar_posi_J2000[2] = Tar_pv[2];
|
||||
Tar_velo_J2000[0] = Tar_pv[3];
|
||||
Tar_velo_J2000[1] = Tar_pv[4];
|
||||
Tar_velo_J2000[2] = Tar_pv[5];
|
||||
|
||||
A2q(QuatECI2VVLH, MtxECI2VVLH);
|
||||
qleft(QoiX, QuatECI2VVLH);
|
||||
mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
|
||||
Q_J2000[0] = QuatECI[0];
|
||||
Q_J2000[1] = QuatECI[1];
|
||||
Q_J2000[2] = QuatECI[2];
|
||||
Q_J2000[3] = QuatECI[3];
|
||||
|
||||
*Epoch2017 =Epoch_J2000;
|
||||
|
||||
orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
|
||||
orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
|
||||
|
||||
Tar_orbInfo[0] = Tar_posi_J2000[0]; Tar_orbInfo[1] = Tar_posi_J2000[1]; Tar_orbInfo[2] = Tar_posi_J2000[2];
|
||||
Tar_orbInfo[3] = Tar_velo_J2000[0]; Tar_orbInfo[4] = Tar_velo_J2000[1]; Tar_orbInfo[5] = Tar_velo_J2000[2];
|
||||
|
||||
spdlog::info("函数{0}的QuatECI为:{1:.1f},{2:.5f},{3:.5f},{4:.5f},AttiVelECI为:{5:.5f},{6:.5f},{7:.5f},{8:.5f},{9:.5f},{10:.5f}",
|
||||
__FUNCTION__,
|
||||
QuatECI[0], QuatECI[1], QuatECI[2], QuatECI[3],
|
||||
AttiVelECI[0], AttiVelECI[1], AttiVelECI[2],
|
||||
DeltaPosVVLH[0],DeltaPosVVLH[1],DeltaPosVVLH[2]);
|
||||
|
||||
spdlog::info("函数{0}轨道信息:{1:.1f},{2:.5f},{3:.5f},{4:.5f},{5:.5f},{6:.5f},{7:.5f},\
|
||||
{8:.5f},{9:.5f},{10:.5f},{11:.5f},{12:.5f},{13:.5f},{14:.5f},{15:.5f}",
|
||||
__FUNCTION__,
|
||||
*mjd,
|
||||
posi_J2000[0],
|
||||
posi_J2000[1],
|
||||
posi_J2000[2],
|
||||
velo_J2000[0],
|
||||
velo_J2000[1],
|
||||
velo_J2000[2],
|
||||
Q_J2000[0],
|
||||
Q_J2000[1],
|
||||
Q_J2000[2],
|
||||
Q_J2000[3],
|
||||
AttiVelECI[0],
|
||||
AttiVelECI[1],
|
||||
AttiVelECI[2],
|
||||
Epoch_J2000);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取轨道信息;该函数供通信模块调用
|
||||
/// @param frame:输出量,轨道信息数组
|
||||
/// @return 1:成功
|
||||
uint8_t get_orbit_info(uint8_t *frame,uint8_t id)
|
||||
{
|
||||
|
||||
s_rescmd.Epoch = Epoch_J2000 ;
|
||||
s_rescmd.resCmder.pack_id = id;
|
||||
s_rescmd.resCmder.posi_x_J2000 = posi_J2000[0] ;
|
||||
s_rescmd.resCmder.posi_y_J2000 = posi_J2000[1] ;
|
||||
s_rescmd.resCmder.posi_z_J2000 = posi_J2000[2] ;
|
||||
|
||||
s_rescmd.resCmder.velo_x_J2000 = velo_J2000[0] ;
|
||||
s_rescmd.resCmder.velo_y_J2000 = velo_J2000[1] ;
|
||||
s_rescmd.resCmder.velo_z_J2000 = velo_J2000[2] ;
|
||||
|
||||
s_rescmd.resCmder.q0_J2000 = Q_J2000[0] ;
|
||||
s_rescmd.resCmder.q1_J2000 = Q_J2000[1] ;
|
||||
s_rescmd.resCmder.q2_J2000 = Q_J2000[2] ;
|
||||
s_rescmd.resCmder.q3_J2000 = Q_J2000[3] ;
|
||||
|
||||
spdlog::info("函数{0}反馈的轨道历元为:{1:.3f},节点号为:{2:2d},位置为:{3:.3f},速度为:{4:.3f},姿态为:{5:.3f}"
|
||||
,__FUNCTION__,Epoch_J2000,
|
||||
s_rescmd.resCmder.pack_id,
|
||||
posi_J2000[0],
|
||||
velo_J2000[0],
|
||||
Q_J2000[0]);
|
||||
|
||||
memcpy(frame,(uint8_t *)&s_rescmd.Epoch,sizeof(STEP_CMDER_RES));
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取J2000系历元信息
|
||||
/// @param Epoch:返回的历元数据
|
||||
/// @return 0:获取失败; 1:成功
|
||||
uint8_t get_orbit_epoic(double *Epoch)
|
||||
{
|
||||
*Epoch = Epoch_J2000;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取mjd
|
||||
/// @param Epoch:返回的位置数据
|
||||
/// @return 0:获取失败; 1:成功
|
||||
//double get_orbit_mjd(void)
|
||||
//{
|
||||
// return mjd;
|
||||
//}
|
||||
|
||||
|
||||
/// @brief 获取J2000系位置信息
|
||||
/// @param Epoch:返回的位置数据
|
||||
/// @return 0:获取失败; 1:成功
|
||||
uint8_t get_orbit_posi(double *posi)
|
||||
{
|
||||
posi[0] = posi_J2000[0];
|
||||
posi[1] = posi_J2000[1];
|
||||
posi[2] = posi_J2000[2];
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取J2000系速度信息
|
||||
/// @param Epoch:返回的速度数据
|
||||
/// @return 0:获取失败; 1:成功
|
||||
uint8_t get_orbit_velo(double *velo)
|
||||
{
|
||||
velo[0] = velo_J2000[0];
|
||||
velo[1] = velo_J2000[1];
|
||||
velo[2] = velo_J2000[2];
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取J2000系姿态四元数信息
|
||||
/// @param Epoch:返回的姿态四元数数据
|
||||
/// @return 0:获取失败; 1:成功
|
||||
uint8_t get_orbit_Q(double *Q)
|
||||
{
|
||||
Q[0] = Q_J2000[0];
|
||||
Q[1] = Q_J2000[1];
|
||||
Q[2] = Q_J2000[2];
|
||||
Q[3] = Q_J2000[3];
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief 获取轨道赋值标志位
|
||||
/// @param
|
||||
/// @return 轨道赋值标志位
|
||||
uint8_t get_orbit_flag(void)
|
||||
{
|
||||
return set_orbit_flag;
|
||||
}
|
||||
|
||||
/* 动力学模型 */
|
||||
void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI) {
|
||||
|
||||
/* 动力学模型模块 */
|
||||
/* 输入量 */
|
||||
/* w 角速度初值 */
|
||||
/* Tq 力矩 */
|
||||
/* WheelAM 动量轮角动量 */
|
||||
/* 输出量 */
|
||||
/* AttiVelDotECI 角加速度 */
|
||||
|
||||
double CrossMatrixW[3][3], IW[3], Hs[3];
|
||||
double temp1[3], temp2[3], temp3[3][3], temp4[3];
|
||||
|
||||
mtxMtp((double*)IW, (double*)SatMtxI, 3, 3, w, 3, 1);
|
||||
mtxAdd((double*)Hs, (double*)IW, (double*)WheelAM, 3, 1);
|
||||
CrossMatrixGet(CrossMatrixW, w);
|
||||
mtxMtp((double*)temp1, (double*)CrossMatrixW, 3, 3, (double*)Hs, 3, 1);
|
||||
mtxSub((double*)temp2, (double*)Tq, (double*)temp1, 3, 1);
|
||||
mtxInv((double*)temp3, (double*)SatMtxI, 3);
|
||||
mtxMtp((double*)AttiVelDotECI, (double*)temp3, 3, 3, (double*)temp2, 3, 1);
|
||||
}
|
||||
|
||||
|
||||
/* 四阶龙格库塔算法 */
|
||||
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI) {
|
||||
|
||||
/* 四阶龙格库塔算法模块 */
|
||||
/* 输入量 */
|
||||
/* AngularVelocity 角速度初值 */
|
||||
/* h 迭代步长 */
|
||||
/* Tq 力矩 */
|
||||
/* WheelAM 动量轮角动量 */
|
||||
/* 输出量 */
|
||||
/* AngularVelocity 角速度终值 */
|
||||
|
||||
double work[3], AttiVelDotECI[3], k0[3], k1[3], k2[3], k3[3];
|
||||
int j;
|
||||
|
||||
Dynamic_Model((double*)AttiVelDotECI, AngularVelocity, Tq, WheelAM, SatMtxI);//计算k1
|
||||
for (j = 0; j < 3; ++j)
|
||||
k0[j] = h * AttiVelDotECI[j];
|
||||
for (j = 0; j < 3; ++j)
|
||||
work[j] = *(AngularVelocity + j) + 0.5 * k0[j];//用数组work存储w的变化量
|
||||
|
||||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k2
|
||||
for (j = 0; j < 3; ++j)
|
||||
k1[j] = h * AttiVelDotECI[j];
|
||||
for (j = 0; j < 3; ++j)
|
||||
work[j] = *(AngularVelocity + j) + 0.5 * k1[j];//用数组work存储w的变化量
|
||||
|
||||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k3
|
||||
for (j = 0; j < 3; ++j)
|
||||
k2[j] = h * AttiVelDotECI[j];
|
||||
for (j = 0; j < 3; ++j)
|
||||
work[j] = *(AngularVelocity + j) + k2[j];//用数组work存储w的变化量
|
||||
|
||||
Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k4
|
||||
for (j = 0; j < 3; ++j)
|
||||
k3[j] = h * AttiVelDotECI[j];
|
||||
for (j = 0; j < 3; ++j)
|
||||
work[j] = *(AngularVelocity + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6;
|
||||
mtxCpy((double*)AngularVelocity, (double*)work, 3, 1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* 运动学更新模拟 */
|
||||
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval) {
|
||||
|
||||
/* 运动学更新模拟模块 */
|
||||
/* 输入量 */
|
||||
/* AttiVelECI 姿态角速度Wbi */
|
||||
/* OrbitVel 轨道角速度Woi */
|
||||
/* 输出量 */
|
||||
/* AttiVelVVLH 姿态角速度Wbo */
|
||||
/* AttiMtxVVLH 姿态矩阵Abo */
|
||||
/* QuatVVLH 姿态四元数qbo */
|
||||
|
||||
double Abo[3][3], Wo[3], woi[3], CrossVelVVLH[4][4], temp[4][4], qbo[4], Rc, ii;
|
||||
|
||||
TransQ(Abo, QuatVVLH);
|
||||
mtxMtp((double*)woi, (double*)Abo, 3, 3, (double*)OrbitVel, 3, 1);
|
||||
AttiVelVVLH[0] = AttiVelECI[0] - woi[0];
|
||||
AttiVelVVLH[1] = AttiVelECI[1] - woi[1];
|
||||
AttiVelVVLH[2] = AttiVelECI[2] - woi[2];
|
||||
|
||||
CrossVelVVLH[0][0] = 0.0; CrossVelVVLH[0][1] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[0][2] = -0.5 * AttiVelVVLH[1]; CrossVelVVLH[0][3] = -0.5 * AttiVelVVLH[2];
|
||||
CrossVelVVLH[1][0] = 0.5 * AttiVelVVLH[0]; CrossVelVVLH[1][1] = 0.0; CrossVelVVLH[1][2] = 0.5 * AttiVelVVLH[2]; CrossVelVVLH[1][3] = -0.5 * AttiVelVVLH[1];
|
||||
CrossVelVVLH[2][0] = 0.5 * AttiVelVVLH[1]; CrossVelVVLH[2][1] = -0.5 * AttiVelVVLH[2]; CrossVelVVLH[2][2] = 0.0; CrossVelVVLH[2][3] = 0.5 * AttiVelVVLH[0];
|
||||
CrossVelVVLH[3][0] = 0.5 * AttiVelVVLH[2]; CrossVelVVLH[3][1] = 0.5 * AttiVelVVLH[1]; CrossVelVVLH[3][2] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[3][3] = 0.0;
|
||||
mtxMtp((double*)temp, (double*)CrossVelVVLH, 4, 4, &tInterval, 1, 1);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
temp[i][i] += 1.0; /* temp = eye(4,4)+tInterval*0.5*(wbo叉乘矩阵） */
|
||||
}
|
||||
mtxCpy((double*)qbo, (double*)QuatVVLH, 4, 1);
|
||||
mtxMtp((double*)QuatVVLH, (double*)temp, 4, 4, (double*)qbo, 4, 1); /* qbo=qbo(上一时刻)+tInterval*0.5*(wbo叉乘矩阵）*qbo 运动学方程 */
|
||||
Rc = 1.0 / norm(QuatVVLH, 4);
|
||||
mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &Rc, 1, 1);
|
||||
ii = -1.0;
|
||||
if (QuatVVLH[0] < 0) {
|
||||
mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &ii, 1, 1);
|
||||
}
|
||||
|
||||
TransQ(AttiMtxVVLH, QuatVVLH);
|
||||
qua2euler(EulerVVLH, QuatVVLH, qbo);
|
||||
}
|
||||
|
||||
|
||||
/* 计算地球引力 */
|
||||
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]) {
|
||||
/*
|
||||
输入:地固系下的卫星位置矢量,m
|
||||
输出:地固系下的卫星加速度矢量，m/s2
|
||||
*/
|
||||
double GM = 398600.4418e9; //单位：m3/s3
|
||||
double Re = 6378.137e3; //单位：m
|
||||
double J2 = 1.08262617385222e-3;
|
||||
double J3 = -2.53241051856772e-6;
|
||||
double J4 = -1.61989759991697e-6;
|
||||
double J5 = -2.27753590730836e-7;
|
||||
double J6 = 5.40666576283813e-7;
|
||||
|
||||
double rx = R_ECEF[0];
|
||||
double ry = R_ECEF[1];
|
||||
double rz = R_ECEF[2];
|
||||
double r = norm(R_ECEF, 3);
|
||||
|
||||
// 中心引力加速度
|
||||
double Acc_TB[3];
|
||||
double r3 = pow(r, 3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
Acc_TB[i] = -1.0 * (GM / r3) * R_ECEF[i];
|
||||
}
|
||||
|
||||
// J2摄动加速度
|
||||
double r5 = pow(r, 5);
|
||||
double Re2 = pow(Re, 2);
|
||||
double k0 = -1.0 * (3.0 * GM * J2 * Re2) / (2 * r5);
|
||||
double k1 = 5.0 * rz * rz / (r * r);
|
||||
double Acc_J2[3];
|
||||
Acc_J2[0] = k0 * (1 - k1) * rx;
|
||||
Acc_J2[1] = k0 * (1 - k1) * ry;
|
||||
Acc_J2[2] = k0 * (3 - k1) * rz;
|
||||
|
||||
// J3摄动加速度
|
||||
double r7 = pow(r, 7);
|
||||
double Re3 = pow(Re, 3);
|
||||
double k01 = -1.0 * (5.0 * J3 * GM * Re3) / (2.0 * r7);
|
||||
double k11 = 3.0 * rz - 7.0 * rz * rz * rz / (r * r);
|
||||
double k21 = 6.0 * rz * rz - 7.0 * rz * rz * rz * rz / (r * r) - 3.0 / 5.0 * r * r;
|
||||
double Acc_J3[3];
|
||||
Acc_J3[0] = k01 * rx * k11;
|
||||
Acc_J3[1] = k01 * ry * k11;
|
||||
Acc_J3[2] = k01 * k21;
|
||||
|
||||
// J4摄动加速度
|
||||
//double r7 = pow(r, 7);
|
||||
double Re4 = pow(Re, 4);
|
||||
|
||||
double k02 = (15.0 * J4 * GM * Re4) / (8 * r7);
|
||||
double k12 = 1.0 - 14.0 * rz * rz / (r * r);
|
||||
double k22 = 5.0 - 70.0 * rz * rz / (3 * r * r);
|
||||
double k32 = 21.0 * rz * rz * rz * rz / (r * r * r * r);
|
||||
double Acc_J4[3];
|
||||
Acc_J4[0] = k02 * (k12 + k32) * rx;
|
||||
Acc_J4[1] = k02 * (k12 + k32) * ry;
|
||||
Acc_J4[2] = k02 * (k22 + k32) * rz;
|
||||
|
||||
// J5摄动加速度
|
||||
double r9 = pow(r, 9);
|
||||
double Re5 = pow(Re, 5);
|
||||
double k03 = (3.0 * J5 * GM * Re5 * rz) / (8 * r9);
|
||||
double k001 = rz * rz / (r * r);
|
||||
double k002 = pow(k001, 2);
|
||||
double k003 = pow(k001, 3);
|
||||
double k13 = 35.0 - 210.0 * k001 + 231.0 * k002;
|
||||
double k23 = 105.0 - 315.0 * k001 + 231.0 * k002;
|
||||
double Acc_J5[3];
|
||||
Acc_J5[0] = k03 * k13 * rx;
|
||||
Acc_J5[1] = k03 * k13 * ry;
|
||||
Acc_J5[2] = k03 * k23 * rz;
|
||||
Acc_J5[2] = Acc_J5[2] - 15.0 * J5 * GM * Re5 / (8 * r7);
|
||||
|
||||
// J6摄动加速度
|
||||
double Re6 = pow(Re, 6);
|
||||
double k04 = -1.0 * (J6 * GM * Re6) / (16.0 * r9);
|
||||
double k14 = 35.0 - 945.0 * k001 + 3465.0 * k002 - 3003.0 * k003;
|
||||
double k24 = 245.0 - 2205.0 * k001 + 4851.0 * k002 - 3003.0 * k003;
|
||||
double Acc_J6[3];
|
||||
Acc_J6[0] = k04 * k14 * rx;
|
||||
Acc_J6[1] = k04 * k14 * rz;//确定是rz吗?
|
||||
Acc_J6[2] = k04 * k24 * rz;
|
||||
|
||||
// 总加速度
|
||||
/*for (int i = 0; i < 3; i++) {
|
||||
Acc_Earth[i] = Acc_TB[i] + Acc_J2[i] + Acc_J3[i] + Acc_J4[i] + Acc_J5[i] + Acc_J6[i];
|
||||
}
|
||||
*/
|
||||
Acc_Earth[0] = Acc_TB[0] + Acc_J2[0] + Acc_J3[0] + Acc_J4[0] + Acc_J5[0] + Acc_J6[0];
|
||||
Acc_Earth[1] = Acc_TB[1] + Acc_J2[1] + Acc_J3[1] + Acc_J4[1] + Acc_J5[1] + Acc_J6[1];
|
||||
Acc_Earth[2] = Acc_TB[2] + Acc_J2[2] + Acc_J3[2] + Acc_J4[2] + Acc_J5[2] + Acc_J6[2];
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* 轨道动力学 */
|
||||
void Fun_orbit(double dots[6], double RV[6]) {
|
||||
|
||||
double Acc_Earth[3], SatPosECI[3], MtxECI2Body[3][3], MtxBody2ECI[3][3], ThrustForceECI[3], Acc_Thrust_ECI[3];
|
||||
|
||||
SatPosECI[0] = RV[0];
|
||||
SatPosECI[1] = RV[1];
|
||||
SatPosECI[2] = RV[2];
|
||||
Fun_AccEarth(Acc_Earth, SatPosECI);
|
||||
|
||||
// Acc_Thrust_ECI = MtxBody2ECI* 本体系下三轴推力 / 整星质量
|
||||
TransQ(MtxECI2Body, Q_J2000);
|
||||
mtxInv((double*)MtxBody2ECI, (double*)MtxECI2Body, 3);
|
||||
mtxMtp((double*)ThrustForceECI, (double*)MtxBody2ECI, 3, 3, (double*)ThrustForceBody, 3, 1);
|
||||
|
||||
dots[0] = RV[3];
|
||||
dots[1] = RV[4];
|
||||
dots[2] = RV[5];
|
||||
dots[3] = Acc_Earth[0] + ThrustForceECI[0] / ChaSatMass;
|
||||
dots[4] = Acc_Earth[1] + ThrustForceECI[1] / ChaSatMass;
|
||||
dots[5] = Acc_Earth[2] + ThrustForceECI[2] / ChaSatMass;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* 轨道的四阶龙格库塔算法 */
|
||||
void Orbit_Runge_Kutta(double* PosVel, double h) {
|
||||
/* 输入量 */
|
||||
/* RV 惯性系下位置速度初始值 */
|
||||
/* h 迭代步长 */
|
||||
|
||||
/* 输出量 */
|
||||
/* RV 迭代出的位置速度*/
|
||||
|
||||
double work[6], dots[6], k0[6], k1[6], k2[6], k3[6];
|
||||
int j;
|
||||
|
||||
Fun_orbit((double*)dots, PosVel);//计算k1
|
||||
for (j = 0; j < 6; ++j)
|
||||
k0[j] = h * dots[j];
|
||||
for (j = 0; j < 6; ++j)
|
||||
work[j] = *(PosVel + j) + 0.5 * k0[j];//用数组work存储w的变化量
|
||||
|
||||
Fun_orbit((double*)dots, (double*)work);//计算k2
|
||||
for (j = 0; j < 6; ++j)
|
||||
k1[j] = h * dots[j];
|
||||
for (j = 0; j < 6; ++j)
|
||||
work[j] = *(PosVel + j) + 0.5 * k1[j];//用数组work存储w的变化量
|
||||
|
||||
Fun_orbit((double*)dots, (double*)work);//计算k3
|
||||
for (j = 0; j < 6; ++j)
|
||||
k2[j] = h * dots[j];
|
||||
for (j = 0; j < 6; ++j)
|
||||
work[j] = *(PosVel + j) + k2[j];//用数组work存储w的变化量
|
||||
|
||||
Fun_orbit((double*)dots, (double*)work);//计算k4
|
||||
for (j = 0; j < 6; ++j)
|
||||
k3[j] = h * dots[j];
|
||||
for (j = 0; j < 6; ++j)
|
||||
work[j] = *(PosVel + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6.0;
|
||||
mtxCpy((double*)PosVel, (double*)work, 6, 1);
|
||||
}
|
||||
|
||||
|
||||
/* J6摄动轨道递推 */
|
||||
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]) {
|
||||
|
||||
double state[6];
|
||||
double t0 = 0;
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
state[i] = RV0[i];
|
||||
}
|
||||
|
||||
int steps = (int)((tf - t0) / dt) + 1;
|
||||
|
||||
for (int i = 1; i < steps; i++) {
|
||||
Orbit_Runge_Kutta((double*)state, dt);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
RV[i] = state[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t get_orbit_source_flag(void)
|
||||
{
|
||||
return orbit_source_flag;
|
||||
}
|
||||
|
||||
|
||||
|
||||
110
sensor/source/sensor_gnss.cpp
Normal file
110
sensor/source/sensor_gnss.cpp
Normal 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);
|
||||
}
|
||||
|
||||
141
sensor/source/sensor_gyro.cpp
Normal file
141
sensor/source/sensor_gyro.cpp
Normal 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;
|
||||
}
|
||||
139
sensor/source/sensor_mems.cpp
Normal file
139
sensor/source/sensor_mems.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
#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;
|
||||
}
|
||||
|
||||
338
sensor/source/sensor_optical.cpp
Normal file
338
sensor/source/sensor_optical.cpp
Normal file
@@ -0,0 +1,338 @@
|
||||
#include "sensor_optical.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"
|
||||
|
||||
void GetMeasDISRemote(double Dis_Remote[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double LOS_Remote[9], int TargetFLAG);
|
||||
void GetMeasDISClose(double Dis_Close[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double Euler[3], double R_Plane[2], int TargetFLAG);
|
||||
|
||||
/// @brief 生成小光电遥测帧
|
||||
/// @param frame:生成的遥测帧数据
|
||||
/// @return 生成帧的长度,生成失败返回0
|
||||
//输入: TarRVECI[6],目标轨道 从姿轨动力学获得
|
||||
// ChaRVECI[6],本体轨道 从姿轨动力学获得
|
||||
// MtxECI2VVLH[3][3], 转移矩阵 从姿轨动力学获得
|
||||
// LOS_Remote[9], 远距离相机测角，从TX2中获取
|
||||
// Euler[3], 姿态角 从姿轨动力学获得
|
||||
// R_Plane[2], 近距离相机，从TX2中获取
|
||||
// TargetFLAG 标记位 //XGD和TX2使用标记,1--左帆板, 0--本体
|
||||
//输出:frame 生成帧
|
||||
void generate_xgd_frame(uint8_t *frame, double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double LOS_Remote[9], double Euler[3], double R_Plane[2], int TargetFLAG)
|
||||
{
|
||||
static uint8_t frame_cnt = 0;
|
||||
OPTICAL_TLM_STR xgd;
|
||||
memset(frame,0,OPTIC_FRAME_LENGTH);
|
||||
memset( (uint8_t *)&xgd.FRAME_HEAD,0,OPTIC_FRAME_LENGTH);
|
||||
|
||||
|
||||
double Dis_Remote[2], Dis_Close[2];
|
||||
GetMeasDISRemote(Dis_Remote, TarRVECI, ChaRVECI, MtxECI2VVLH, LOS_Remote, TargetFLAG);
|
||||
GetMeasDISClose(Dis_Close, TarRVECI, ChaRVECI, MtxECI2VVLH, MtxVVLH2Body, Euler, R_Plane, TargetFLAG);
|
||||
|
||||
//printf("LOS_Remote is %10.3f \t %10.3f\r\n", Dis_Remote[0], Dis_Remote[1]);
|
||||
//printf("LOS_Close is %10.3f \t %10.3f\r\n", Dis_Remote[0], Dis_Remote[1]);
|
||||
spdlog::info("函数{0} Dis_Remote为:{1:.6f}, {2:.6f}, Dis_Close为:{3:.6f}, {4:.6f}",__FUNCTION__,
|
||||
Dis_Remote[0], Dis_Remote[1],Dis_Close[0], Dis_Close[1]);
|
||||
|
||||
xgd.FRAME_HEAD = 0x90EB;
|
||||
xgd.length = OPTIC_FRAME_LENGTH - 3;
|
||||
xgd.AI01 = convert_endian((float)Dis_Remote[1]);
|
||||
xgd.AI02 = convert_endian((float)Dis_Close[1]);
|
||||
xgd.AI03 = htons(2);
|
||||
xgd.AI19 = htonl(150);
|
||||
xgd.AI_far = (uint8_t)Dis_Remote[0];
|
||||
xgd.AI_close = (uint8_t)Dis_Close[0];
|
||||
xgd.AI10 = frame_cnt++;
|
||||
|
||||
|
||||
uint8_t checksum = 0;
|
||||
uint8_t *buffer = (uint8_t *)&xgd.FRAME_HEAD;
|
||||
for(int kc = 2;kc<OPTIC_FRAME_LENGTH-1;kc++)
|
||||
checksum +=buffer[kc];
|
||||
xgd.Check_sum = checksum;
|
||||
|
||||
spdlog::info("函数{0}OPTICAL数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
|
||||
sizeof(OPTICAL_TLM_STR),xgd.Check_sum);
|
||||
|
||||
memcpy(frame,(uint8_t *)&xgd.FRAME_HEAD,OPTIC_FRAME_LENGTH);
|
||||
}
|
||||
|
||||
|
||||
void generate_photo_frame(uint8_t *frame)
|
||||
{
|
||||
static uint16_t frame_cnt = 0;
|
||||
memset(frame,0,249);
|
||||
frame[0] = 0xEB;
|
||||
frame[1] = 0x91;
|
||||
frame[2] = 0;
|
||||
frame[3] = 245;
|
||||
frame[4] = (frame_cnt & 0xFF00)>>8;
|
||||
frame[5] = (frame_cnt & 0x00FF);
|
||||
|
||||
for(int kc = 6;kc<248;kc++) //不给校验位赋值
|
||||
{
|
||||
frame[kc] = frame_cnt+kc;
|
||||
}
|
||||
|
||||
if(frame_cnt++>=270) frame_cnt = 0;
|
||||
|
||||
for(int kc= 2;kc<248;kc++)
|
||||
frame[248] += frame[kc];
|
||||
}
|
||||
|
||||
|
||||
/* 远距离激光测距 */
|
||||
void GetMeasDISRemote(double Dis_Remote[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double LOS_Remote[9], int TargetFLAG) {
|
||||
|
||||
/* 远距离激光测距模拟模块 */
|
||||
/* 输入量 */
|
||||
/* TarRVECI 目标惯性系下RV */
|
||||
/* ChaRVECI 追踪星惯性系下RV */
|
||||
/* MtxECI2VVLH 追踪星惯性系到轨道系的旋转矩阵Aoi */
|
||||
/* LOS_Remote 远程测距角度信息 */
|
||||
/* TargetFLAG 目标选择标志 */
|
||||
/* 输出量 */
|
||||
/* Dis_Remote 远距离激光测距信息：标志位和距离*/
|
||||
|
||||
double Dis_Remote_Measure = 0.0;
|
||||
double LOSReal, Dis_Measure, temp[3];
|
||||
int LOS_Flag, DisRemoteFlag;
|
||||
static double qbo[4] = { 1, 0, 0, 0 };
|
||||
|
||||
double Alpha_Left_remote = LOS_Remote[2];
|
||||
double Beta_Left_remote = LOS_Remote[1];
|
||||
int LeftLOSRemoteFlag = LOS_Remote[0];
|
||||
double Alpha_Body_remote = LOS_Remote[5];
|
||||
double Beta_Body_remote = LOS_Remote[4];
|
||||
int BodyLOSRemoteFlag = LOS_Remote[3];
|
||||
double Alpha_Right_remote = LOS_Remote[8];
|
||||
double Beta_Right_remote = LOS_Remote[7];
|
||||
int RightLOSRemoteFlag = LOS_Remote[6];
|
||||
|
||||
// 真实值
|
||||
double RT[3] = { TarRVECI[0], TarRVECI[1], TarRVECI[2] };
|
||||
double RC[3] = { ChaRVECI[0], ChaRVECI[1], ChaRVECI[2] };
|
||||
double R_Co5[3] = { 0 };
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
R_Co5[i] += MtxECI2VVLH[i][j] * (RT[j] - RC[j]);
|
||||
}
|
||||
}
|
||||
double Dis_Body = norm(R_Co5, 3);
|
||||
double a[3] = { 0, -10.682, 0 };
|
||||
mtxAdd((double*)temp, (double*)R_Co5, (double*)a, 3, 1);
|
||||
double Dis_Fan_Left = norm(temp, 3);
|
||||
double b[3] = { 0, 10.682, 0 };
|
||||
mtxAdd((double*)temp, (double*)R_Co5, (double*)b, 3, 1);
|
||||
double Dis_Fan_Right = norm(temp, 3);
|
||||
|
||||
// 目标选择，0~本体，1~左帆板，2~右帆板,在OBC里做切换
|
||||
|
||||
/*if (TargetFLAG == 0)
|
||||
{
|
||||
Dis_Measure = Dis_Body;
|
||||
LOSReal = fabs(Beta_Body_remote - 90.0);
|
||||
LOS_Flag = BodyLOSRemoteFlag;
|
||||
}
|
||||
else if (TargetFLAG == 1)
|
||||
{
|
||||
Dis_Measure = Dis_Fan_Left;
|
||||
LOSReal = fabs(Beta_Left_remote - 90.0);
|
||||
LOS_Flag = LeftLOSRemoteFlag;
|
||||
}
|
||||
else if (TargetFLAG == 2)
|
||||
{
|
||||
Dis_Measure = Dis_Fan_Right;
|
||||
LOSReal = fabs(Beta_Right_remote - 90.0);
|
||||
LOS_Flag = RightLOSRemoteFlag;
|
||||
}*/
|
||||
Dis_Measure = Dis_Body;
|
||||
LOSReal = fabs(Beta_Body_remote - 90.0);
|
||||
LOS_Flag = BodyLOSRemoteFlag;
|
||||
|
||||
// 在测角有效、角度在4°以内，且距离在一定范围之内，测距有效
|
||||
if (LOS_Flag == 0 || LOSReal > 4.0 || Dis_Measure > 2000)
|
||||
{
|
||||
DisRemoteFlag = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
DisRemoteFlag = 1;
|
||||
}
|
||||
|
||||
if (DisRemoteFlag == 1)
|
||||
{
|
||||
if (Dis_Measure < 2000 && Dis_Measure >= 500)
|
||||
{
|
||||
Dis_Remote_Measure = Dis_Measure + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 1.0 / 3.0;
|
||||
}
|
||||
else if (Dis_Measure < 500 && Dis_Measure >= 50)
|
||||
{
|
||||
Dis_Remote_Measure = Dis_Measure + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3.0;
|
||||
}
|
||||
}
|
||||
|
||||
// 返回结果数组
|
||||
Dis_Remote[0] = DisRemoteFlag;
|
||||
Dis_Remote[1] = Dis_Remote_Measure;
|
||||
}
|
||||
|
||||
|
||||
/* 近距离激光测距 */
|
||||
void GetMeasDISClose(double Dis_Close[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double Euler[3], double R_Plane[2], int TargetFLAG) {
|
||||
|
||||
/* 近距离激光测距模拟模块 */
|
||||
/* 输入量 */
|
||||
/* TarRVECI 目标惯性系下RV */
|
||||
/* ChaRVECI 追踪星惯性系下RV */
|
||||
/* MtxECI2VVLH 追踪星惯性系到轨道系的旋转矩阵Aoi */
|
||||
/* Euler 姿态角（滚俯偏123）弧度制 */
|
||||
/* R_Plane */
|
||||
/* TargetFLAG 目标选择标志 */
|
||||
/* 输出量 */
|
||||
/* Dis_Close 近距离激光测距信息：标志位和距离 */
|
||||
|
||||
double Dis_Close_Measure = 0.0;
|
||||
int DisCloseFlag;
|
||||
double RVLVLH[6];
|
||||
double RLVLH[3];
|
||||
double PointLVLH[3];
|
||||
double MtxVVLH2LVLH[3][3] = { {0, 0,-1},
|
||||
{1, 0, 0},
|
||||
{0,-1, 0} };
|
||||
// 真实值
|
||||
double RT[3] = { TarRVECI[0], TarRVECI[1], TarRVECI[2] };
|
||||
double RC[3] = { ChaRVECI[0], ChaRVECI[1], ChaRVECI[2] };
|
||||
FunECI2LVLH(RLVLH, RVLVLH, ChaRVECI, TarRVECI);
|
||||
double R_Co5[3] = { 0 };
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
R_Co5[i] += MtxECI2VVLH[i][j] * (RT[j] - RC[j]);
|
||||
}
|
||||
}
|
||||
|
||||
//Euler是偏滚俯，这依次需要俯仰和偏航
|
||||
/*if (R_Co5[0] >= 70)
|
||||
{
|
||||
DisCloseFlag = 0;
|
||||
}
|
||||
else if (R_Co5[0] < 70 && R_Co5[0] >= 50)
|
||||
{
|
||||
Dis_Close_Measure = fabs(R_Co5[0] / (cos(Euler[2] * 3.1415926 / 180.0) * cos(Euler[0] * 3.1415926 / 180.0))) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.02 / 3.0;
|
||||
}
|
||||
else if (R_Co5[0] < 50 && R_Co5[0] >= 0)
|
||||
{
|
||||
Dis_Close_Measure = fabs(R_Co5[0] / (cos(Euler[2] * 3.1415926 / 180.0) * cos(Euler[0] * 3.1415926 / 180.0))) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.01 / 3.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
DisCloseFlag = 0;
|
||||
}*/
|
||||
if (R_Co5[0] >= 70)
|
||||
{
|
||||
DisCloseFlag = 0;
|
||||
}
|
||||
else if (R_Co5[0] < 70 && R_Co5[0] >= 50)
|
||||
{
|
||||
DisCloseFlag = 1;
|
||||
Dis_Close_Measure = fabs(R_Co5[0]) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.02 / 3.0;
|
||||
}
|
||||
else if (R_Co5[0] < 50 && R_Co5[0] >= 0)
|
||||
{
|
||||
DisCloseFlag = 1;
|
||||
Dis_Close_Measure = fabs(R_Co5[0]) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.01 / 3.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
DisCloseFlag = 0;
|
||||
}
|
||||
|
||||
// 判断激光测距是否有效
|
||||
/*if (TargetFLAG == 1)
|
||||
{
|
||||
if (R_Plane[0] < 0)
|
||||
{
|
||||
DisCloseFlag = 1;
|
||||
}
|
||||
}
|
||||
else if (TargetFLAG == 2)
|
||||
{
|
||||
if (R_Plane[1] > 0)
|
||||
{
|
||||
DisCloseFlag = 1;
|
||||
}
|
||||
}
|
||||
else if (TargetFLAG == 0)
|
||||
{
|
||||
DisCloseFlag = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
DisCloseFlag = 0;
|
||||
}*/
|
||||
|
||||
|
||||
/************ 2024.12.04 近距离激光是否有效与姿态相关 ************/
|
||||
double PointBody[3] = { -1, 0, 0 };
|
||||
double MtxBody2VVLH[3][3], PointVVLH[3], R_Co5_Norm, PointVVLHNorm[3], DeltaPointLVLH[3];
|
||||
// 计算激光向量在VVLH坐标系下指向
|
||||
mtxT((double*)MtxBody2VVLH, (double*)MtxVVLH2Body, 3, 3);
|
||||
mtxMtp((double*)PointVVLH, (double*)MtxBody2VVLH, 3, 3, (double*)PointBody, 3, 1);
|
||||
// 计算激光向量在LVLH面内投影
|
||||
R_Co5_Norm = norm(R_Co5, 3);
|
||||
PointVVLHNorm[0] = PointVVLH[0] * R_Co5_Norm;
|
||||
PointVVLHNorm[1] = PointVVLH[1] * R_Co5_Norm;
|
||||
PointVVLHNorm[2] = PointVVLH[2] * R_Co5_Norm;
|
||||
mtxMtp((double*)PointLVLH, (double*)MtxVVLH2LVLH, 3, 3, (double*)PointVVLHNorm, 3, 1);
|
||||
mtxSub(DeltaPointLVLH, RLVLH, PointLVLH, 3, 1);
|
||||
// 判断激光在LVLH面内投影是否落下卫星本体上
|
||||
// 卫星尺寸 本体1720 * 2400mm 三角长度1370mm 帆板1680 * 6000mm
|
||||
double a_Body = 1720 / 2 * 1e-3;
|
||||
double b_Body = 2400 / 2 * 1e-3;
|
||||
double c_Body = 2200 / 2 * 1e-3;
|
||||
double a_sanjiao = 1370 * 1e-3;
|
||||
double a_Left = 6000 / 2 * 1e-3;
|
||||
double b_Left = 1600 / 2 * 1e-3;
|
||||
int Flag_Body, Flag_Left;
|
||||
if ( DisCloseFlag == 1 ) {
|
||||
// 激光是否落在本体上
|
||||
if ((fabs(DeltaPointLVLH[2]) <= a_Body) && (fabs(DeltaPointLVLH[0]) <= b_Body)) {
|
||||
Flag_Body = 1;
|
||||
Dis_Close_Measure = Dis_Close_Measure - c_Body;
|
||||
}
|
||||
else {
|
||||
Flag_Body = 0;
|
||||
}
|
||||
// 激光落在帆板上
|
||||
if ((fabs(DeltaPointLVLH[2]) >= a_Body) && (fabs(DeltaPointLVLH[2]) <= (a_Body + a_sanjiao + a_Left)) && ((fabs(DeltaPointLVLH[0]) <= b_Left))) {
|
||||
Flag_Left = 1;
|
||||
}
|
||||
else {
|
||||
Flag_Left = 0;
|
||||
}
|
||||
if (Flag_Body == 1 || Flag_Left == 1) {
|
||||
DisCloseFlag = 1;
|
||||
}
|
||||
else {
|
||||
DisCloseFlag = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 返回结果数组
|
||||
Dis_Close[0] = DisCloseFlag;
|
||||
Dis_Close[1] = Dis_Close_Measure;
|
||||
}
|
||||
|
||||
|
||||
253
sensor/source/sensor_star.cpp
Normal file
253
sensor/source/sensor_star.cpp
Normal file
@@ -0,0 +1,253 @@
|
||||
#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;
|
||||
|
||||
}
|
||||
235
sensor/source/sensor_thrust.cpp
Normal file
235
sensor/source/sensor_thrust.cpp
Normal file
@@ -0,0 +1,235 @@
|
||||
#include "sensor_thrust.h"
|
||||
|
||||
#include "liblog.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <stdlib.h>
|
||||
#include "GNCFunction.h"
|
||||
|
||||
//static double ImpCost = 0.0; /* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||||
|
||||
void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12],
|
||||
double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);
|
||||
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod);
|
||||
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]);
|
||||
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);
|
||||
|
||||
|
||||
|
||||
//输出:ThrustForce[3], ThrustTorque[3],
|
||||
// SatMass, SatMtxI[3][3],
|
||||
//输入: JetPWMPlan[12]
|
||||
void sensor_ThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12])
|
||||
{
|
||||
double JetPWMPeriod, JetPos[3][12], JetDir[3][12], ThrustIsp, SatMassTemp1;
|
||||
|
||||
/*喷口PWM调制周期，即GNC控制周期，与控制程序周期相同*/
|
||||
JetPWMPeriod = 1;
|
||||
/*比例系数*/
|
||||
ThrustIsp = 343.0;
|
||||
/*本体系下的喷口安装位置，列向量 * 12，m*/
|
||||
JetPos[0][0] = -0.233; JetPos[0][1] = -0.233; JetPos[0][2] = 0.383; JetPos[0][3] = 0.383; JetPos[0][4] = 0.18; JetPos[0][5] = -0.18; JetPos[0][6] = 0.18; JetPos[0][7] = -0.18; JetPos[0][8] = 0; JetPos[0][9] = 0; JetPos[0][10] = 0; JetPos[0][11] = 0;
|
||||
JetPos[1][0] = 0; JetPos[1][1] = 0; JetPos[1][2] = 0; JetPos[1][3] = 0; JetPos[1][4] = -0.183; JetPos[1][5] = -0.183; JetPos[1][6] = 0.183; JetPos[1][7] = 0.183; JetPos[1][8] = 0.156; JetPos[1][9] = -0.156; JetPos[1][10] = 0.156; JetPos[1][11] = -0.156;
|
||||
JetPos[2][0] = 0.147; JetPos[2][1] = -0.147; JetPos[2][2] = 0.147; JetPos[2][3] = -0.147; JetPos[2][4] = 0; JetPos[2][5] = 0; JetPos[2][6] = 0; JetPos[2][7] = 0; JetPos[2][8] = -0.323; JetPos[2][9] = -0.323; JetPos[2][10] = 0.323; JetPos[2][11] = 0.323;
|
||||
/*本体系下的推力方向，列向量 * 12，单位方向矢量*/
|
||||
JetDir[0][0] = 1; JetDir[0][1] = 1; JetDir[0][2] = -1; JetDir[0][3] = -1; JetDir[0][4] = 0; JetDir[0][5] = 0; JetDir[0][6] = 0; JetDir[0][7] = 0; JetDir[0][8] = 0; JetDir[0][9] = 0; JetDir[0][10] = 0; JetDir[0][11] = 0;
|
||||
JetDir[1][0] = 0; JetDir[1][1] = 0; JetDir[1][2] = 0; JetDir[1][3] = 0; JetDir[1][4] = 1; JetDir[1][5] = 1; JetDir[1][6] = -1; JetDir[1][7] = -1; JetDir[1][8] = 0; JetDir[1][9] = 0; JetDir[1][10] = 0; JetDir[1][11] = 0;
|
||||
JetDir[2][0] = 0; JetDir[2][1] = 0; JetDir[2][2] = 0; JetDir[2][3] = 0; JetDir[2][4] = 0; JetDir[2][5] = 0; JetDir[2][6] = 0; JetDir[2][7] = 0; JetDir[2][8] = 1; JetDir[2][9] = 1; JetDir[2][10] = -1; JetDir[2][11] = -1;
|
||||
/*平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
|
||||
int Cmd = 1;
|
||||
/*初始推进剂质量，kg*/
|
||||
double InitGasMass = 3.55;//6.5 * 0.7692;
|
||||
/*平台质量，kg，5种工况，1 * 5*/
|
||||
double DryMassData[5] = { 48.381, 48.381, 46.013, 45.898, 43.53 };//{ 34.99881, 34.99881, 32.312115, 32.22625, 29.535285 };
|
||||
/*推进剂质心卫星位置，m，推力器布局坐标系*/
|
||||
double GasMassPos[3] = { -25 * 1e-3, 0.0, -201.44 * 1e-3 };
|
||||
/*平台质心位置，m，推力器布局坐标系，5种工况，3 * 5*/
|
||||
double DryMassPosData[3][5] = { {8.39 * 0.001, 8.03 * 0.001, 10.29 * 0.001, 10.19 * 0.001, 12.96 * 0.001},
|
||||
{ -8.92 * 0.001, -7.94 * 0.001, -15.98 * 0.001, 0.19 * 0.001, -8.00 * 0.001},
|
||||
{ -4.72 * 0.001, -4.72 * 0.001, 0.25 * 0.001, 1.30 * 0.001, 7.50 * 0.001} };
|
||||
/*初始推进剂转动惯量，kg·m3，取决于推进剂质心且对齐到推力器布局坐标系*/
|
||||
double InitGasMtxI[3][3] = { {0.0571059330000000, 0, -0.00222918000000000},
|
||||
{0, 0.0319816850000000, 0},
|
||||
{-0.00222918000000000, 0, 0.0678860230000000} };
|
||||
/*平台转动惯量，kg·m3，取决于平台质心且对齐到推力器布局坐标系，5种工况，3* (3 * 5)*/
|
||||
double DryMtxIData[3][15] = { {1.86687640893600, 0.0286157968140000, -0.439427130912000, 1.89572816175600, 0.0409105154700000, -0.437885017878000, 1.87455028889500, 0.0482311009420000, -0.445634256610000, 1.90556951219600, 0.0362650204340000, -0.452313634868000, 1.89121192802100, 0.0438251188440000, -0.462662149911000},
|
||||
{0.0286157968140000, 2.3271403853280, -0.0114310573740000, 0.0409105154700000, 2.31908819868000, -0.0163531346220000, 0.0482311009420000, 2.29461077938400, -0.00115876103900000, 0.0362650204340000, 2.32061908341600, -0.0323862321540000, 0.0438251188440000, 2.29928834970900, -0.0167186100900000},
|
||||
{-0.439427130912000, -0.0114310573740000, 1.20621470673000, -0.437885017878000, -0.0163531346220000, 1.22701427290200, -0.445634256610000, -0.00115876103900000, 1.17827923915600, -0.452313634868000, -0.0323862321540000, 1.19281841776200, -0.462662149911000, -0.0167186100900000, 1.14977779874100} };
|
||||
|
||||
GetThrustMeas(ThrustForce, ThrustTorque, &SatMassTemp1, SatMtxI, JetPWMPlan, JetPWMPeriod, JetPos, JetDir,
|
||||
ThrustIsp, InitGasMass, DryMassData, GasMassPos, DryMassPosData, Cmd, InitGasMtxI, DryMtxIData);
|
||||
*SatMass = SatMassTemp1;
|
||||
|
||||
//printf("ThrustForce is %10.5f \t %10.5f \t %10.5f\r\n", ThrustForce[0], ThrustForce[1], ThrustForce[2]);
|
||||
//printf("ThrustTorque is %10.5f \t %10.5f \t %10.5f\r\n", ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
|
||||
//spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},ThrustTorque为:{4:.6f}, {5:.6f}, {6:.6f}",__FUNCTION__,
|
||||
// ThrustForce[0], ThrustForce[1], ThrustForce[2],
|
||||
// ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
|
||||
spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},{4:.6f}, {5:.6f}, {6:.6f}, {7:.6f}", __FUNCTION__,
|
||||
ThrustForce[0], ThrustForce[1], ThrustForce[2],
|
||||
ThrustTorque[0], ThrustTorque[1], ThrustTorque[2],
|
||||
*SatMass);
|
||||
}
|
||||
|
||||
|
||||
/* 获得微推力器三轴推力、力矩以及气体消耗 */
|
||||
void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12],
|
||||
double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {
|
||||
|
||||
/* 微推力器模拟模块 */
|
||||
/* 输入量 */
|
||||
/* FullTankForceStd 充气后推力器推力标称值 */
|
||||
/* NullTankForceStd 触发充气推力器推力标称值 */
|
||||
/* TankForceDscPara 推力下降系数，N/(N*s) */
|
||||
/* TankGasTime 二级储箱充气时长 */
|
||||
/* JetPWMPlan 喷口PWM调制指令值，即GNC输出的推力器指令 */
|
||||
/* JetPWMPeriod 喷口PWM调制周期，即GNC控制周期 */
|
||||
/* tNowSec 仿真时刻，s */
|
||||
/* JetPos 本体系下的喷口安装位置，列向量*12，m */
|
||||
/* JetDir 本体系下的推力方向，列向量*12，单位方向矢量 */
|
||||
/* SatMassPos 二级储箱充气时长 */
|
||||
/* ThrustIsp 比例系数 */
|
||||
/* 输出量 */
|
||||
/* ThrustForce 推力器合力 */
|
||||
/* ThrustTorque 合力矩*/
|
||||
/* GasCost 储气消耗*/
|
||||
|
||||
static double ImpCost = 0.0; /* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||||
double JetForce[12], SatMassPos[3], GasCost, SatMassTemp;
|
||||
|
||||
GetJetForce(JetForce, JetPWMPlan, JetPWMPeriod);
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
ImpCost += JetForce[i];
|
||||
}
|
||||
GasCost = ImpCost / ThrustIsp;
|
||||
|
||||
SatMassProperty(&SatMassTemp, SatMassPos, SatMtxI, InitGasMass, DryMassData, GasMassPos, DryMassPosData, GasCost, Cmd, InitGasMtxI, DryMtxIData);
|
||||
*SatMass = SatMassTemp;
|
||||
Jet2Thrust(ThrustForce, ThrustTorque, JetForce, JetPos, JetDir, SatMassPos);
|
||||
}
|
||||
|
||||
|
||||
/* 推力器12喷口推力输出模拟 */
|
||||
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod) {
|
||||
|
||||
/* 推力器12喷口推力输出模拟模块 */
|
||||
/* 输入量 */
|
||||
/* FullTankForceStd 充气后推力器推力标称值 */
|
||||
/* NullTankForceStd 触发充气推力器推力标称值 */
|
||||
/* TankForceDscPara 推力下降系数，N/(N*s) */
|
||||
/* TankGasTime 二级储箱充气时长 */
|
||||
/* JetPWMPlan 喷口PWM调制指令值，即GNC输出的推力器指令 */
|
||||
/* JetPWMPeriod 喷口PWM调制周期，即GNC控制周期 */
|
||||
/* tNowSec 仿真时刻，s */
|
||||
/* ImpCost 从仿真开始起，推力器累计冲量消耗 */
|
||||
/* 输出量 */
|
||||
/* JetForce 推力器12喷口推力输出值（1s内平均推力） */
|
||||
|
||||
/*充气后推力器推力标称值*/
|
||||
double FullTankForceStd = 40 * 1e-3; //45 * 1e-3;
|
||||
// 当前二级储箱压力（推力）
|
||||
double TankForce = FullTankForceStd;
|
||||
|
||||
for (int ii = 0; ii < 12; ii++) {
|
||||
JetForce[ii] = TankForce * (JetPWMPlan[ii] / JetPWMPeriod);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* 获得本体系三轴推力和力矩 */
|
||||
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]) {
|
||||
|
||||
/* 推力器合力和合力矩模拟模块 */
|
||||
/* 输入量 */
|
||||
/* JetForce 推力器12喷口推力输出值（1s内平均推力） */
|
||||
/* JetPos 本体系下的喷口安装位置，列向量*12，m */
|
||||
/* JetDir 本体系下的推力方向，列向量*12，单位方向矢量 */
|
||||
/* SatMassPos 二级储箱充气时长 */
|
||||
/* 输出量 */
|
||||
/* ThrustForce 推力器合力 */
|
||||
/* ThrustTorque 合力矩*/
|
||||
|
||||
double iiJetForceVec[3], iiJetPosVec[3], iiJetTorque[3], ThrustForceTemp[3], ThrustTorqueTemp[3];
|
||||
|
||||
ThrustForceTemp[0] = 0.0; ThrustForceTemp[1] = 0.0; ThrustForceTemp[2] = 0.0;
|
||||
ThrustTorqueTemp[0] = 0.0; ThrustTorqueTemp[1] = 0.0; ThrustTorqueTemp[2] = 0.0;
|
||||
for (int ii = 0; ii < 12; ii++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
iiJetForceVec[j] = JetForce[ii] * JetDir[j][ii];
|
||||
}
|
||||
for (int j = 0; j < 3; j++) {
|
||||
iiJetPosVec[j] = JetPos[j][ii] - SatMassPos[j];
|
||||
}
|
||||
//iiJetTorque = cross( iiJetPosVec,iiJetForceVec );
|
||||
iiJetTorque[0] = iiJetPosVec[1] * iiJetForceVec[2] - iiJetPosVec[2] * iiJetForceVec[1];
|
||||
iiJetTorque[1] = iiJetPosVec[2] * iiJetForceVec[0] - iiJetPosVec[0] * iiJetForceVec[2];
|
||||
iiJetTorque[2] = iiJetPosVec[0] * iiJetForceVec[1] - iiJetPosVec[1] * iiJetForceVec[0];
|
||||
|
||||
/*mtxAdd((double*)ThrustForceTemp, (double*)ThrustForceTemp, (double*)iiJetForceVec, 3, 1);
|
||||
mtxAdd((double*)ThrustTorqueTemp, (double*)ThrustTorqueTemp, (double*)iiJetTorque, 3, 1);*/
|
||||
for (int j = 0; j < 3; j++) {
|
||||
ThrustForceTemp[j] += iiJetForceVec[j];
|
||||
ThrustTorqueTemp[j] += iiJetTorque[j];
|
||||
}
|
||||
}
|
||||
mtxCpy((double*)ThrustForce, (double*)ThrustForceTemp, 3, 1);
|
||||
mtxCpy((double*)ThrustTorque, (double*)ThrustTorqueTemp, 3, 1);
|
||||
}
|
||||
|
||||
|
||||
/* 追踪星质量特性 */
|
||||
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {
|
||||
|
||||
/* 推力器合力和合力矩模拟模块 */
|
||||
/* 输入量 */
|
||||
/* InitGasMass 初始推进剂质量，kg */
|
||||
/* DryMassData 平台质量，kg，5种工况，1*5； */
|
||||
/* GasMassPos 推进剂质心卫星位置，m，推力器布局坐标系 */
|
||||
/* DryMassPosData 平台质心位置，m，推力器布局坐标系，5种工况，3*5 */
|
||||
/* GasCost 累积冲量消耗质量，kg*/
|
||||
/* Cmd 平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
|
||||
/* 输出量 */
|
||||
/* SatMassPos 整星质心位置，m,推力器布局坐标系 */
|
||||
|
||||
//推进剂剩余质量
|
||||
double GasMass = InitGasMass - GasCost;
|
||||
//卫星平台质量，根据指令选择
|
||||
int ii = Cmd - 1;
|
||||
double DryMass = DryMassData[ii];
|
||||
//整星质量
|
||||
*SatMass = DryMass + GasMass;
|
||||
//卫星平台质心位置，根据指令选择
|
||||
double DryMassPos[3] = { DryMassPosData[0][ii], DryMassPosData[1][ii], DryMassPosData[2][ii] };
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
SatMassPos[i] = (GasMass * GasMassPos[i] + DryMass * DryMassPos[i]) / *SatMass;
|
||||
}
|
||||
//推进剂转动惯量，与剩余质量成正比
|
||||
double GasMtxI[3][3], DryMtxI[3][3], dGasPos[3], dDryPos[3], dGasPosMtx[3][3], dDryPosMtx[3][3], temp1[3][3], temp2[3][3];
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
GasMtxI[i][j] = (GasMass / InitGasMass) * InitGasMtxI[i][j];
|
||||
}
|
||||
}
|
||||
//卫星平台转动惯量，根据指令选择
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
DryMtxI[i][j] = DryMtxIData[i][3 * Cmd - 3 + j];
|
||||
}
|
||||
}
|
||||
//整星转动惯量计算
|
||||
mtxSub((double*)dGasPos, GasMassPos, (double*)SatMassPos, 3, 1);
|
||||
mtxSub((double*)dDryPos, DryMassPos, (double*)SatMassPos, 3, 1);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
dGasPosMtx[i][j] = dGasPos[i] * dGasPos[j] * GasMass;
|
||||
dDryPosMtx[i][j] = dDryPos[i] * dDryPos[j] * GasMass;
|
||||
}
|
||||
}
|
||||
mtxAdd((double*)temp1, (double*)GasMtxI, (double*)dGasPosMtx, 3, 3);
|
||||
mtxAdd((double*)temp2, (double*)DryMtxI, (double*)dDryPosMtx, 3, 3);
|
||||
mtxAdd((double*)SatMtxI, (double*)temp1, (double*)temp2, 3, 3);
|
||||
}
|
||||
1095
sensor/source/sensor_tx2.cpp
Normal file
1095
sensor/source/sensor_tx2.cpp
Normal file
File diff suppressed because it is too large
Load Diff
129
sensor/source/sensor_wheel.cpp
Normal file
129
sensor/source/sensor_wheel.cpp
Normal file
@@ -0,0 +1,129 @@
|
||||
#include "sensor_wheel.h"
|
||||
#include "orbit_info.h"
|
||||
#include "GNCFunction.h"
|
||||
|
||||
#include "libconvert.h"
|
||||
|
||||
#include "liblog.h"
|
||||
|
||||
|
||||
double WheelOutputPeriod = 1.0; //动量轮输出周期
|
||||
|
||||
|
||||
//其中k为被转换发送的数据，exp取1000，limit4 取4.294967296e9，数组a[]为转换后发送的4字节数据。
|
||||
void cal_cmder_para(double k, double exp, uint8_t *a);
|
||||
|
||||
/// @brief 生成遥测动量轮帧
|
||||
/// @param frame:生成的遥测帧数据
|
||||
/// @return:0
|
||||
uint8_t generate_wheel_frame(uint8_t *frame,double CmdWheel)
|
||||
{
|
||||
FlyWheel_Frame wheel;
|
||||
memset(frame,0,WHEEL_FRAME_LENGTH);
|
||||
memset( (uint8_t *)&wheel.header,0,WHEEL_FRAME_LENGTH);
|
||||
|
||||
wheel.header = 0xFF;
|
||||
//wheel.rotate_speed[3] = 10;
|
||||
cal_cmder_para(CmdWheel,WHEEL_EXP,wheel.rotate_speed);
|
||||
wheel.id = 0x11;
|
||||
|
||||
uint8_t checksum = 0;
|
||||
uint8_t *buffer = (uint8_t *)&wheel.header;
|
||||
for(int kc = 1;kc<WHEEL_FRAME_LENGTH-2;kc++)
|
||||
checksum +=buffer[kc];
|
||||
wheel.checksum = checksum;
|
||||
memcpy(frame,(uint8_t *)&wheel.header,WHEEL_FRAME_LENGTH);
|
||||
|
||||
spdlog::info("函数{0}动量轮数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
|
||||
sizeof(FlyWheel_Frame),frame[WHEEL_FRAME_LENGTH-1]);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void cal_cmder_para(double k, double exp, uint8_t *a)
|
||||
{
|
||||
//float limit4=4.294967296e9;
|
||||
unsigned long t;
|
||||
if(k<0) t=(unsigned long)(WHEEL_LIMIT4+k*exp);
|
||||
else t=(unsigned long)(k*exp);
|
||||
a[0]=(uint8_t)(t>>24)&0xff;
|
||||
a[1]=(uint8_t)(t>>16)&0xff;
|
||||
a[2]=(uint8_t)(t>>8)&0xff;
|
||||
a[3]=(uint8_t)t&0xff;
|
||||
}
|
||||
|
||||
|
||||
double cal_message_para(int s1, int s2, int s3, int s4, int exp)
|
||||
{
|
||||
//float limit4=4.294967296e9;
|
||||
double result=0;
|
||||
unsigned long k = 0;
|
||||
|
||||
k=(((unsigned long)s1<<24) & 0xff000000) | (((unsigned long)s2<<16) & 0xff0000) | (((unsigned long)s3<<8) & 0xff00) | ((unsigned long)s4 & 0xff);
|
||||
|
||||
if(k>0x7fffffff)
|
||||
{
|
||||
result = (double)k;
|
||||
result = (result-WHEEL_LIMIT4);
|
||||
result = result/exp;
|
||||
}
|
||||
else
|
||||
{
|
||||
result = (double)k;
|
||||
result = result/exp;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/* 动量轮输出模拟 */
|
||||
void sensor_WheelMeas(double WheelTorqueBody[3], double WheelAM[3], double CmdWheel[3]) {
|
||||
|
||||
/* 动量轮测量模拟模块 */
|
||||
/* 输入量 */
|
||||
/* WheelMtxI 动量轮转动惯量(主对角) */
|
||||
/* WheelMtxInstall 动量轮总安装矩阵 */
|
||||
/* CmdWheel 动量轮转速指令rpm*/
|
||||
/* WheelOutputPeriod 动量轮采样周期rpm*/
|
||||
/* 输出量 */
|
||||
/* MeasWheelRPM 动量轮输出值rpm转每分 */
|
||||
/* WheelTorqueBody 动量轮总力矩 */
|
||||
/* WheelAM 动量轮角动量 */
|
||||
|
||||
double wgi_true[3], DeltaWheelVel[3], temp[3], WheelTorque[3], WheelAMInstall[3], MeasWheelRPM[3];
|
||||
static double WheelRPM[3] = { 0, 0, 0 };
|
||||
double WheelMtxI = 1.273 * 1e-4;
|
||||
double WheelMtxInstall[3][3] = { {0, 1, 0},
|
||||
{-1, 0, 0},
|
||||
{0, 0, 1} };
|
||||
|
||||
MeasWheelRPM[0] = CmdWheel[0] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
|
||||
MeasWheelRPM[1] = CmdWheel[1] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
|
||||
MeasWheelRPM[2] = CmdWheel[2] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
|
||||
|
||||
DeltaWheelVel[0] = (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[0] * 2.0 * 3.1415926535 / 60.0);
|
||||
DeltaWheelVel[1] = (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[1] * 2.0 * 3.1415926535 / 60.0);
|
||||
DeltaWheelVel[2] = (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[2] * 2.0 * 3.1415926535 / 60.0);
|
||||
|
||||
temp[0] = DeltaWheelVel[0] / WheelOutputPeriod;
|
||||
temp[1] = DeltaWheelVel[1] / WheelOutputPeriod;
|
||||
temp[2] = DeltaWheelVel[2] / WheelOutputPeriod;
|
||||
|
||||
WheelTorque[0] = -1.0 * WheelMtxI * temp[0];
|
||||
WheelTorque[1] = -1.0 * WheelMtxI * temp[1];
|
||||
WheelTorque[2] = -1.0 * WheelMtxI * temp[2];
|
||||
|
||||
mtxMtp((double*)WheelTorqueBody, (double*)WheelMtxInstall, 3, 3, (double*)WheelTorque, 3, 1);
|
||||
|
||||
WheelAMInstall[0] = WheelMtxI * (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0);
|
||||
WheelAMInstall[1] = WheelMtxI * (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0);
|
||||
WheelAMInstall[2] = WheelMtxI * (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0);
|
||||
|
||||
mtxMtp((double*)WheelAM, (double*)WheelMtxInstall, 3, 3, (double*)WheelAMInstall, 3, 1);
|
||||
|
||||
WheelRPM[0] = CmdWheel[0];
|
||||
WheelRPM[1] = CmdWheel[1];
|
||||
WheelRPM[2] = CmdWheel[2];
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user