XdCxRhDW/XdCxRhDW.App/Api/PosApi.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using DevExpress.Drawing.Internal.Fonts.Interop;
using DevExpress.XtraBars.Docking2010.Views.Widget;
using XdCxRhDW.App.Model;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TextBox;

namespace XdCxRhDW.App.Api
{
    /// <summary>
    ///TODO 定位算法接口.在这里调用罗博士的算法库
    /// </summary>
    public static class PosApi
    {
        #region cpp dll Interop 

        //一星一地测向定位
        private const string XDCX = @"AddIns\DLL_DTO_DOA_DW.dll";

        //两星一地定位
        private const string X2D1 = @"AddIns\DLL_SC_2X1D_DW.dll";

        [DllImport(XDCX, EntryPoint = "XD_CX_DW", CallingConvention = CallingConvention.Cdecl)]//一星一地
        private extern static void X1D1_POS2023_Core(double[] mainSat, double[] satStation, double[] cdbStation, double[] cxStation, double[] refStation, double[] zone, double theta, double tarDto, double refDto, double[] res);


        [DllImport(X2D1, EntryPoint = "SC_2X1D_DW", CallingConvention = CallingConvention.Cdecl)]//两星一地
        public extern static void X2D1_POS_Core(double[] mainSat, double[] adjaSat, double[] cdbStation, double[] satStation1, double[] satStation2, double[] satStation3, double[] satStation4,
             double[] satStation5, double[] refStation, double[] zone, double tarSxDto, double tarXdDto, double samp_main_dto, double samp_neigh_dto, double[] res);

        #endregion


        /// <summary>
        /// 一星一地,返返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes"></param>
        /// <param name="cxRes"></param>
        /// <returns></returns>
        public static double[] X1D1_POS(CgRes cgRes, List<TxInfo> listTx, CxRes cxRes)
        {
            var satTx = listTx.Find(p => p.TxType == EnumTxType.MainSat);
            var cdbTx = listTx.Find(p => p.TxType == EnumTxType.Cdb);
            var cxTx = listTx.Find(p => p.TxType == EnumTxType.Cx);
            var refTx = listTx.Find(p => p.TxType == EnumTxType.Ref);
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] satStation = new double[3] { satTx.Lon, satTx.Lat, 0 };
            double[] cdbStation = new double[3] { cdbTx.Lon, cdbTx.Lat, 0 };
            double[] cxStation = new double[3] { cxTx.Lon, cxTx.Lat, 0 };
            double[] refStation = new double[3] { refTx.Lon, refTx.Lat, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double theta = cxRes.Fx;//单位°
            double[] res = new double[6];
            X1D1_POS2023_Core(mainSat, satStation, cdbStation, cxStation, refStation, zone, theta, cgRes.DtoCdb / 1e6, cgRes.YbMain / 1e6, res);
            return res.Select(p => Math.Round(p, 4)).ToArray();
        }

        /// <summary>
        /// 两星一地，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes"></param>
        /// <returns></returns>
        public static double[] X2D1_POS(CgRes cgRes, List<TxInfo> listTx)
        {
            var satTx = listTx.Find(p => p.TxType == EnumTxType.MainSat);
            var cdbTx = listTx.Find(p => p.TxType == EnumTxType.Cdb);
            var cxTx = listTx.Find(p => p.TxType == EnumTxType.Cx);
            var refTx = listTx.Find(p => p.TxType == EnumTxType.Ref);
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] adjaSat = new double[3] { cgRes.AdjaX, cgRes.AdjaY, cgRes.AdjaZ };
            double[] satStation = new double[3] { satTx.Lon, satTx.Lat, 0 };
            double[] cdbStation = new double[3] { cdbTx.Lon, cdbTx.Lat, 0 };
            double[] cxStation = new double[3] { cxTx.Lon, cxTx.Lat, 0 };
            double[] refStation = new double[3] { refTx.Lon, refTx.Lat, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            X2D1_POS_Core(mainSat, adjaSat, cdbStation, satStation, satStation, satStation, satStation, satStation, refStation, zone, cgRes.DtoSx / 1e6, cgRes.DtoCdb / 1e6, cgRes.YbMain / 1e6, cgRes.YbAdja / 1e6, res);
            return res;
        }

        /// <summary>
        /// 融合定位，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <returns></returns>
        public static double[] RH_POS(CgRes cgRes, List<TxInfo> listTx, CxRes cxRes)
        {
            var res1 = X1D1_POS(cgRes, listTx, cxRes);
            var res2 = X2D1_POS(cgRes, listTx);
            double[] res = new double[] { 999, 999, 0, 999, 999, 0 };
            if (IsGeoPoint(res1) && IsGeoPoint(res2))
            {
                res = new double[6] {
                    (res1[0] + res2[0]) / 2 + 0.003,
                    (res1[1] + res2[1]) / 2 - 0.002,
                    (res1[2] + res2[2]) / 2,
                    (res1[3] + res2[3]) / 2 + 0.003,
                    (res1[4] + res2[4]) / 2 - 0.002,
                    (res1[5] + res2[5]) / 2,
                };
            }
            else if (IsGeoPoint(res1))
            {
                res = new double[6]
                {
                    res1[0] + 0.003,
                    res1[1] - 0.002,
                    res1[2],
                    res1[3] + 0.003,
                    res1[4] - 0.002,
                    res1[5],

                };
            }
            else if (IsGeoPoint(res2))
            {
                res = new double[6]
               {
                    res2[0] + 0.003,
                    res2[1] - 0.002,
                    res2[2],
                    res2[3] + 0.003,
                    res2[4] - 0.002,
                    res2[5],

               };
            }
            return res;
        }

        public static bool IsGeoPoint(double[] res)
        {
            return res[0] >= -180 && res[0] <= 180 && res[1] >= -90 && res[1] <= 90;
        }
    }
}