339 lines
10 KiB
C++
339 lines
10 KiB
C++
#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;
|
||
}
|
||
|
||
|