DW5S/DW5S.KxcApi/AddIns/GDOP误差椭圆/TheoryDtoDfoApi.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using DW5S.Entity;

namespace DW5S.KxcApi
{
    /// <summary>
    /// 理论时频差API
    /// </summary>
    public static class TheoryDtoDfoApi
    {
        #region cpp dll Interop 
        //一星一地测向带参定位
        private const string dll = @"AddIns\GDOP误差椭圆\GDOP_Draw_11.dll";

        //主接收站位置[3]，
        //邻接收站位置[3]
        //主星历[6]（x,y,z,Vx,Vy,Vz）
        //邻星历[6]（x,y,z,Vx,Vy,Vz）
        //上行频（单位，Hz）
        //下行频（单位，Hz）
        [DllImport(dll, EntryPoint = "theryDfo", CallingConvention = CallingConvention.Cdecl)]//双星理论频差
        private extern static double TheoryDfo(double[] target_llh, double[] rec_llh1, double[] rec_llh2, double[] main_sat, double[] neigh_sat, double fu, double fd);
        #endregion


        public static EnumTargetState TheoryDfo(CgRes cRes, StationRes sRes, PosRes posRes, int YDPZThreshold)
        {
            //张老板说理论频差如果和参估计算出来的频差差值比较大,就说明目标在运动(多路参估每一路都要比较,有一路判定运动则为运动)
            //?具体多少差异需要确定一下
            //主邻星本振不一样时该怎么计算
            if (cRes.TarFreqUp == null || cRes.TarFreqDown == null || YDPZThreshold <= 0)
                return EnumTargetState.Unknown;
            double[] target = new double[3] { posRes.PosLon, posRes.PosLat, 0 };
            double[] rec = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            if (cRes.MainVx == null || cRes.MainVy == null || cRes.MainVz == null) return EnumTargetState.Unknown;
            double[] xlMain = new double[6] { cRes.MainX.Value, cRes.MainY.Value, cRes.MainZ.Value, cRes.MainVx.Value, cRes.MainVy.Value, cRes.MainVz.Value };

            if (posRes.PosResType == EnumPosResType.X2D1 || posRes.PosResType == EnumPosResType.X2D1NoRef || posRes.PosResType == EnumPosResType.RH)
            {
                if (cRes.Dfo1 == null || cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
                var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
                double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
                var val = Math.Abs(res - cRes.Dfo1.Value);
                if (val < YDPZThreshold)
                    return EnumTargetState.Stationary;
                else
                    return EnumTargetState.Movement;
            }
            else if (posRes.PosResType == EnumPosResType.X2Dfo)
            {
                if (cRes.Dfo1 == null) return EnumTargetState.Unknown;
                var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
                if (cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
                double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
                var val = Math.Abs(res - cRes.Dfo1.Value);
                if (val < YDPZThreshold)
                    return EnumTargetState.Stationary;
                else
                    return EnumTargetState.Movement;
            }
            else if (posRes.PosResType == EnumPosResType.X1D1CX)
            {
                //星地不知道接口是否支持
                return EnumTargetState.Unknown;
            }
            else if (posRes.PosResType == EnumPosResType.X3 || posRes.PosResType == EnumPosResType.X3NoRef || posRes.PosResType == EnumPosResType.X3TwoDfo)
            {
                if (cRes.Dfo1 == null && cRes.Dfo2 == null) return EnumTargetState.Unknown;
                if (cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
                if (cRes.Dfo1 != null)
                {
                    var xlAdja1 = new double[6] { cRes.Adja1X.Value, cRes.Adja1Y.Value, cRes.Adja1Z.Value, cRes.Adja1Vx.Value, cRes.Adja1Vy.Value, cRes.Adja1Vz.Value };
                    if (cRes.Adja1Vx == null || cRes.Adja1Vy == null || cRes.Adja1Vz == null) return EnumTargetState.Unknown;
                    double res = TheoryDfo(target, rec, rec, xlMain, xlAdja1, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
                    var val = Math.Abs(res - cRes.Dfo1.Value);
                    if (val >= YDPZThreshold)
                        return EnumTargetState.Movement;
                }
                if (cRes.Dfo2 != null)
                {
                    var xlAdja2 = new double[6] { cRes.Adja2X.Value, cRes.Adja2Y.Value, cRes.Adja2Z.Value, cRes.Adja2Vx.Value, cRes.Adja2Vy.Value, cRes.Adja2Vz.Value };
                    if (cRes.Adja2Vx == null || cRes.Adja2Vy == null || cRes.Adja2Vz == null) return EnumTargetState.Unknown;
                    double res = TheoryDfo(target, rec, rec, xlMain, xlAdja2, cRes.TarFreqUp.Value, cRes.TarFreqDown.Value);
                    var val = Math.Abs(res - cRes.Dfo2.Value);
                    if (val >= YDPZThreshold)
                        return EnumTargetState.Movement;
                }
                return EnumTargetState.Stationary;
            }
            else
            {
                return EnumTargetState.Unknown;
            }
        }
    }
}