XdCxRhDW54/XdCxRhDW.Api/AddIns/低轨/LeoPosApi.cs

using System;
using System.Collections.Generic;
using System.Configuration;
using System.Linq;
using System.Runtime.InteropServices;
using System.Security.Policy;
using System.Text;
using System.Threading.Tasks;
using XdCxRhDW.Entity;

namespace XdCxRhDW.Api
{
    public static class LeoPosApi
    {
        //低轨道单星和双星定位
        private const string leoPos = @"AddIns\低轨\DLL_XDXZ.dll";


        private static double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域


        /// <summary>
        /// 地轨双星dw
        /// </summary>
        /// <param name="main_sat"></param>
        /// <param name="neigh_sat"></param>
        /// <param name="Ref_Station_LLH"></param>
        /// <param name="Zone"></param>
        /// <param name="target_dto"></param>
        /// <param name="target_dfo"></param>
        /// <param name="ref_dto"></param>
        /// <param name="ref_dfo"></param>
        /// <param name="fu1"></param>
        /// <param name="fu2"></param>
        /// <param name="target_llh"></param>
        [DllImport(leoPos, EntryPoint = "TwoStar_DTFO_DW", CallingConvention = CallingConvention.Cdecl)]
        public extern static void TwoStar_DTFO_DW(double[] main_sat, double[] neigh_sat, double[] Ref_Station_LLH
            , double[] Zone, double target_dto, double target_dfo,
        double ref_dto, double ref_dfo, double fu1, double fu2, double[] target_llh);

        /// <summary>
        /// 单星dw
        /// </summary>
        /// <param name="main_sat">第一时刻星历 长度6</param>
        /// <param name="neigh_sat1">第二时刻星历 长度6</param>
        /// <param name="neigh_sat2">第三时刻星历 长度6</param>
        /// <param name="Zone"></param>
        /// <param name="target_dfo1">频差1(Hz)</param>
        /// <param name="target_dfo2">频差2(Hz)</param>
        /// <param name="fu">上行频点(Hz)</param>
        /// <param name="target_llh"></param>
        [DllImport(leoPos, EntryPoint = "SingleStar_DFO_DW", CallingConvention = CallingConvention.Cdecl)]
        public extern static void SingleStar_DFO_DW(double[] main_sat, double[] neigh_sat1, double[] neigh_sat2
            , double[] Zone, double target_dfo1, double target_dfo2, double fu, double[] target_llh);

        /// <summary>
        /// 低轨双星时差线
        /// </summary>
        /// <param name="main_sat_pos">主星星历 长度6</param>
        /// <param name="neigh_sat_pos">邻星星历 长度6</param>
        /// <param name="ref_pos">参考站位置</param>
        /// <param name="Zone">-85, 85, -180, 180</param>
        /// <param name="target_dto">目标时差(s)</param>
        /// <param name="ref_dto">参考时差(s)</param>
        /// <param name="LOP_Value"></param>
        /// <param name="LOP_Len"></param>
        [DllImport(leoPos, EntryPoint = "TwoStar_SCX", CallingConvention = CallingConvention.Cdecl)]
        public extern static void TwoStar_SCX(double[] main_sat_pos, double[] neigh_sat_pos, double[] ref_pos, double[] Zone,
        double target_dto, double ref_dto, out IntPtr LOP_Value, ref int LOP_Len);

        /// <summary>
        /// 低轨双星频差线
        /// </summary>
        /// <param name="main_sat"></param>
        /// <param name="neigh_sat"></param>
        /// <param name="ref_pos"></param>
        /// <param name="Zone"></param>
        /// <param name="target_dfo">目标频差(Hz)</param>
        /// <param name="ref_dfo">参考频差(Hz)</param>
        /// <param name="fu1">上行频点1(Hz)</param>
        /// <param name="fu2">上行频点2(Hz)</param>
        /// <param name="LOP_Value"></param>
        /// <param name="LOP_Len"></param>
        [DllImport(leoPos, EntryPoint = "TwoStar_PCX", CallingConvention = CallingConvention.Cdecl)]
        public extern static void TwoStar_PCX(double[] main_sat, double[] neigh_sat, double[] ref_pos,
            double[] Zone, double target_dfo, double ref_dfo, double fu1, double fu2, out IntPtr LOP_Value, ref int LOP_Len);

        /// <summary>
        /// 单星频差线
        /// </summary>
        /// <param name="main_sat"></param>
        /// <param name="neigh_sat"></param>
        /// <param name="Zone"></param>
        /// <param name="target_dfo"></param>
        /// <param name="fu"></param>
        /// <param name="LOP_Value"></param>
        /// <param name="LOP_Len"></param>
        [DllImport(leoPos, EntryPoint = "SingleStar_PCX", CallingConvention = CallingConvention.Cdecl)]
        public extern static void SingleStar_PCX(double[] main_sat, double[] neigh_sat
            , double[] Zone, double target_dfo, double fu, out IntPtr LOP_Value, ref int LOP_Len);

        public static double[] X1_POS(CgRes cg, double upfreqHz)
        {

            double[] mainSat = new double[6] { cg.MainX.Value, cg.MainY.Value, cg.MainZ.Value, cg.MainVx.Value, cg.MainVy.Value, cg.MainVz.Value };
            double[] adjaSat1 = new double[6] { cg.Adja1X.Value, cg.Adja1Y.Value, cg.Adja1Z.Value, cg.Adja1Vx.Value, cg.Adja1Vy.Value, cg.Adja1Vz.Value };
            double[] adjaSat2 = new double[6] { cg.Adja2X.Value, cg.Adja2Y.Value, cg.Adja2Z.Value, cg.Adja2Vx.Value, cg.Adja2Vy.Value, cg.Adja2Vz.Value };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            SingleStar_DFO_DW(mainSat, adjaSat1, adjaSat2, zone, cg.Dfo1.Value, cg.Dfo2.Value, upfreqHz, res);

            var posRes = ConvertToGeoPoint(res);
            if (posRes[1] < 5 && posRes[4] > 10 && posRes[4] < 30)
            {

                var p0 = posRes[0];
                var p1 = posRes[1];
                var p2 = posRes[2];
                posRes[0] = posRes[3];
                posRes[1] = posRes[4];
                posRes[2] = posRes[5];
                posRes[3] = p0;
                posRes[4] = p1;
                posRes[5] = p2;
            }
            return posRes;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="cg"></param>
        /// <param name="refTx"></param>
        /// <param name="upfreqHz1">目标上行频点</param>
        /// <param name="upfreqHz2">参考上行频点</param>
        /// <returns></returns>
        public static double[] X2_POS(CgRes cg, StationRes sRes, double upfreqHz1, double upfreqHz2)
        {
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };

            double[] mainSat = new double[6] { cg.MainX.Value, cg.MainY.Value, cg.MainZ.Value, cg.MainVx.Value, cg.MainVy.Value, cg.MainVz.Value };
            double[] adjaSat1 = new double[6] { cg.Adja1X.Value, cg.Adja1Y.Value, cg.Adja1Z.Value, cg.Adja1Vx.Value, cg.Adja1Vy.Value, cg.Adja1Vz.Value };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];

            var refDto = (cg.YbMainDto.Value - cg.YbAdja1Dto.Value) / 1e6;
            var refDfo = cg.YbMainDfo.Value - cg.YbAdja1Dfo.Value;
            TwoStar_DTFO_DW(mainSat, adjaSat1, refStation, zone, cg.Dto1.Value / 1e6, cg.Dfo1.Value, refDto, refDfo, upfreqHz1, upfreqHz2, res);
            var posRes = ConvertToGeoPoint(res);
            if (posRes[1] < 5 && posRes[4] > 10 && posRes[4] < 30)
            {

                var p0 = posRes[0];
                var p1 = posRes[1];
                var p2 = posRes[2];
                posRes[0] = posRes[3];
                posRes[1] = posRes[4];
                posRes[2] = posRes[5];
                posRes[3] = p0;
                posRes[4] = p1;
                posRes[5] = p2;
            }
            return posRes;
        }
        private static double[] ConvertToGeoPoint(double[] res)
        {
            if (res == null || res.Length == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            else if (res.Length != 3 && res.Length != 6)
            {
                throw new Exception("定位结果解析异常,长度不是3或6");
            }

            var list = res.Select(p => Math.Round(p, 4)).ToArray();
            if (list.Length == 3)
            {
                list = list.Concat(new double[4] { 999, 999, 0, -1 }).ToArray();
            }
            list[2] = list[5] = 0;//高度

            if (double.IsNaN(list[0]) || double.IsNaN(list[1]))
            {
                list[0] = list[1] = 999;
            }
            if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
            {
                list[3] = list[4] = 999;
            }
            if (list[0] < -180 || list[0] > 180)
            {
                list[0] = list[1] = 999;
            }
            else if (list[1] < -90 || list[1] > 90)
            {
                list[0] = list[1] = 999;
            }
            if (list[3] < -180 || list[3] > 180)
            {
                list[3] = list[4] = 999;
            }
            else if (list[4] < -90 || list[4] > 90)
            {
                list[3] = list[4] = 999;
            }
            if (list.Length == 6)
            {
                var listNew = list.ToList();
                listNew.Add(-1);
                list = listNew.ToArray();
            }
            return list;
        }

        public static IEnumerable<(double lon, double lat)> DtoLineLeoX2(DtoLineTwoStartOption opt)
        {

            List<DtoLinePoint> list = new List<DtoLinePoint>();
            IntPtr LOP_ValuePtr;
            int LOP_Len = 0;

            TwoStar_SCX(
                opt.MsEph,
                 opt.NsEph,
                opt.RefGeod,
                 zone,
                opt.TargetDto * 1e-6,
                opt.RefDto * 1e-6, out LOP_ValuePtr, ref LOP_Len);
            double[] LOP_Value = new double[LOP_Len * 3];
            if (LOP_Len > 0)
            {
                Marshal.Copy(LOP_ValuePtr, LOP_Value, 0, LOP_Value.Length);

                list = OutputHelper.WriteDtoLine(LOP_Value, LOP_Len);
            }
            var Lines = list.Select(p => (p.Lon, p.Lat));
            return Lines;
        }

        public static IEnumerable<(double lon, double lat)> DfoLineLeo2(LeoDfoLineTwo opt)
        {

            IntPtr LOP_ValuePtr;
            int LOP_Len = 0;
            TwoStar_PCX(
                opt.MsEph,
                 opt.NsEph,
                opt.RefGeod,
                 zone,
                opt.TargetDfo,
                opt.RefDfo,
               opt.fu1,
               opt.fu2, out LOP_ValuePtr, ref LOP_Len);
            return ParseResult(LOP_ValuePtr, LOP_Len);
        }

        public static IEnumerable<(double lon, double lat)> DfoLineLeoX1(double[] main_sat, double[] neigh_sat, double target_dfo, double fu)
        {
            IntPtr LOP_ValuePtr;
            int LOP_Len = 0;
            SingleStar_PCX(main_sat, neigh_sat
            , zone, target_dfo, fu, out LOP_ValuePtr, ref LOP_Len);
            return ParseResult(LOP_ValuePtr, LOP_Len);
        }

        private static IEnumerable<(double lon, double lat)> ParseResult(IntPtr LOP_ValuePtr, int LOP_Len)
        {
            List<DtoLinePoint> list = new List<DtoLinePoint>();
            double[] LOP_Value = new double[LOP_Len * 3];
            if (LOP_Len > 0)
            {
                Marshal.Copy(LOP_ValuePtr, LOP_Value, 0, LOP_Value.Length);
                for (int idx = 0; idx < LOP_Len; ++idx)
                {
                    if (LOP_Value[3 * idx + 1] != -1)
                    {
                        list.Add(new DtoLinePoint()
                        {
                            Lon = LOP_Value[3 * idx + 1],
                            Lat = LOP_Value[3 * idx]
                        });
                    }
                }
                for (int idx = 0; idx < LOP_Len; ++idx)
                {
                    if (LOP_Value[3 * idx + 2] != -1)
                    {
                        list.Add(new DtoLinePoint()
                        {
                            Lon = LOP_Value[3 * idx + 2],
                            Lat = LOP_Value[3 * idx]
                        });
                    }
                }
            }
            var Lines = list.Select(p => (p.Lon, p.Lat));
            return Lines;
        }

    }
}