#include "sensor_tx2.h" #include "orbit_info.h" #include "libconvert.h" #include "liblog.h" #include #include #include #include #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= 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]; }