real Fs_y = -(mu_s-mu_sd)*y/pow(R_s,3.);

real Fs_z = -(mu_s-mu_sd)*z/pow(R_s,3.);

real R_l = sqrt((x-xl)*(x-xl)+(y-yl)*(y-yl)+(z-zl)*(z-zl));

real Fl_x = -mu_l*x/pow(R_l,3.);

real Fl_y = -mu_l*y/pow(R_l,3.);

real Fl_z = -mu_l*z/pow(R_l,3.);

real V_ka = sqrt(Vx*Vx+Vy*Vy+Vz*Vz);

real Fa_x = (-Cx*sm_a/(2*m))*ro*V_ka*Vx;

real Fa_y = (-Cx*sm_a/(2*m))*ro*V_ka*Vy;

real Fa_z = (-Cx*sm_a/(2*m))*ro*V_ka*Vz;

const real c20 = -1.09808e-3;

const real c22 = 5.74e-6;

const real d22 = -1.58e-6;

const real r_e = 6378137.;

real cr = mu_z*r_e*r_e/pow(R_ka,5);

real lr = 2*atan(y/x);

real mr = 3*(c22*cos(lr)+d22*sin(lr));

real U20_x = cr*x*(c20*(1.5-7.5*z*z/pow(R_ka,2))+mr*(5*z*z/pow(R_ka,2)-3));

real U20_y = cr*y*(c20*(1.5-7.5*z*z/pow(R_ka,2))+mr*(5*z*z/pow(R_ka,2)-3));

real U20_z = cr*z*(c20*(4.5-7.5*z*z/pow(R_ka,2))+5*mr*(z*z/pow(R_ka,2)-1));

dery[0] = Vx;

dery[1] = Vy;

dery[2] = Vz;

dery[3] = (Fz_x+U20_x+Fs_x+Fl_x+Fa_x+akor[0]);

dery[4] = (Fz_y+U20_y+Fs_y+Fl_y+Fa_y+akor[1]);

dery[5] = (Fz_z+U20_z+Fs_z+Fl_z+Fa_z+akor[2]);

Fz = sqrt(Fz_x*Fz_x+Fz_y*Fz_y+Fz_z*Fz_z);

Fs = sqrt(Fs_x*Fs_x+Fs_y*Fs_y+Fs_z*Fs_z);

Fl = sqrt(Fl_x*Fl_x+Fl_y*Fl_y+Fl_z*Fl_z);

Fa = sqrt(Fa_x*Fa_x+Fa_y*Fa_y+Fa_z*Fa_z);

U20 = sqrt(U20_x*U20_x+U20_y*U20_y+U20_z*U20_z);

parn[3] = parn[3]+w_s*t;

par_or(f,par);

korr(t,f,dery);

if ((u_last-par[7]) > 300*g_r)

Fl_u = 1;

u_last = par[7];

}

void korr(real& t, real *f, real *)

{

if (t > (Tkor+172800.))

{

if ((fabs(dl) > 0.1*g_r) && (!Fl_ka) && (!Fl_kp) && (!Fl_ki))

{

Fl_kp = 1;

Fl_ka = 0;

Fl_ki = 0;

cout << "Результат измерений накоплен" << '\n';

cout << "Необходима коррекция периода. dl=" << dl*r_g << "град." << '\n';

cout << "Период ном.=" << parn [6] << "Период тек.=" << par[6] << '\n';

cout << "Параметры орбиты" << '\n';

cout << " Rp = " << par[2]*(1-par[1]) << '\n';

cout << " Ra = " << par[2]*(1+par[1]) << '\n';

cout << " p = " << par[0] << '\n';

cout << " a = " << par[2] << " e = " << par[1] << "\n T = "

<< par[6] << " w = " << par[5]*r_g << " u = " << par[7]*r_g

<< '\n';

clrscr();

}

}

Fl_a = 0;

Fl_p = 0;

Fl_lu = 0;

real da;

if (par[5] > par[7])

da = fabs(par[5]-par[7]-M_PI);

else

da = fabs(par[5]-par[7]+M_PI);

if (da < .1*g_r)

{

Fl_a = 1;

}

if (fabs(par[5] - par[7]) < .1*g_r)

{

Fl_p = 1;

}

if (par[7] < .1*g_r )

{

Fl_lu = 1;

}

real Vk;

if (T_vd)

if (t >= (T_vd +20))

{

T_vd = 0;

akor[0] = 0;

akor[1] = 0;

akor[2] = 0;

cout << "Выкл.дв. \n t = " << t;

}

if (((Fl_kp && Fl_a) || (Fl_ka && Fl_p) || (Fl_ki && Fl_lu)) && (!T_vd))

{

cout << " \n Коррекция \n";

cout << "\n Начало t=" << t << "сек \n";

int sim;

if ((t-Tkor) < 2500)

{

cout << "Не корректировать!";

return;

}

Tkor = t;

real R_t = sqrt(f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);

real V_t = sqrt(f[3]*f[3]+f[4]*f[4]+f[5]*f[5]);

real R_n = parn[0];

if (Fl_a)

{

dRa = R_t-R_n;

dRp = par[2]*(1-par[1])-R_n;

cout << "Апоцентр dRp:" << dRp << "м \n";

cout << "dRa:" << dRa << "м \n";

cout << "w=" << par[5]*r_g << "u=" << par[7]*r_g << '\n';

real l,ln;

l = -(w_z-w_s)*par[6];

ln = -(w_z-w_s)*parn[6];

dl = -(w_z-w_s)*(par[6]-parn[6]);

cout << "T=" << par[6] << "Тном=" << parn[6] << " T-Tном="

<< par[6]-parn[6] << '\n' << "l=" << l*r_g << "lном="

<< ln*r_g << "l-lном=" << (l-ln)*r_g << "dl=" << dl

<< '\n';

if (dRp > 0)

Sig_a = -1;

else

Sig_a = 1;

cout << "Знак ускорения:" << Sig_a << '\n';

clrscr();

real Rp = par[2]*(1-par[1]);

real Ra_p = par[2]*(1+par[1]);

real Rp_p2 = Rp;

real Ra_p2 = R_t;

cout << "Rp=" << Rp_p2 << "Ra=" << Ra_p2 << '\n';

cout << "Ra_p=" << Ra_p << "\n Rt=" << R_t << '\n';

if (fabs(Rp - R_n) < 500)

{

Fl_kp = 0;

Fl_ka = 1;

cout << "Закончить коррекцию в апоцентре \n" << "dRp=" << Rp-R_n

<< "dRa=" << dRa << "t=" << t << '\n';

cout << "Параметры орбиты: \n" << "Rp=" << par[2]*(1-par[1])

<< "Ra=" << par[2]*(1+par[1]) << "\n p=" << par[0]

<< "a=" << par[2] << "e=" << par[1] << "\n T="

<< par[6] << "w=" << par[5]*r_g << "u=" << par[7]*r_g

<< '\n';

cout << "Суммарный импульс для коррекции перицентра=" << dV_ps << '\n';

clrscr();

}

else

{

if (R_t > R_n)

{

Rp_p = R_n;

Ra_p = R_t;

a_p = (Ra_p+Rp_p)/2.;

e_p = 1-Rp_p/a_p;

p_p = a_p*(1-e_p*e_p);

Vk = sqrt(mu_z/p_p)*(1-e_p);

}

else

{

Rp_p = R_t;

Ra_p = R_n;

a_p = (Ra_p+Rp_p)/2.;

e_p = 1-Rp_p/a_p;

p_p = a_p*(1-e_p*e_p);

Vk = sqrt(mu_z/p_p)*(1+e_p);

}

real dV = Vk-V_t;

real dVmax = 20*25./m;

cout << "\n dVтреб=" << dV << "dVmax за 20 сек=" << dVmax;

if (fabs(dV) > dVmax)

{

akor[0] = Sig_a*(25./m)*f[3]/V_t;

akor[1] = Sig_a*(25./m)*f[4]/V_t;

akor[2] = Sig_a*(25./m)*f[5]/V_t;

cout << "\n dV=" << dV << "dVmax=" << dVmax;

cout << "\n Корректирующее ускорение:" << akor[0] << '\t' << akor[1]

<< '\t' << akor[2] << '\t' <<

sqrt(akor[0]*akor[0]+akor[1]*akor[1]+akor[2]*akor[2]) << '\n';

dV_ps = dV_ps+dVmax;

cout << "Суммарный импульс=" << dV_ps << '\n';

}

else