1096 lines
47 KiB
C++
1096 lines
47 KiB
C++
#include "sensor_tx2.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 GetMeasLOSRemote(double LOS_Remote[9], double TarRVECI[6], double ChaRVECI[6], double ChaMtxVVLH2Body[3][3], double ChaMtxECI2VVLH[3][3], int TargetFLAG);
|
||
void GetMeasLOSClose(double LOS_Close[9], double R_Plane[2], double TarRVECI[6], double ChaRVECI[6], double ChaMtxVVLH2Body[3][3], double ChaMtxECI2VVLH[3][3], int TargetFLAG);
|
||
|
||
/// @brief 生成TX2遥测帧
|
||
/// @param frame:生成的遥测帧数据
|
||
/// @return 生成帧的长度,生成失败返回0
|
||
|
||
//输出: frame 帧
|
||
// LOS_Remote[9] 远距离相机测角
|
||
// R_Plane[2] 近距离相机
|
||
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)
|
||
{
|
||
static uint8_t frame_cnt = 0;
|
||
VISION_TLM_STRUCT tx2;
|
||
memset( (uint8_t *)&tx2.FRAME_HEAD,0,VISION_Info_Length);
|
||
|
||
|
||
double LOS_Close[9];
|
||
GetMeasLOSRemote(LOS_Remote, TarRVECI, ChaRVECI, ChaMtxVVLH2Body, ChaMtxECI2VVLH, TargetFLAG);
|
||
GetMeasLOSClose(LOS_Close, R_Plane, TarRVECI, ChaRVECI, ChaMtxVVLH2Body, ChaMtxECI2VVLH, TargetFLAG);
|
||
|
||
//printf("LOS_Remote is %10.3f \t %10.3f \t %10.3f\r\n", LOS_Remote[3], LOS_Remote[4], LOS_Remote[5]);
|
||
//printf("LOS_Close is %10.3f \t %10.3f \t %10.3f\r\n", LOS_Close[3], LOS_Close[4], LOS_Close[5]);
|
||
/*spdlog::info("TarRVECI为:{1:.3f}, {2:.3f}, {3:.3f}, {4:.3f}, {5:.3f}, {6:.3f},ChaRVECI为:{7:.3f}, {8:.3f}, {9:.3f}, {10:.3f}, {11:.3f}, {12:.3f}", __FUNCTION__,
|
||
TarRVECI[0], TarRVECI[1], TarRVECI[2], TarRVECI[3], TarRVECI[4], TarRVECI[5], ChaRVECI[0], ChaRVECI[1], ChaRVECI[2], ChaRVECI[3], ChaRVECI[4], ChaRVECI[5]);*/
|
||
spdlog::info("LOS_Remote为:{1:.3f}, {2:.3f}, {3:.3f}, {4:.3f}, {5:.3f}, {6:.3f},{7:.3f}, {8:.3f}, {9:.3f}, LOS_Close为:{10:.3f}, {11:.3f}, {12:.3f},{13:.3f}, {14:.3f}, {15:.3f},{16:.3f}, {17:.3f}, {18:.3f}", __FUNCTION__,
|
||
LOS_Remote[0], LOS_Remote[1], LOS_Remote[2], LOS_Remote[3], LOS_Remote[4], LOS_Remote[5], LOS_Remote[6], LOS_Remote[7], LOS_Remote[8],
|
||
LOS_Close[0], LOS_Close[1], LOS_Close[2], LOS_Close[3], LOS_Close[4], LOS_Close[5], LOS_Close[6], LOS_Close[7], LOS_Close[8]);
|
||
|
||
|
||
tx2.FRAME_HEAD = 0x90EB;
|
||
tx2.Type_Code = 0xF0;
|
||
tx2.Code_Length = htons(VISION_Info_Length);
|
||
|
||
tx2.Image_flag = 0x12;
|
||
tx2.image_exist_flag.Flag0 = 0x07;
|
||
tx2.image_exist_flag.Flag1 = 0x04;
|
||
|
||
tx2.FRAME_RESERVE1 = 0xA18A;
|
||
|
||
tx2.TARGET_Horizontal = convert_endian(1024.0);
|
||
tx2.frame_noise_ratio = convert_endian(123.0);
|
||
tx2.Image_time_sec = htonl(15);
|
||
tx2.Image_time_ms =htons(13);
|
||
tx2.Telemetry_Counter = frame_cnt++;
|
||
|
||
tx2.Left_Sail_far.recog_Valid = (uint8_t)LOS_Remote[0];
|
||
tx2.Left_Sail_far.recog_Alpha = convert_endian((float)(LOS_Remote[1]));
|
||
tx2.Left_Sail_far.recog_Beta = convert_endian((float)(LOS_Remote[2]));
|
||
tx2.Self_plat_far.recog_Valid = (uint8_t)LOS_Remote[3];
|
||
tx2.Self_plat_far.recog_Alpha = convert_endian((float)(LOS_Remote[4]));
|
||
tx2.Self_plat_far.recog_Beta = convert_endian((float)(LOS_Remote[5]));
|
||
tx2.Right_Sail_far.recog_Valid = (uint8_t)LOS_Remote[6];
|
||
tx2.Right_Sail_far.recog_Alpha = convert_endian((float)(LOS_Remote[7]));
|
||
tx2.Right_Sail_far.recog_Beta = convert_endian((float)(LOS_Remote[8]));
|
||
|
||
tx2.Left_Sail_close.recog_Valid = (uint8_t)LOS_Close[0];
|
||
tx2.Left_Sail_close.recog_Alpha = convert_endian((float)(LOS_Close[1]));
|
||
tx2.Left_Sail_close.recog_Beta = convert_endian((float)(LOS_Close[2]));
|
||
tx2.Self_plat_close.recog_Valid = (uint8_t)LOS_Close[3];
|
||
tx2.Self_plat_close.recog_Alpha = convert_endian((float)(LOS_Close[4]));
|
||
tx2.Self_plat_close.recog_Beta = convert_endian((float)(LOS_Close[5]));
|
||
tx2.Right_Sail_close.recog_Valid = (uint8_t)LOS_Close[6];
|
||
tx2.Right_Sail_close.recog_Alpha = convert_endian((float)(LOS_Close[7]));
|
||
tx2.Right_Sail_close.recog_Beta = convert_endian((float)(LOS_Close[8]));
|
||
|
||
tx2.frame_noise_ratio_close = convert_endian(15.0);
|
||
tx2.frame_contrast_close = convert_endian(17.0);
|
||
uint8_t checksum = 0;
|
||
uint8_t *buffer = (uint8_t *)&tx2.FRAME_HEAD;
|
||
for(int kc = 2;kc<VISION_Info_Length-1;kc++)
|
||
checksum +=buffer[kc];
|
||
tx2.checksum = checksum;
|
||
|
||
|
||
spdlog::info("函数{0}TX2数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,sizeof(VISION_TLM_STRUCT),tx2.checksum);
|
||
memcpy(frame,(uint8_t *)&tx2.FRAME_HEAD,VISION_Info_Length);
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
|
||
void GetMeasLOSRemote(double LOS_Remote[9], double TarRVECI[6], double ChaRVECI[6], double ChaMtxVVLH2Body[3][3], double ChaMtxECI2VVLH[3][3], int TargetFLAG) {
|
||
|
||
/* 输入量 */
|
||
/* TarRVECI 目标星惯性系下RV */
|
||
/* ChaRVECI 追踪星惯性系下RV */
|
||
/* ChaMtxVVLH2Body 追踪星VVLH系到本体系的转换矩阵*/
|
||
/* ChaMtxECI2VVLH 追踪星VVLH系到惯性系的转换矩阵*/
|
||
/* TargetFLAG 目标标识符;*/
|
||
/* 输出量*/
|
||
/* LOS_Remote 远距离相机下测得的视线矢量*/
|
||
|
||
/*初始化*/
|
||
//相平面中位置坐标
|
||
double R_Plane1_remote[3] = { 0, 0, 0 };
|
||
double R_Plane2_remote[3] = { 0, 0, 0 };
|
||
double R_Plane3_remote[3] = { 0, 0, 0 };
|
||
double R_Plane4_remote[3] = { 0, 0, 0 };
|
||
double R_Plane5_remote[3] = { 0, 0, 0 };
|
||
double R_Plane6_remote[3] = { 0, 0, 0 };
|
||
double R_Plane7_remote[3] = { 0, 0, 0 };
|
||
double R_Plane8_remote[3] = { 0, 0, 0 };
|
||
double R_Plane9_remote[3] = { 0, 0, 0 };
|
||
|
||
//Beta高低角， Alpha方位角
|
||
double Beta_Left_remote = 0.0;
|
||
double Alpha_Left_remote = 0.0;
|
||
double Beta_Body_remote = 0.0;
|
||
double Alpha_Body_remote = 0.0;
|
||
double Beta_Right_remote = 0.0;
|
||
double Alpha_Right_remote = 0.0;
|
||
//测量值
|
||
double Alpha_Left_remote_Mea = 0.0;
|
||
double Beta_Left_remote_Mea = 0.0;
|
||
double Alpha_Body_remote_Mea = 0.0;
|
||
double Beta_Body_remote_Mea = 0.0;
|
||
double Alpha_Right_Remote_Mea = 0.0;
|
||
double Beta_Right_Remote_Mea = 0.0;
|
||
|
||
// 小孔成像焦距
|
||
double f_remote = 8.841e-3;
|
||
/*
|
||
追踪星中心VVLH下相对位置
|
||
1:左帆板左边缘；3：左帆板右边缘；4:本体左边缘；6：本体右边缘；7:右帆板左边缘；9：右帆板右边缘；
|
||
*/
|
||
double RT[3], RC[3];
|
||
RT[0] = TarRVECI[0]; RT[1] = TarRVECI[1]; RT[2] = TarRVECI[2];
|
||
RC[0] = ChaRVECI[0]; RC[1] = ChaRVECI[1]; RC[2] = ChaRVECI[2];
|
||
|
||
double R_Co5[3] = { 0 };
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
R_Co5[i] += ChaMtxECI2VVLH[i][j] * (RT[j] - RC[j]);
|
||
}
|
||
}
|
||
|
||
double R_Co1[3], R_Co3[3], R_Co4[3], R_Co6[3], R_Co7[3], R_Co9[3];
|
||
|
||
double a[3] = { 0, -8230e-3, 0 };
|
||
mtxAdd((double*)R_Co1, (double*)R_Co5, (double*)a, 3, 1);
|
||
|
||
double b[3] = { 0, -2230e-3, 0 };
|
||
mtxAdd((double*)R_Co3, (double*)R_Co5, (double*)b, 3, 1);
|
||
|
||
double c[3] = { 0, -860e-3, 0 };
|
||
mtxAdd((double*)R_Co4, (double*)R_Co5, (double*)c, 3, 1);
|
||
|
||
double d[3] = { 0, 860e-3, 0 };
|
||
mtxAdd((double*)R_Co6, (double*)R_Co5, (double*)d, 3, 1);
|
||
|
||
double e[3] = { 0, 2230e-3, 0 };
|
||
mtxAdd((double*)R_Co7, (double*)R_Co5, (double*)e, 3, 1);
|
||
|
||
double f[3] = { 0, 8230e-3, 0 };
|
||
mtxAdd((double*)R_Co9, (double*)R_Co5, (double*)f, 3, 1);
|
||
|
||
//相机坐标系原点在追踪星本体系下的位置矢量，暂无。
|
||
double r_cs[3] = { 0, 0, 0 };
|
||
/*追踪星本体坐标系到相机坐标系的转换矩阵,暂认为相机坐标系与本体坐标系重合，需要和小光电对接。
|
||
写到初始化函数里。 */
|
||
double A_sCb[3][3] = {
|
||
{ 1.0, 0, 0 },
|
||
{ 0, -1.0, 0 },
|
||
{ 0, 0, -1.0 }
|
||
};//
|
||
|
||
//误差矩阵
|
||
double A_SetErr[3][3] = {
|
||
{ 1.0000, 0.0017, 0 },
|
||
{ -0.0017, 1.000, 0.0035 },
|
||
{ 0, -0.0035, 1.0000 }
|
||
};
|
||
|
||
/*两星相对位置矢量由VVLH转本体，再转相机坐标系，相机安装误差暂没考虑
|
||
视线坐标系位置 = 安装误差*本体转视线（现均为VVLH）*（-安装矢量+追踪星轨道转本体*追踪星轨道系矢量）
|
||
ChaMtxVVLH2Body：追踪星VVLH到本体系转换矩阵
|
||
*/
|
||
double temp1_1[3], temp1_2[3], temp1_3[3], R_LOS1[3];
|
||
mtxMtp((double*)temp1_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co1, 3, 1);
|
||
mtxSub((double*)temp1_2, (double*)temp1_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp1_3, (double*)A_sCb, 3, 3, (double*)temp1_2, 3, 1);
|
||
mtxMtp((double*)R_LOS1, (double*)A_SetErr, 3, 3, (double*)temp1_3, 3, 1);
|
||
|
||
double temp3_1[3], temp3_2[3], temp3_3[3], R_LOS3[3];
|
||
mtxMtp((double*)temp3_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co3, 3, 1);
|
||
mtxSub((double*)temp3_2, (double*)temp3_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp3_3, (double*)A_sCb, 3, 3, (double*)temp3_2, 3, 1);
|
||
mtxMtp((double*)R_LOS3, (double*)A_SetErr, 3, 3, (double*)temp3_3, 3, 1);
|
||
|
||
double temp4_1[3], temp4_2[3], temp4_3[3], R_LOS4[3];
|
||
mtxMtp((double*)temp4_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co4, 3, 1);
|
||
mtxSub((double*)temp4_2, (double*)temp4_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp4_3, (double*)A_sCb, 3, 3, (double*)temp4_2, 3, 1);
|
||
mtxMtp((double*)R_LOS4, (double*)A_SetErr, 3, 3, (double*)temp4_3, 3, 1);
|
||
|
||
double temp6_1[3], temp6_2[3], temp6_3[3], R_LOS6[3];
|
||
mtxMtp((double*)temp6_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co6, 3, 1);
|
||
mtxSub((double*)temp6_2, (double*)temp6_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp6_3, (double*)A_sCb, 3, 3, (double*)temp6_2, 3, 1);
|
||
mtxMtp((double*)R_LOS6, (double*)A_SetErr, 3, 3, (double*)temp6_3, 3, 1);
|
||
|
||
double temp7_1[3], temp7_2[3], temp7_3[3], R_LOS7[3];
|
||
mtxMtp((double*)temp7_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co7, 3, 1);
|
||
mtxSub((double*)temp7_2, (double*)temp7_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp7_3, (double*)A_sCb, 3, 3, (double*)temp7_2, 3, 1);
|
||
mtxMtp((double*)R_LOS7, (double*)A_SetErr, 3, 3, (double*)temp7_3, 3, 1);
|
||
|
||
double temp9_1[3], temp9_2[3], temp9_3[3], R_LOS9[3];
|
||
mtxMtp((double*)temp9_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co9, 3, 1);
|
||
mtxSub((double*)temp9_2, (double*)temp9_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp9_3, (double*)A_sCb, 3, 3, (double*)temp9_2, 3, 1);
|
||
mtxMtp((double*)R_LOS9, (double*)A_SetErr, 3, 3, (double*)temp9_3, 3, 1);
|
||
|
||
|
||
/*
|
||
相机平面位置
|
||
远距离相机投影
|
||
*/
|
||
//将视线系数据转换到相平面，作为判据使用；f_remote：小孔成像焦距
|
||
// 1：左帆板左边缘
|
||
R_Plane1_remote[1] = -(R_LOS1[1] / R_LOS1[0]) * f_remote;
|
||
R_Plane1_remote[2] = -(R_LOS1[2] / R_LOS1[0]) * f_remote;
|
||
|
||
// 3：左帆板右边缘
|
||
R_Plane3_remote[1] = -(R_LOS3[1] / R_LOS3[0]) * f_remote;
|
||
R_Plane3_remote[2] = -(R_LOS3[2] / R_LOS3[0]) * f_remote;
|
||
|
||
// 4：本体左边缘
|
||
R_Plane4_remote[1] = -(R_LOS4[1] / R_LOS4[0]) * f_remote;
|
||
R_Plane4_remote[2] = -(R_LOS4[2] / R_LOS4[0]) * f_remote;
|
||
|
||
// 6：本体右边缘
|
||
R_Plane6_remote[1] = -(R_LOS6[1] / R_LOS6[0]) * f_remote;
|
||
R_Plane6_remote[2] = -(R_LOS6[2] / R_LOS6[0]) * f_remote;
|
||
|
||
// 7：右帆板左边缘
|
||
R_Plane7_remote[1] = -(R_LOS7[1] / R_LOS7[0]) * f_remote;
|
||
R_Plane7_remote[2] = -(R_LOS7[2] / R_LOS7[0]) * f_remote;
|
||
|
||
// 9：右帆板右边缘
|
||
R_Plane9_remote[1] = -(R_LOS9[1] / R_LOS9[0]) * f_remote;
|
||
R_Plane9_remote[2] = -(R_LOS9[2] / R_LOS9[0]) * f_remote;
|
||
|
||
|
||
/*
|
||
图像宽度
|
||
*/
|
||
const double Planelong_remote = 1024 * 5.5e-6;
|
||
// 图像左边缘
|
||
double Plane_left_remote[2] = { -Planelong_remote, 0 };
|
||
// 图像右边缘
|
||
double Plane_right_remote[2] = { Planelong_remote, 0 };
|
||
|
||
|
||
/*
|
||
远距离相机角度模拟
|
||
*/
|
||
int LeftLOSRemoteFlag = 1;
|
||
int BodyLOSRemoteFlag = 1;
|
||
int RightLOSRemoteFlag = 1;
|
||
//判断,在相平面上
|
||
// //当左帆板左边缘超过图像左边缘
|
||
if (Plane_left_remote[0] < R_Plane1_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane1_remote[1]) {
|
||
// 且左帆板左边缘超过图像右边缘，则3个标志位置零
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane3_remote[1]) {
|
||
// 若左帆板右边缘超过图像右边缘，
|
||
// 识别左帆板中心距y=（图像右边缘距离+左帆板左边缘距离）/2
|
||
R_Plane2_remote[1] = (Plane_right_remote[0] + R_Plane1_remote[1]) / 2.0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane4_remote[1]) {
|
||
// 本体左边缘超过图像右边缘，
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2
|
||
R_Plane2_remote[1] = (R_Plane3_remote[1] + R_Plane1_remote[1]) / 2.0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane6_remote[1]) {
|
||
// 本体右边缘超过图像右边缘,
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2,
|
||
// 识别本体中心y=（图像右边缘距离+本体左边缘距离）/2
|
||
R_Plane2_remote[1] = (R_Plane3_remote[1] + R_Plane1_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (Plane_right_remote[0] + R_Plane4_remote[1]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane7_remote[1]) {
|
||
// 右帆板左边缘超过图像右边缘,
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2,
|
||
// 识别本体中心y=（本体右边缘距离+本体左边缘距离）/2
|
||
R_Plane2_remote[1] = (R_Plane3_remote[1] + R_Plane1_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane6_remote[1] + R_Plane4_remote[1]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
// 右帆板右边缘超过图像右边缘
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2
|
||
// 识别本体中心y=（图像右边缘距离+本体左边缘距离）/2
|
||
// 识别右帆板中心距y=（图像右边缘距离+右帆板左边缘距离）/2
|
||
R_Plane2_remote[1] = (R_Plane3_remote[1] + R_Plane1_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane6_remote[1] + R_Plane4_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (Plane_right_remote[0] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
else {
|
||
R_Plane2_remote[1] = (R_Plane3_remote[1] + R_Plane1_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane6_remote[1] + R_Plane4_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane9_remote[1] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
// //当左帆板右边缘超过图像左边缘
|
||
else if (Plane_left_remote[0] < R_Plane3_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane3_remote[1]) {
|
||
// 当左帆板右边缘超过图像右边缘
|
||
// 识别左帆板中心距y=（图像左边缘距离+图像右边缘距离）/2
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + Plane_right_remote[0]) / 2.0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane4_remote[1]) {
|
||
// 本体左边缘超过图像右边缘
|
||
// 识别左帆板中心距y=（图像左边缘距离+左帆板右边缘距离）/2
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + R_Plane3_remote[1]) / 2.0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane6_remote[1]) {
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + R_Plane3_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane7_remote[1]) {
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + R_Plane3_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + R_Plane3_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane7_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
}
|
||
else {
|
||
R_Plane2_remote[1] = (Plane_left_remote[0] + R_Plane3_remote[1]) / 2.0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane9_remote[1] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
// // 当本体左边缘超过图像左边缘
|
||
else if (Plane_left_remote[0] < R_Plane4_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane4_remote[1]) {
|
||
// 当本体左边缘超过图像右边缘
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane6_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane7_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane7_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (R_Plane4_remote[1] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane9_remote[1] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
// // 当本体右边缘超过图像左边缘
|
||
else if (Plane_left_remote[0] < R_Plane6_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane6_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (Plane_left_remote[0] + Plane_right_remote[0]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane7_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (Plane_left_remote[0] + R_Plane6_remote[1]) / 2.0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (Plane_left_remote[0] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane7_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSRemoteFlag = 0;
|
||
R_Plane5_remote[1] = (Plane_left_remote[0] + R_Plane6_remote[1]) / 2.0;
|
||
R_Plane8_remote[1] = (R_Plane9_remote[1] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
// // 当右帆板左边缘超过图像左边缘
|
||
else if (Plane_left_remote[0] < R_Plane7_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane7_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
R_Plane8_remote[1] = (R_Plane7_remote[1] + Plane_right_remote[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
R_Plane8_remote[1] = (R_Plane9_remote[1] + R_Plane7_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
// // 当右帆板右边缘超过图像左边缘
|
||
else if (Plane_left_remote[0] < R_Plane9_remote[1]) {
|
||
if (Plane_right_remote[0] < R_Plane9_remote[1]) {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
R_Plane8_remote[1] = (Plane_left_remote[0] + Plane_right_remote[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
R_Plane8_remote[1] = (Plane_left_remote[0] + R_Plane9_remote[1]) / 2.0;
|
||
}
|
||
}
|
||
else {
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
|
||
//计算相平面坐标系下目标点的位置
|
||
|
||
if (LeftLOSRemoteFlag == 1) {
|
||
//识别左帆板中心距 z=（左帆板左边缘距离 z+左帆板右边缘距离 z）/2
|
||
//识别左帆板中心距 x= 小孔成像焦距
|
||
R_Plane2_remote[2] = (R_Plane1_remote[2] + R_Plane3_remote[2]) / 2.0;
|
||
R_Plane2_remote[0] = f_remote;
|
||
}
|
||
|
||
if (BodyLOSRemoteFlag == 1) {
|
||
R_Plane5_remote[2] = (R_Plane4_remote[2] + R_Plane6_remote[2]) / 2.0;
|
||
R_Plane5_remote[0] = f_remote;
|
||
}
|
||
|
||
if (RightLOSRemoteFlag == 1) {
|
||
R_Plane8_remote[2] = (R_Plane7_remote[2] + R_Plane9_remote[2]) / 2.0;
|
||
R_Plane8_remote[0] = f_remote;
|
||
}
|
||
|
||
|
||
// 归一化
|
||
double norm_R_Plane2 = norm(R_Plane2_remote, 3);
|
||
R_Plane2_remote[0] /= norm_R_Plane2;
|
||
R_Plane2_remote[1] /= norm_R_Plane2;
|
||
R_Plane2_remote[2] /= norm_R_Plane2;
|
||
|
||
double norm_R_Plane5 = norm(R_Plane5_remote, 3);
|
||
R_Plane5_remote[0] /= norm_R_Plane5;
|
||
R_Plane5_remote[1] /= norm_R_Plane5;
|
||
R_Plane5_remote[2] /= norm_R_Plane5;
|
||
|
||
double norm_R_Plane8 = norm(R_Plane8_remote, 3);
|
||
R_Plane8_remote[0] /= norm_R_Plane8;
|
||
R_Plane8_remote[1] /= norm_R_Plane8;
|
||
R_Plane8_remote[2] /= norm_R_Plane8;
|
||
|
||
//相机的alpha与beta，和视线坐标系定义不同
|
||
// 计算方位角和高低角
|
||
if (LeftLOSRemoteFlag == 1) {
|
||
Beta_Left_remote = asin(R_Plane2_remote[0]);
|
||
Alpha_Left_remote = atan2(R_Plane2_remote[2], R_Plane2_remote[1]);
|
||
}
|
||
|
||
if (BodyLOSRemoteFlag == 1) {
|
||
Beta_Body_remote = asin(R_Plane5_remote[0]);
|
||
Alpha_Body_remote = atan2(R_Plane5_remote[2], R_Plane5_remote[1]);
|
||
}
|
||
|
||
if (RightLOSRemoteFlag == 1) {
|
||
Beta_Right_remote = asin(R_Plane8_remote[0]);
|
||
Alpha_Right_remote = atan2(R_Plane8_remote[2], R_Plane8_remote[1]);
|
||
}
|
||
|
||
//相机判断
|
||
double PI = 3.1415926535;
|
||
double Dis_Body = norm(R_Co5, 3);
|
||
double Dis_Fan_Left = sqrt(R_Co5[0] * R_Co5[0] + pow((R_Co5[1] - 10.682), 2) + R_Co5[2] * R_Co5[2]);
|
||
double Dis_Fan_Right = sqrt(R_Co5[0] * R_Co5[0] + pow((R_Co5[1] + 10.682), 2) + R_Co5[2] * R_Co5[2]);
|
||
|
||
// 目标选择,0~本体，1~左帆板，2~右帆板,在OBC里做切换
|
||
double Dis_Measure;
|
||
/* if (TargetFLAG == 0) {
|
||
Dis_Measure = Dis_Body;
|
||
}
|
||
else if (TargetFLAG == 1) {
|
||
Dis_Measure = Dis_Fan_Left;
|
||
}
|
||
else if (TargetFLAG == 2) {
|
||
Dis_Measure = Dis_Fan_Right;
|
||
}
|
||
else {
|
||
Dis_Measure = 0;
|
||
}*/
|
||
Dis_Measure = Dis_Body;
|
||
|
||
// 根据目标距离进行操作
|
||
if (fabs(Dis_Measure) >= 15000) { // 小光电测角最大距离20km
|
||
LeftLOSRemoteFlag = 0;
|
||
BodyLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Dis_Measure < 15000 && Dis_Measure >= 870) { // 大于870m开始面目标测量
|
||
Alpha_Body_remote_Mea = Alpha_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Body_remote_Mea = Beta_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
LeftLOSRemoteFlag = 0;
|
||
RightLOSRemoteFlag = 0;
|
||
}
|
||
else if (Dis_Measure < 870 && Dis_Measure >= 50) {
|
||
Alpha_Left_remote_Mea = Alpha_Left_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Left_remote_Mea = Beta_Left_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Alpha_Body_remote_Mea = Alpha_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Body_remote_Mea = Beta_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Alpha_Right_Remote_Mea = Alpha_Right_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Right_Remote_Mea = Beta_Right_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
}
|
||
else if (Dis_Measure < 50 && Dis_Measure >= 5) {
|
||
Alpha_Left_remote_Mea = Alpha_Left_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Left_remote_Mea = Beta_Left_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Alpha_Body_remote_Mea = Alpha_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Body_remote_Mea = Beta_Body_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Alpha_Right_Remote_Mea = Alpha_Right_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Right_Remote_Mea = Beta_Right_remote + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
}
|
||
|
||
// 如果相应标志位为0，则设置测量角度为0
|
||
if (LeftLOSRemoteFlag == 0) {
|
||
Alpha_Left_remote_Mea = 0;
|
||
Beta_Left_remote_Mea = 0;
|
||
}
|
||
if (BodyLOSRemoteFlag == 0) {
|
||
Alpha_Body_remote_Mea = 0;
|
||
Beta_Body_remote_Mea = 0;
|
||
}
|
||
if (RightLOSRemoteFlag == 0) {
|
||
Alpha_Right_Remote_Mea = 0;
|
||
Beta_Right_Remote_Mea = 0;
|
||
}
|
||
|
||
//返回结果数组
|
||
//LOS_Remote[0] = LeftLOSRemoteFlag;
|
||
//LOS_Remote[1] = Alpha_Left_remote_Mea;//方位角——面内角
|
||
//LOS_Remote[2] = Beta_Left_remote_Mea;//高低角——面外角
|
||
//LOS_Remote[3] = BodyLOSRemoteFlag;
|
||
//LOS_Remote[4] = Alpha_Body_remote_Mea;
|
||
//LOS_Remote[5] = Beta_Body_remote_Mea;
|
||
//LOS_Remote[6] = RightLOSRemoteFlag;
|
||
//LOS_Remote[7] = Alpha_Right_Remote_Mea;
|
||
//LOS_Remote[8] = Beta_Right_Remote_Mea;
|
||
LOS_Remote[0] = 0.0;
|
||
LOS_Remote[1] = Beta_Left_remote_Mea * 180.0 / PI;//高低角——面外角
|
||
LOS_Remote[2] = Alpha_Left_remote_Mea * 180.0 / PI;//方位角——面内角
|
||
LOS_Remote[3] = BodyLOSRemoteFlag;
|
||
LOS_Remote[4] = Beta_Body_remote_Mea * 180.0 / PI;
|
||
LOS_Remote[5] = Alpha_Body_remote_Mea * 180.0 / PI;
|
||
LOS_Remote[6] = 0.0;
|
||
LOS_Remote[7] = Beta_Right_Remote_Mea * 180.0 / PI;
|
||
LOS_Remote[8] = Alpha_Right_Remote_Mea * 180.0 / PI;
|
||
//printf("LOS_Remote is %10.3f \t %10.3f \t %10.3f\r\n", LOS_Remote[3], LOS_Remote[4], LOS_Remote[5]);
|
||
}
|
||
|
||
|
||
void GetMeasLOSClose(double LOS_Close[9], double R_Plane[2], double TarRVECI[6], double ChaRVECI[6], double ChaMtxVVLH2Body[3][3], double ChaMtxECI2VVLH[3][3], int TargetFLAG) {
|
||
/* 输入量 */
|
||
/* TarRVECI 目标星惯性系下RV */
|
||
/* ChaRVECI 追踪星惯性系下RV */
|
||
/* ChaMtxVVLH2Body 追踪星VVLH系到本体的转换矩阵*/
|
||
/* ChaMtxECI2VVLH 追踪星惯性系到VVLH系的旋转矩阵*/
|
||
/* TargetFLAG 目标标识符*/
|
||
/* 输出量*/
|
||
/* LOS_Close 近距离相机下测得的视线矢量*/
|
||
/* R_Plane 近距离相机下测得的视线矢量在Body系下的坐标*/
|
||
|
||
// 初始化
|
||
double R_Plane1_Close[3] = { 0, 0, 0 };
|
||
double R_Plane2_Close[3] = { 0, 0, 0 };
|
||
double R_Plane3_Close[3] = { 0, 0, 0 };
|
||
double R_Plane4_Close[3] = { 0, 0, 0 };
|
||
double R_Plane5_Close[3] = { 0, 0, 0 };
|
||
double R_Plane6_Close[3] = { 0, 0, 0 };
|
||
double R_Plane7_Close[3] = { 0, 0, 0 };
|
||
double R_Plane8_Close[3] = { 0, 0, 0 };
|
||
double R_Plane9_Close[3] = { 0, 0, 0 };
|
||
|
||
double Beta_Left_Close = 0.0;
|
||
double Alpha_Left_Close = 0.0;
|
||
double Beta_Body_Close = 0.0;
|
||
double Alpha_Body_Close = 0.0;
|
||
double Beta_Right_Close = 0.0;
|
||
double Alpha_Right_Close = 0.0;
|
||
double Alpha_Left_Close_Mea = 0.0;
|
||
double Beta_Left_Close_Mea = 0.0;
|
||
double Alpha_Body_Close_Mea = 0.0;
|
||
double Beta_Body_Close_Mea = 0.0;
|
||
double Alpha_Right_Close_Mea = 0.0;
|
||
double Beta_Right_Close_Mea = 0.0;
|
||
|
||
// 小孔成像焦距
|
||
double f_Close = 5.071 * 1e-3;
|
||
|
||
/*%追踪星中心VVLH下相对位置
|
||
1:左帆板左边缘；3：左帆板右边缘；4:本体左边缘；
|
||
6：本体右边缘；7:右帆板左边缘；9：右帆板右边缘；
|
||
*/
|
||
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] += ChaMtxECI2VVLH[i][j] * (RT[j] - RC[j]);
|
||
}
|
||
}
|
||
|
||
double R_Co1[3], R_Co3[3], R_Co4[3], R_Co6[3], R_Co7[3], R_Co9[3];
|
||
|
||
double a[3] = { 0, -8230e-3, 0 };
|
||
mtxAdd((double*)R_Co1, (double*)R_Co5, (double*)a, 3, 1);
|
||
|
||
double b[3] = { 0, -2230e-3, 0 };
|
||
mtxAdd((double*)R_Co3, (double*)R_Co5, (double*)b, 3, 1);
|
||
|
||
double c[3] = { 0, -860e-3, 0 };
|
||
mtxAdd((double*)R_Co4, (double*)R_Co5, (double*)c, 3, 1);
|
||
|
||
double d[3] = { 0, 860e-3, 0 };
|
||
mtxAdd((double*)R_Co6, (double*)R_Co5, (double*)d, 3, 1);
|
||
|
||
double e[3] = { 0, 2230e-3, 0 };
|
||
mtxAdd((double*)R_Co7, (double*)R_Co5, (double*)e, 3, 1);
|
||
|
||
double f[3] = { 0, 8230e-3, 0 };
|
||
mtxAdd((double*)R_Co9, (double*)R_Co5, (double*)f, 3, 1);
|
||
|
||
//相机坐标系原点在追踪星本体系下的位置矢量，暂无。
|
||
double r_cs[3] = { 0 };
|
||
/*追踪星本体坐标系到相机坐标系的转换矩阵,暂认为相机坐标系与本体坐标系重合，需要和小光电对接。
|
||
写到初始化函数里。 */
|
||
double A_sCb[3][3] = {
|
||
{ 1.0, 0, 0 },
|
||
{ 0, -1.0, 0 },
|
||
{ 0, 0, -1.0 }
|
||
};
|
||
|
||
//误差矩阵（0°）
|
||
/*double A_SetErr[3][3] = {
|
||
{ 1.0, 0, 0 },
|
||
{ 0, 1.0, 0 },
|
||
{ 0, 0, 1.0 }
|
||
};*/
|
||
|
||
//误差矩阵（0.3° 偏航0.1° 俯仰0.2°）
|
||
double A_SetErr[3][3] = {
|
||
{ 1.0000, 0.0017, 0 },
|
||
{ -0.0017, 1.000, 0.0035 },
|
||
{ 0, -0.0035, 1.0000 }
|
||
};
|
||
|
||
//误差矩阵（1° 偏航0.5° 俯仰0.5°）
|
||
/*double A_SetErr[3][3] = {
|
||
{ 1.000, 0.0087, 0 },
|
||
{ -0.0087, 0.9999, 0.0087 },
|
||
{ 0.0001, -0.0087, 1.0000 }
|
||
};*/
|
||
|
||
/*两星相对位置矢量由VVLH转本体，再转相机坐标系，相机安装误差暂没考虑
|
||
视线坐标系位置 = 安装误差*本体转视线（现均为VVLH）*（-安装矢量+追踪星轨道转本体*追踪星轨道系矢量）
|
||
ChaMtxVVLH2Body：追踪星VVLH到本体系转换矩阵
|
||
*/
|
||
double temp1_1[3], temp1_2[3], temp1_3[3], R_LOS1[3];
|
||
mtxMtp((double*)temp1_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co1, 3, 1);
|
||
mtxSub((double*)temp1_2, (double*)temp1_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp1_3, (double*)A_sCb, 3, 3, (double*)temp1_2, 3, 1);
|
||
mtxMtp((double*)R_LOS1, (double*)A_SetErr, 3, 3, (double*)temp1_3, 3, 1);
|
||
|
||
double temp3_1[3], temp3_2[3], temp3_3[3], R_LOS3[3];
|
||
mtxMtp((double*)temp3_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co3, 3, 1);
|
||
mtxSub((double*)temp3_2, (double*)temp3_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp3_3, (double*)A_sCb, 3, 3, (double*)temp3_2, 3, 1);
|
||
mtxMtp((double*)R_LOS3, (double*)A_SetErr, 3, 3, (double*)temp3_3, 3, 1);
|
||
|
||
double temp4_1[3], temp4_2[3], temp4_3[3], R_LOS4[3];
|
||
mtxMtp((double*)temp4_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co4, 3, 1);
|
||
mtxSub((double*)temp4_2, (double*)temp4_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp4_3, (double*)A_sCb, 3, 3, (double*)temp4_2, 3, 1);
|
||
mtxMtp((double*)R_LOS4, (double*)A_SetErr, 3, 3, (double*)temp4_3, 3, 1);
|
||
|
||
double temp6_1[3], temp6_2[3], temp6_3[3], R_LOS6[3];
|
||
mtxMtp((double*)temp6_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co6, 3, 1);
|
||
mtxSub((double*)temp6_2, (double*)temp6_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp6_3, (double*)A_sCb, 3, 3, (double*)temp6_2, 3, 1);
|
||
mtxMtp((double*)R_LOS6, (double*)A_SetErr, 3, 3, (double*)temp6_3, 3, 1);
|
||
|
||
double temp7_1[3], temp7_2[3], temp7_3[3], R_LOS7[3];
|
||
mtxMtp((double*)temp7_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co7, 3, 1);
|
||
mtxSub((double*)temp7_2, (double*)temp7_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp7_3, (double*)A_sCb, 3, 3, (double*)temp7_2, 3, 1);
|
||
mtxMtp((double*)R_LOS7, (double*)A_SetErr, 3, 3, (double*)temp7_3, 3, 1);
|
||
|
||
double temp9_1[3], temp9_2[3], temp9_3[3], R_LOS9[3];
|
||
mtxMtp((double*)temp9_1, (double*)ChaMtxVVLH2Body, 3, 3, (double*)R_Co9, 3, 1);
|
||
mtxSub((double*)temp9_2, (double*)temp9_1, (double*)r_cs, 3, 1);
|
||
mtxMtp((double*)temp9_3, (double*)A_sCb, 3, 3, (double*)temp9_2, 3, 1);
|
||
mtxMtp((double*)R_LOS9, (double*)A_SetErr, 3, 3, (double*)temp9_3, 3, 1);
|
||
|
||
/*
|
||
相机平面位置
|
||
近距离相机投影
|
||
*/
|
||
//将视线系数据转换到相平面，作为判据使用；f_remote：小孔成像焦距
|
||
// 1：左帆板左边缘
|
||
R_Plane1_Close[1] = -(R_LOS1[1] / R_LOS1[0]) * f_Close;
|
||
R_Plane1_Close[2] = -(R_LOS1[2] / R_LOS1[0]) * f_Close;
|
||
|
||
// 3：左帆板右边缘
|
||
R_Plane3_Close[1] = -(R_LOS3[1] / R_LOS3[0]) * f_Close;
|
||
R_Plane3_Close[2] = -(R_LOS3[2] / R_LOS3[0]) * f_Close;
|
||
|
||
// 4：本体左边缘
|
||
R_Plane4_Close[1] = -(R_LOS4[1] / R_LOS4[0]) * f_Close;
|
||
R_Plane4_Close[2] = -(R_LOS4[2] / R_LOS4[0]) * f_Close;
|
||
|
||
// 6：本体右边缘
|
||
R_Plane6_Close[1] = -(R_LOS6[1] / R_LOS6[0]) * f_Close;
|
||
R_Plane6_Close[2] = -(R_LOS6[2] / R_LOS6[0]) * f_Close;
|
||
|
||
// 7：右帆板左边缘
|
||
R_Plane7_Close[1] = -(R_LOS7[1] / R_LOS7[0]) * f_Close;
|
||
R_Plane7_Close[2] = -(R_LOS7[2] / R_LOS7[0]) * f_Close;
|
||
|
||
// 9：右帆板右边缘
|
||
R_Plane9_Close[1] = -(R_LOS9[1] / R_LOS9[0]) * f_Close;
|
||
R_Plane9_Close[2] = -(R_LOS9[2] / R_LOS9[0]) * f_Close;
|
||
|
||
|
||
// 图像宽度
|
||
const double Planelong_Close = 1024 * 5.5e-6;
|
||
// 图像左边缘
|
||
double Plane_left_Close[2] = { -Planelong_Close, 0 };
|
||
// 图像右边缘
|
||
double Plane_right_Close[2] = { Planelong_Close, 0 };
|
||
|
||
// 近距离相机角度模拟标志
|
||
int LeftLOSCloseFlag = 1;
|
||
int BodyLOSCloseFlag = 1;
|
||
int RightLOSCloseFlag = 1;
|
||
|
||
|
||
|
||
if (Plane_left_Close[0] < R_Plane1_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane1_Close[1]) {
|
||
// 且左帆板左边缘超过图像右边缘，则3个标志位置零
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane3_Close[1]) {
|
||
// 若左帆板右边缘超过图像右边缘，
|
||
// 识别左帆板中心距y=（图像右边缘距离+左帆板左边缘距离）/2
|
||
R_Plane2_Close[1] = (Plane_right_Close[0] + R_Plane1_Close[1]) / 2.0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane4_Close[1]) {
|
||
// 本体左边缘超过图像右边缘，
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2
|
||
R_Plane2_Close[1] = (R_Plane3_Close[1] + R_Plane1_Close[1]) / 2.0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane6_Close[1]) {
|
||
// 本体右边缘超过图像右边缘,
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2,
|
||
// 识别本体中心y=（图像右边缘距离+本体左边缘距离）/2
|
||
R_Plane2_Close[1] = (R_Plane3_Close[1] + R_Plane1_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (Plane_right_Close[0] + R_Plane4_Close[1]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane7_Close[1]) {
|
||
// 右帆板左边缘超过图像右边缘,
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2,
|
||
// 识别本体中心y=（图像右边缘距离+本体左边缘距离）/2
|
||
R_Plane2_Close[1] = (R_Plane3_Close[1] + R_Plane1_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane6_Close[1] + R_Plane4_Close[1]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
// 右帆板右边缘超过图像右边缘
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2
|
||
// 识别本体中心y=（本体右边缘距离+本体左边缘距离）/2
|
||
// 识别右帆板中心距y=（图像右边缘距离+右帆板左边缘距离）/2
|
||
R_Plane2_Close[1] = (R_Plane3_Close[1] + R_Plane1_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane6_Close[1] + R_Plane4_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (Plane_right_Close[0] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
else {
|
||
// 识别左帆板中心距y=（左帆板右边缘距离+左帆板左边缘距离）/2
|
||
// 识别本体中心y=（本体右边缘距离+本体左边缘距离）/2
|
||
// 识别右帆板中心距y=（右帆板右边缘距离+右帆板左边缘距离）/2
|
||
R_Plane2_Close[1] = (R_Plane3_Close[1] + R_Plane1_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane6_Close[1] + R_Plane4_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane9_Close[1] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else if (Plane_left_Close[0] < R_Plane3_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane3_Close[1]) {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + Plane_right_Close[0]) / 2.0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane4_Close[1]) {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + R_Plane3_Close[1]) / 2.0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane6_Close[1]) {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + R_Plane3_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane7_Close[1]) {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + R_Plane3_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + R_Plane3_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane7_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
}
|
||
else {
|
||
R_Plane2_Close[1] = (Plane_left_Close[0] + R_Plane3_Close[1]) / 2.0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane9_Close[1] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else if (Plane_left_Close[0] < R_Plane4_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane4_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane6_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane7_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane7_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (R_Plane4_Close[1] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane9_Close[1] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else if (Plane_left_Close[0] < R_Plane6_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane6_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (Plane_left_Close[0] + Plane_right_Close[0]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane7_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (Plane_left_Close[0] + R_Plane6_Close[1]) / 2.0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (Plane_left_Close[0] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane7_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSCloseFlag = 0;
|
||
R_Plane5_Close[1] = (Plane_left_Close[0] + R_Plane6_Close[1]) / 2.0;
|
||
R_Plane8_Close[1] = (R_Plane9_Close[1] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else if (Plane_left_Close[0] < R_Plane7_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane7_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
R_Plane8_Close[1] = (R_Plane7_Close[1] + Plane_right_Close[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
R_Plane8_Close[1] = (R_Plane9_Close[1] + R_Plane7_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else if (Plane_left_Close[0] < R_Plane9_Close[1]) {
|
||
if (Plane_right_Close[0] < R_Plane9_Close[1]) {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
R_Plane8_Close[1] = (Plane_left_Close[0] + Plane_right_Close[0]) / 2.0;
|
||
}
|
||
else {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
R_Plane8_Close[1] = (Plane_left_Close[0] + R_Plane9_Close[1]) / 2.0;
|
||
}
|
||
}
|
||
else {
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
|
||
//计算相平面坐标系下目标点的位置
|
||
if (LeftLOSCloseFlag == 1) {
|
||
R_Plane2_Close[2] = (R_Plane1_Close[2] + R_Plane3_Close[2]) / 2.0;
|
||
R_Plane2_Close[0] = f_Close;
|
||
}
|
||
|
||
if (BodyLOSCloseFlag == 1) {
|
||
R_Plane5_Close[2] = (R_Plane4_Close[2] + R_Plane6_Close[2]) / 2.0;
|
||
R_Plane5_Close[0] = f_Close;
|
||
}
|
||
|
||
if (RightLOSCloseFlag == 1) {
|
||
R_Plane8_Close[2] = (R_Plane7_Close[2] + R_Plane9_Close[2]) / 2.0;
|
||
R_Plane8_Close[0] = f_Close;
|
||
}
|
||
|
||
// 归一化
|
||
double norm_R_Plane2 = norm(R_Plane2_Close, 3);
|
||
R_Plane2_Close[0] /= norm_R_Plane2;
|
||
R_Plane2_Close[1] /= norm_R_Plane2;
|
||
R_Plane2_Close[2] /= norm_R_Plane2;
|
||
|
||
double norm_R_Plane5 = norm(R_Plane5_Close, 3);
|
||
R_Plane5_Close[0] /= norm_R_Plane5;
|
||
R_Plane5_Close[1] /= norm_R_Plane5;
|
||
R_Plane5_Close[2] /= norm_R_Plane5;
|
||
|
||
double norm_R_Plane8 = norm(R_Plane8_Close, 3);
|
||
R_Plane8_Close[0] /= norm_R_Plane8;
|
||
R_Plane8_Close[1] /= norm_R_Plane8;
|
||
R_Plane8_Close[2] /= norm_R_Plane8;
|
||
|
||
// 计算相平面坐标系下目标点的方位角和俯仰角
|
||
if (LeftLOSCloseFlag == 1) {
|
||
Beta_Left_Close = asin(R_Plane2_Close[0]);
|
||
Alpha_Left_Close = atan2(R_Plane2_Close[2], R_Plane2_Close[1]);
|
||
}
|
||
|
||
if (BodyLOSCloseFlag == 1) {
|
||
Beta_Body_Close = asin(R_Plane5_Close[0]);
|
||
Alpha_Body_Close = atan2(R_Plane5_Close[2], R_Plane5_Close[1]);
|
||
}
|
||
|
||
if (RightLOSCloseFlag == 1) {
|
||
Beta_Right_Close = asin(R_Plane8_Close[0]);
|
||
Alpha_Right_Close = atan2(R_Plane8_Close[2], R_Plane8_Close[1]);
|
||
}
|
||
|
||
//相机判断
|
||
double PI = 3.1415926535;
|
||
double Dis_Body = norm(R_Co5, 3);
|
||
double Dis_Fan_Left = sqrt(R_Co5[0] * R_Co5[0] + pow((R_Co5[1] - 10.682), 2) + R_Co5[2] * R_Co5[2]);
|
||
double Dis_Fan_Right = sqrt(R_Co5[0] * R_Co5[0] + pow((R_Co5[1] + 10.682), 2) + R_Co5[2] * R_Co5[2]);
|
||
|
||
// 目标选择，0~本体，1~左帆板，2~右帆板,在OBC里做切换
|
||
double Dis_Measure = 0.0;
|
||
/*if (TargetFLAG == 0) {
|
||
Dis_Measure = Dis_Body;
|
||
}
|
||
else if (TargetFLAG == 1) {
|
||
Dis_Measure = Dis_Fan_Left;
|
||
}
|
||
else if (TargetFLAG == 2) {
|
||
Dis_Measure = Dis_Fan_Right;
|
||
}*/
|
||
Dis_Measure = Dis_Body;
|
||
|
||
|
||
if (fabs(Dis_Measure) >= 2000) {
|
||
// 小光电测角最大距离2km
|
||
LeftLOSCloseFlag = 0;
|
||
BodyLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Dis_Measure < 2000 && Dis_Measure >= 500) {
|
||
// 大于500m开始面目标测量
|
||
Alpha_Body_Close_Mea = Alpha_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Body_Close_Mea = Beta_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
LeftLOSCloseFlag = 0;
|
||
RightLOSCloseFlag = 0;
|
||
}
|
||
else if (Dis_Measure < 500 && Dis_Measure >= 50) {
|
||
Alpha_Left_Close_Mea = Alpha_Left_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Left_Close_Mea = Beta_Left_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Alpha_Body_Close_Mea = Alpha_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Body_Close_Mea = Beta_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Alpha_Right_Close_Mea = Alpha_Right_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
Beta_Right_Close_Mea = Beta_Right_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3) * PI / 180.0;
|
||
}
|
||
else if (Dis_Measure < 50 && Dis_Measure >= 0) {
|
||
Alpha_Left_Close_Mea = Alpha_Left_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Left_Close_Mea = Beta_Left_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Alpha_Body_Close_Mea = Alpha_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Body_Close_Mea = Beta_Body_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Alpha_Right_Close_Mea = Alpha_Right_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
Beta_Right_Close_Mea = Alpha_Right_Close + (((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.2 / 3) * PI / 180.0;
|
||
}
|
||
|
||
|
||
if (LeftLOSCloseFlag == 0) {
|
||
Alpha_Left_Close_Mea = 0;
|
||
Beta_Left_Close_Mea = 0;
|
||
}
|
||
if (BodyLOSCloseFlag == 0) {
|
||
Alpha_Body_Close_Mea = 0;
|
||
Beta_Body_Close_Mea = 0;
|
||
}
|
||
if (RightLOSCloseFlag == 0) {
|
||
Alpha_Right_Close_Mea = 0;
|
||
Beta_Right_Close_Mea = 0;
|
||
}
|
||
|
||
|
||
//返回结果数组
|
||
/*LOS_Close[0] = LeftLOSCloseFlag;
|
||
LOS_Close[1] = Alpha_Left_Close_Mea;
|
||
LOS_Close[2] = Beta_Left_Close_Mea;
|
||
LOS_Close[3] = BodyLOSCloseFlag;
|
||
LOS_Close[4] = Alpha_Body_Close_Mea;
|
||
LOS_Close[5] = Beta_Body_Close_Mea;
|
||
LOS_Close[6] = RightLOSCloseFlag;
|
||
LOS_Close[7] = Alpha_Right_Close_Mea;
|
||
LOS_Close[8] = Beta_Right_Close_Mea;*/
|
||
LOS_Close[0] = LeftLOSCloseFlag;
|
||
LOS_Close[1] = Beta_Left_Close_Mea * 180.0 / PI;
|
||
LOS_Close[2] = Alpha_Left_Close_Mea * 180.0 / PI;
|
||
LOS_Close[3] = BodyLOSCloseFlag;
|
||
LOS_Close[4] = Beta_Body_Close_Mea * 180.0 / PI;
|
||
LOS_Close[5] = Alpha_Body_Close_Mea * 180.0 / PI;
|
||
LOS_Close[6] = RightLOSCloseFlag;
|
||
LOS_Close[7] = Beta_Right_Close_Mea * 180.0 / PI;
|
||
LOS_Close[8] = Alpha_Right_Close_Mea * 180.0 / PI;
|
||
|
||
R_Plane[0] = R_Plane1_Close[1];
|
||
R_Plane[1] = R_Plane9_Close[1];
|
||
|
||
}
|
||
|