Esempio n. 1
0
        public static void upfg(Vehicle vehicle = null)
        {
            if (vehicle is null)
            {
                vehicle = Globals.VehicleData;
            }

            var gamma = Globals.TargetData.Angle;
            var iy    = Globals.TargetData.Normal;
            var rdval = Globals.TargetData.Radius;
            var vdval = Globals.TargetData.Velocity;
            var t     = Globals.VehicleSate.Time;
            var m     = Globals.VehicleSate.Mass;
            var r     = Globals.VehicleSate.Radius;
            var v     = Globals.VehicleSate.Velocity;
            var cser  = Globals.PreviousUpfgState.Cser;
            var rbias = Globals.PreviousUpfgState.Rbias;
            var rd    = Globals.PreviousUpfgState.Rd;
            var rgrav = Globals.PreviousUpfgState.Rgrav;
            var tp    = Globals.PreviousUpfgState.Time;
            var vprev = Globals.PreviousUpfgState.Velocity;
            var vgo   = Globals.PreviousUpfgState.Vgo;

            var SM = new List <int>();
            var aL = new List <double>();
            var md = new List <double>();
            var ve = new List <double>();
            var fT = new List <double>();
            var aT = new List <double>();
            var tu = new List <double>();
            var tb = new List <double>();

            foreach (var stage in vehicle.Stages)
            {
                SM.Add(stage.UpfgMode);
                aL.Add(stage.GLim * Globals.G0);

                var pack = stage.getThrust(Globals.G0);
                fT.Add(pack[0]);
                md.Add(pack[1]);
                ve.Add(pack[2] * Globals.G0);
                aT.Add(fT[fT.Count - 1] / stage.MassTotal);
                tu.Add(ve[ve.Count - 1] / aT[aT.Count - 1]);
            }

            var dt       = t - tp;
            var dvsensed = v - vprev;

            vgo -= dvsensed;

            tb[0] -= Globals.PreviousUpfgState.Tb;

            switch (SM[0])
            {
            case 1:
                aT[0] = fT[0] / m;
                break;

            case 2:
                aT[0] = aL[0];
                break;
            }

            tu[0] = ve[0] / aT[0];

            double L  = 0;
            var    Li = new List <double>();

            var newVehicle = new Vehicle();

            for (var i = 0; i < vehicle.Stages.Count; i++)
            {
                switch (SM[i])
                {
                case 1:
                    Li.Add(ve[i] * Math.Log(tu[i] / (tu[i] - tb[i])));
                    break;

                case 2:
                    Li.Add(aL[i] * tb[i]);
                    break;

                default:
                    Li.Add(0);
                    break;
                }

                L += Li[i];

                if (!(L < AgcMath.magnitude(vgo)))
                {
                    continue;
                }

                var stages = vehicle.Stages.GetRange(0, vehicle.Stages.Count - 1);
                newVehicle.Stages = stages;
                upfg(newVehicle);
            }

            Li.Add(AgcMath.magnitude(vgo) - L);

            var tgoi = new List <double>();

            for (var i = 0; i < vehicle.Stages.Count; i++)
            {
                switch (SM[i])
                {
                case 1:
                    tb[i] = tu[i] * (1 - Math.Pow(Math.E, (-Li[i] / ve[i])));
                    break;

                case 2:
                    tb[i] = Li[i] / aL[i];
                    break;
                }

                if (i == 0)
                {
                    tgoi.Add(tb[i]);
                }
                else
                {
                    tgoi.Add(tgoi[i - 1] + tb[i]);
                }
            }

            var L1  = Li[0];
            var tgo = tgoi[vehicle.Stages.Count - 1];

            L = 0;
            double J     = 0;
            double S     = 0;
            double Q     = 0;
            double H     = 0;
            double P     = 0;
            var    Ji    = new List <double>();
            var    Si    = new List <double>();
            var    Qi    = new List <double>();
            var    Pi    = new List <double>();
            double tgoi1 = 0;

            for (var i = 0; i < vehicle.Stages.Count; i++)
            {
                if (i > 0)
                {
                    tgoi1 = tgoi[i - 1];
                }

                switch (SM[i])
                {
                case 1:
                    Ji.Add(tu[i] * Li[i] - ve[i] * tb[i]);
                    Si.Add(-Ji[i] + tb[i] * Li[i]);
                    Qi.Add(Si[i] * (tu[i] + tgoi1) - 0.5 * ve[i] * Math.Pow(tb[i], 2));
                    Pi.Add(Qi[i] * (tu[i] + tgoi1) - 0.5 * ve[i] * Math.Pow(tb[i], 2) * (tb[i] / 3 + tgoi1));
                    break;

                case 2:
                    Ji.Add(0.5 * Li[i] * tb[i]);
                    Si.Add(Ji[i]);
                    Qi.Add(Si[i] * (tb[i] / 3 + tgoi1));
                    Pi.Add((1 / 6) * Si[i] * (Math.Pow(tgoi[i], 2) + 2 * tgoi[i] * tgoi1 + 3 * Math.Pow(tgoi1, 2)));
                    break;
                }

                Ji[i] += Li[i] * tgoi1;
                Si[i] += L * tb[i];
                Qi[i] += J * tb[i];
                Pi[i] += H * tb[i];

                L += Li[i];
                J += Ji[i];
                S += Si[i];
                Q += Qi[i];
                P += Pi[i];
                H  = J * tgoi[i] - Q;
            }

            var lambda = AgcMath.normalize(vgo);

            if (Globals.PreviousUpfgState.Tgo > 0)
            {
                rgrav *= Math.Pow(tgo / Globals.PreviousUpfgState.Tgo, 2);
            }

            var rgo   = rd - (r + v * tgo - rgrav);
            var iz    = AgcMath.normalize(AgcMath.cross(rd, iy));
            var rgoxy = rgo - AgcMath.dot(iz, rgo) * iz;
            var rgoz  = S - AgcMath.dot(lambda, rgoxy) / AgcMath.dot(lambda, iz);

            rgo = rgoxy + rgoz * iz + rbias;

            var lambdade  = Q - S * J / L;
            var lambdadot = (rgo - S * lambda) / lambdade;
            var iF_       = AgcMath.normalize(lambda - lambdadot * J / L);
            var phi       = AgcMath.angle(iF_, lambda) * (Math.PI / 180);
            var phidot    = -phi * L / J;
            var vthrust   = (L - 0.5 * L * Math.Pow(phi, 2) - J * phi * phidot - 0.5 * H * Math.Pow(phidot, 2)) * lambda;
            var rthrust_i = S - 0.5 * S * Math.Pow(phi, 2) - Q * phi * phidot - 0.5 * P * Math.Pow(phidot, 2);
            var rthrust   = rthrust_i * lambda - (S * phi + Q * phidot) * AgcMath.normalize(lambdadot);
            var vbias     = vgo - vthrust;

            rbias = rgo - rthrust;

            var up      = AgcMath.normalize(r);
            var east    = AgcMath.normalize(AgcMath.cross(new AgcTuple(0, 0, 1), up));
            var pitch   = AgcMath.angle(iF_, up);
            var inplane = AgcMath.map(up, iF_);
            var yaw     = AgcMath.angle(up, east);
            var tangent = AgcMath.cross(up, east);

            if (AgcMath.dot(inplane, tangent) < 0)
            {
                yaw = -yaw;
            }

            var rc1 = r - 0.1 * rthrust / tgo - (tgo / 30) * vthrust;
            var vc1 = v + 1.2 * rthrust / tgo - 0.1 * vthrust;

            var cserPack = Cser.cse(rc1, vc1, tgo, cser);

            cser  = cserPack.Previous;
            rgrav = cserPack.Radius - rc1 - vc1 * tgo;
            var vgrav = cserPack.Velocity - vc1;

            var rp = r + v * tgo + rgrav + rthrust;

            rp -= AgcMath.dot(rp, iy) * iy;
            rd  = rdval * AgcMath.normalize(rp);
            var ix = AgcMath.normalize(rd);

            iz = AgcMath.cross(ix, iy);

            var vv1 = new AgcTuple(ix.X, iy.X, iz.X);
            var vv2 = new AgcTuple(ix.Y, iy.Y, iz.Y);
            var vv3 = new AgcTuple(ix.Z, iy.Z, iz.Z);

            var vop = new AgcTuple(
                Math.Sin(gamma),
                0,
                Math.Cos(gamma)

                );

            var vd = new AgcTuple(
                AgcMath.dot(vv1, vop),
                AgcMath.dot(vv2, vop),
                AgcMath.dot(vv3, vop)
                );

            vd *= vdval;

            vgo = vd - v - vgrav - vbias;


            Globals.PreviousUpfgState = Globals.CurrentUpfgState;
            Globals.CurrentUpfgState  = new UpfgState(cser, rbias, rd, rgrav, t, v, vgo,
                                                      Globals.PreviousUpfgState.Tb + dt, tgo, dt);
            Globals.VehicleGuidance = new Data.Guidance(iF_, pitch, yaw, 0, 0, tgo);
        }
Esempio n. 2
0
        public static CserState cse(AgcTuple r0, AgcTuple v0, double dt, CserState last)
        {
            double dtcp;

            if (Math.Abs(last.Dtcp) < 0.0000001)
            {
                dtcp = dt;
            }
            else
            {
                dtcp = last.Dtcp;
            }

            var    xcp  = last.Xcp;
            var    x    = xcp;
            var    a    = last.A;
            var    d    = last.D;
            var    e    = last.E;
            double kmax = 10;
            double imax = 10;
            double f0;

            if (dt >= 0)
            {
                f0 = 1;
            }
            else
            {
                f0 = -1;
            }

            var n   = 0d;
            var r0m = Math.Abs(
                AgcMath.magnitude(r0)
                );
            var f1 = f0 * Math.Sqrt(r0m / Globals.MU);
            var f2 = 1 / f1;
            var f3 = f2 / r0m;
            var f4 = f1 * r0m;
            var f5 = f0 / Math.Sqrt(r0m);
            var f6 = f0 * Math.Sqrt(r0m);

            var ir0     = r0 / r0m;
            var v0s     = v0 * f1;
            var sigma0s = AgcMath.dot(ir0, v0s);
            var b0      = Math.Sqrt(
                Math.Abs(
                    AgcMath.magnitude(v0s)
                    )) - 1;
            var alphas = 1 - b0;

            var           xguess = f5 * x;
            var           xlast  = f5 * xcp;
            var           xmin   = 0;
            var           dts    = f3 * dt;
            var           dtlast = f3 * dtcp;
            double        dtmin  = 0;
            double        dtmax  = 0;
            double        xP     = 0;
            double        Ps     = 0;
            List <double> pack;

            var xmax = 2 * Math.PI / Math.Sqrt(Math.Abs(alphas));

            if (alphas > 0)
            {
                dtmax = xmax / alphas;
                xP    = xmax;
                Ps    = dtmax;

                while (dts >= Ps)
                {
                    n      += 1;
                    dts    -= Ps;
                    dtlast -= Ps;
                    xguess -= xP;
                    xlast  -= xP;
                }
            }
            else
            {
                pack  = ktti(xmax, sigma0s, alphas, kmax);
                dtmax = pack[0];

                if (dtmax < dts)
                {
                    while (dtmax < dts)
                    {
                        dtmin = dtmax;
                        xmin  = (int)xmax;
                        xmax  = 2 * xmax;
                        pack  = ktti(xmax, sigma0s, alphas, kmax);
                        dtmax = pack[0];
                    }
                }
            }

            if (xmin >= xguess || xguess >= xmax)
            {
                xguess = 0.5 * (xmin + xmax);
            }

            pack = ktti(xguess, sigma0s, alphas, kmax);
            var dtguess = pack[0];

            if (dts < dtguess)
            {
                if (xguess < xlast && xlast < xmax && dtguess < dtlast && dtlast < dtmax)
                {
                    xmax  = xlast;
                    dtmax = dtlast;
                }
            }
            else
            {
                if (xmin < xlast && xlast < xguess && dtmin < dtlast && dtlast < dtguess)
                {
                    xmin  = (int)xlast;
                    dtmin = dtlast;
                }
            }

            pack    = kil(imax, dts, xguess, dtguess, xmin, dtmin, xmax, dtmax, sigma0s, alphas, kmax, a, d, e);
            xguess  = pack[0];
            dtguess = pack[1];
            a       = pack[2];
            d       = pack[3];
            e       = pack[4];

            var    rs  = 1 + 2 * (b0 * a + sigma0s * d * e);
            var    b4  = 1 / rs;
            double xc  = 0;
            double dtc = 0;

            if (n > 0)
            {
                xc  = f6 * (xguess + n * xP);
                dtc = f4 * (dtguess + n * Ps);
            }
            else
            {
                xc  = f6 * xguess;
                dtc = f4 * dtguess;
            }

            var f   = 1 - 2 * a;
            var gs  = 2 * (d * e + sigma0s * a);
            var fts = -2 * b4 * d * e;
            var gt  = 1 - 2 * b4 * a;

            var r = r0m * (f * ir0 + gs * v0s);
            var v = f2 * (fts * ir0 + gt * v0s);

            return(new CserState(r, v, dtc, xc, a, d, e, last));
        }