Перечень глобальных идентификаторов





 

В данном разделе приведен перечень используемых в процессе реализации моделей, оговоренных в пояснительной записке, глобальных идентификаторов.

Таблица 7.2

Одиночные идентификаторы (не вектора)
   
Drgr Положение сектора газа
Eta Отклонение руля высоты
Ks Отклонение руля направления
Ts Отклонение элеронов
TIME Время моделирования
DT Шаг интегрирования
BE0 Заданное значение угла скольжения
PH0 Заданное значение угла крена
P0,Q0,R0 Заданные значение проекций угловой скорости
PSIG Заданное значение угла крена
V0 Заданная скорость
H0 Заданная высота
BTeta Угол тангажа, рассчитываемый БИНСом
BGamma Угол крена, рассчитываемый БИНСом
BPsi Угол курса, рассчитываемый БИНСом

 


Таблица 7.3. Глобальный идентификатор-вектор параметров PAR [ ]

индекс обозначение значение размерность комментарий
Qt 1.527778   коэффициент расхода топлива
S кв м площадь крыльев
m кг масса ЛА
Rf 2.130 м плечо вектора тяги
Ix кг*кв м осевой момент инерции
Iy кг*кв м осевой момент инерции
Iz кг*кв м осевой момент инерции
Ixy -400000 кг*кв м центробежный момент инерции
SAH (LMU) 6.608 м средняя аэродинамическая хорда
l (HS) 22.42 м размах крыльев
XSP % центровка относительно САХ
LAM 7.730 - коэффициент влияния подъемной силы на лобовое сопротивление
F0 182353.2 Н максимальная тяга
Ro0 1.225   плотность воздуха на поверхности земли
g 9.80665 м/кв с ускорение от силы тяжести
GR 57.295779 - коэффициент перевода градуса в радианы
pi 3.141 - число "пи"
Vs0 м/с Заданная скорость полета (задана в полетном задании)
H0 м Заданная высота полета (задана в полетном задании)
Gam рад Заданный угол наклона траектории (задан в полетном задании)
Gamma0 рад Заданный угол крена при горизонтальном полете
Gamma0 0.2 рад Заданный угол крена при повороте
Be0 град Заданный угол скольжения на горизонтальном участке (начальный)
Be0 0.02 рад Заданный угол скольжения на участке поворота (начальный)
wx0 рад/с начальное значение проекции угловой скорости
wy0 рад/с начальное значение проекции угловой скорости
wz0 рад/с начальное значение проекции угловой скорости
Psig1 град угол курса до совершения поворота
Psig град угол курса после совершения поворота (угол поворота)
TIME1 c время полета на первом участке
TIME2 с время полета на втором участке
TIME3 с время полета на третьем участке
DT 0.1 с шаг интегрирования
бn1 0.1 м / кв.с. ошибка акселлерометра по оси Х
бn2 0.1 м / кв.с. ошибка акселлерометра по оси Y
бn3 0.1 м / кв.с. ошибка акселлерометра по оси Z
бOMEGA1 0.001 рад / с. ошибка ДУСа по оси Х
бOMEGA2 0.001 рад / с. ошибка ДУСа по оси Y
бOMEGA3 0.001 рад / с. ошибка ДУСа по оси Z
φ0 0.872 рад. начальное значение широты (широта точки старта)
λ0 0.51 рад. начальное значение долготы (долгота точки старта)

Исходный код программы

 

В данном разделе приводится исходный текст разработанной программы. Вся программа состоит из семи форм и 18 модулей. Плюс существует главный модуль, носящий имя DISPCKM. Заметим, что у формы есть 2 вида модулей - .cpp и .h файлы. Все команды пишутся в файлах .cpp, а в .h файлах содержится инициализация компонентов формы. В силу этого мы опустим рассмотрение .h файлов. При желании Вы сможете найти их на прилагаемом к работе компакт-диске.

 

AISB

// АЛГОРИТМ ИНИЦИАЛИЗАЦИИ СОСТОЯНИЯ БОРТА

//-----------------------------------------------------

 

#include "..\\Include\\extpar.h"

#include "math.h"

 

//-----------------------------------------------------

 

void AISB()

{

float psi=X[8];

float gamma=X[7];

float teta=X[6];

 

BFI=FI0;

BLAMB=LAMB0;

BVE=VTR*cos(PSIGTR);

BVN=VTR*sin(PSIGTR);

 

C[0][0]=cos(psi)*cos(teta);

C[0][1]=sin(psi)*sin(gamma)-cos(psi)*sin(teta)*cos(gamma);

C[0][2]=sin(psi)*cos(gamma)+cos(psi)*sin(teta)*sin(gamma);

 

C[2][0]=sin(teta);

C[2][1]=cos(teta)*cos(gamma);

C[2][2]=-cos(teta)*sin(gamma);

 

// помножил Z на -1 (в обр сторону)

C[1][0]=sin(psi)*cos(teta);

C[1][1]=-cos(psi)*sin(gamma)*-sin(psi)*sin(teta)*cos(gamma);

C[1][2]=-cos(psi)*cos(gamma)+sin(psi)*sin(teta)*sin(gamma);

 

BTETA=asin(C[2][0]);

BGAMMA=asin(-C[2][2]/sqrt(1-pow(C[2][0],2)));

BPSI=atan(C[1][0]/fabs(C[0][0]));

BXG=0.0;

BZG=0.0;

}

//-----------------------------------------------------

 

AIVSO

 

// АЛГОРИТМ ИНИЦИАЛИЗАЦИИ ВЕКТОРА СОСТОЯНИЯ ОБЪЕКТА

 

//-----------------------------------------------------

 

#include "..\\Include\\extpar.h"

 

//-----------------------------------------------------

 

void AIVSO()

{

X[0]=VTR;

X[1]=ALP0;

X[2]=BETR;

X[3]=PTR;

X[4]=RTR;

X[5]=QTR;

X[6]=TETA0;

X[7]=GAMMATR;

X[8]=PSIGTR/GR; // в радианах ! (PSIGTR - в градусах!)

X[9]=0.0;

X[10]=HTR;

X[11]=0.0;

X[12]=M0;

X[13]=0.0;

X[14]=0.0;

X[15]=0.0;

X[16]=0.0;

X[17]=0.0;

X[18]=0.0;

X[19]=0.0;

X[20]=FI0;

X[21]=LAMB0;

}

 

//-----------------------------------------------------

AZPZ

 

// АЛГОРИТМ ЗАГРУЗКИ ПОЛЕТНОГО ЗАДАНИЯ

 

//-----------------------------------------------------

 

#include "..\\Include\\extpar.h"

#include "stdio.h"

 

//-----------------------------------------------------

 

int AZPZ()

{

// локальные идентификаторы

 

FILE *FPZ;

int i;

int FREADVALUE(FILE*,void*,char);

 

// основной раздел

 

if ((FPZ=fopen(FPZPATH,"r"))==NULL) return 1; // код ошибки "1" - файл не найден

else

{

FREADVALUE(FPZ,&KORT,'i');

 

SX=new float [KORT];

PSIG=new float [KORT];

 

for (i=0;i<KORT;i++)

{

FREADVALUE(FPZ,&SX[i],'f');

FREADVALUE(FPZ,&PSIG[i],'f');

}

return 0; // код "0" - без ошибок

}

}

 

//-------------------------------------------------------

 

DISPBAL

 

// ДИСПЕТЧЕР ПРОЦЕДУР РАСЧЕТА БАЛАНСИРОВОЧНЫХ ПАРАМЕТРОВ

 

#include "stdio.h"

#include "..\\Include\\extpar.h"

//-------------------------------------------------------

 

void DISPBAL(void)

{

// локальные идентификаторы

 

FILE *fBalance;

float FW;

 

// прототипы используемых функций

 

float WRK50();

float TURNBAL();

 

// основной раздел

 

// обнуление идентификаторов балансировочных параметров

 

DRGR0=0.0;

ETA0=0.0;

ALP0=0.0;

TETA0=0.0;

KS0=0.0;

TS0=0.0;

 

// выбор метода расчета балансировочных параметров в зависимости от режима полета

 

if (PRNP) FW=TURNBAL(); // поворот

else FW=WRK50(); // горизонтальный полет

TETA0=ALP0+GAMTR; // балансировочное значение угла тангажа

 

// запись файла с результатами

 

fBalance=fopen(FBALPATH,"a"); // файл открывается для ДОЗАПИСИ !!!

fprintf(fBalance,"\n\**************************\n");

fprintf(fBalance,"\n Расчет балансировочных параметров :\n");

fprintf(fBalance,"\n\**************************\n");

fprintf(fBalance,"\n Признак поворота = %i",PRNP);

fprintf(fBalance,"\n \n Drgr0 = %f",DRGR0);

fprintf(fBalance,"\n \n Eta0 = %f",ETA0);

fprintf(fBalance,"\n \n Alp0 = %f",ALP0);

fprintf(fBalance,"\n \n Teta0 = %f",TETA0);

fprintf(fBalance,"\n \n Ks0 = %f",KS0);

fprintf(fBalance,"\n \n Ts0 = %f",TS0);

fprintf(fBalance,"\n \n FW = %f",FW);

fprintf(fBalance,"\n\n");

fprintf(fBalance,"\n\n GAMMATR = %f",GAMMATR);

fprintf(fBalance,"\n\n BETR = %f",BETR);

fclose(fBalance);

}

HORIZBAL

 

// процедуры расчета балансировочных параметров при горизонтальном полете

// (WRK50 + PKS + FCT)

 

//-------------------------------------------------------

 

#include "math.h"

#include "..\\Include\\extpar.h"

 

//-------------------------------------------------------

 

float WRK50(

// входные параметры

// выходные параметры

)

{

// локальные идентификаторы

 

const float EPS=0.000000000001;

float P[50][50]={0.0}, FX=0.0, FX0=0.0, XL[5]={0.0}, A=0.0, B=0.0, ZN=0.0,

KN=0.0;

int i=0,j=0,k=0,N=5,M=0;

float BAL[6];

 

// прототипы используемых функций

 

float FCT(float []);

void PKS(float[][50],float[]);

float AMIN(float,float);

float AMAX(float,float);

 

FX0=FCT(BAL);

 

AA: for (i=0;i<N;i++) // for 1

{

for (j=0;j<N;j++)

{

P[i][j]=0.0;

if (i==j) P[i][j]=1.0;

}

} // end for 1

 

PKS(P,BAL);

 

AB: for (i=0;i<N;i++) XL[i]=BAL[i];

PKS(P,BAL);

A=0.0;

for (i=0;i<N;i++) // for 2

{

B=AMIN(fabs(BAL[i]),fabs(XL[i]));

M=0;

if (B<1.0) goto AC;

M=(log10(B)+1);

AC: A=A+pow(((XL[i]-BAL[i])/pow(10,M)),2);

} // end for 2

A=sqrt(A);

if (A<EPS || A==EPS) goto AG;

A=0.0;

for (i=0;i<N;i++) A=A+pow((XL[i]-BAL[i]),2);

A=sqrt(A);

for (i=0;i<N;i++) XL[i]=BAL[i]-XL[i];

for (i=0;i<N;i++) P[i][1]=XL[i]/A;

if (N==1) goto AB;

for (k=2;k<N;k++) // for 3

{

if (A==0.0) goto AE;

A=0.0;

for (i=k;i<N;i++) A=A+pow(XL[i],2);

if (A==0.0) goto AE;

B=A+pow(XL[k-1],2);

ZN=sqrt(A)*sqrt(B);

if (k==2) goto AD;

KN=k-2;

for (i=0;i<KN;i++) P[i][k]=0.0;

AD: P[k-1][k]=-A/ZN;

for(i=k;i<N;i++) P[i][k]=XL[k-1]*XL[i]/ZN;

goto AF;

AE: for (i=0;i<N;i++)

{

P[i][k]=0.0;

if (i==k) P[i][k]=1.0;

}

AF:;

} // end for 3

goto AB;

 

AG: FX=FCT(BAL);

 

if (FX>FX0*(1-EPS) || FX==FX0*(1-EPS)) goto AH;

else

{

FX0=FX;

goto AA;

}

AH: BAL[5]=FX;

 

DRGR0=BAL[0];

ETA0=BAL[1];

ALP0=BAL[2];

KS0=BAL[3];

TS0=BAL[4];

return BAL[5];

}

 

//-------------------------------------------------------

 

void PKS(

// входные параметры

float P[50][50],

 

// выходные параметры

float BAL[]

)

{

// локальные идентификаторы

 

const float EN=0.0000001,

ET=10.0,

DPS=0.000000000001;

 

float Z=0.0, FX=0.0, A=0.0, XMX=0.0, SMN=0.0, FMN=0.0, FMX=0.0, FX1=0.0,

APR=0.0, FX2=0.0, CH=0.0, ZN=0.0, FN=0.0,

XW[5]={0.0};

 

int KP=0, KM=0, i=0, j=0, N=5;

 

// прототипы используемых функций

 

float FCT(float[]);

float AMIN(float,float);

float AMAX(float,float);

 

for (j=0;j<N;j++) // for 1 - цикл на все тело функции

{

Z=1.0;

 

FX=FCT(BAL);

 

A=fabs(BAL[0]);

XMX=A;

SMN=fabs(P[1][j]);

for (i=0;i<N;i++) // for 2

{

SMN=AMIN(SMN,fabs(P[i][j]));

XMX=AMAX(XMX,fabs(BAL[i]));

A=AMIN(A,fabs(BAL[i]));

}// end for 2

if (SMN<DPS) SMN=DPS;

A=A*EN;

if (A<EN) A=EN;

FMN=FX*(1.0-DPS);

FMX=FX*(1.0+DPS);

N4: KP=0;

KM=0;

for (i=0;i<N;i++) XW[i]=BAL[i]+A*P[i][j];

FX1=FCT(XW);

if (FX1<FMN) goto N5;

if (FX1>FMX) KP=1;

for (i=0;i<N;i++) XW[i]=BAL[i]-A*P[i][j];

FX1=FCT(XW);

if (FX1<FMN) goto N7;

if (FX1>FMX) KM=1;

if ((KP+KM)==2) goto N3;

if ((A*SMN)>AMAX(XMX*ET,ET) || (A*SMN)==AMAX(XMX*ET,ET)) goto N3;

A=A*2.0;

goto N4;

N7: Z=-1.0;

N5: APR=A;

A=A*2.0;

for (i=0;i<N;i++) XW[i]=BAL[i]+Z*A*P[i][j];

 

FX2=FCT(XW);

 

if (FX2>FX1 || FX2==FX1) goto N10;

if ((A*SMN)>AMAX(XMX*ET,ET) || (A*SMN)==AMAX(XMX*ET,ET)) goto N9;

FX1=FX2;

goto N5;

N10: CH=3.0*FX-4.0*FX1+FX2;

ZN=2.0*FX-4.0*FX1+2.0*FX2;

if (ZN==0.0) goto N12;

A=(CH/ZN)*A*0.5;

for (i=0;i<N;i++) XW[i]=BAL[i]+Z*A*P[i][j];

FN=FCT(XW);

if (FN<FX1 || FN==FX1) goto N9;

N12: for (i=0;i<N;i++) BAL[i]=BAL[i]+Z*APR*P[i][j];

goto N3;

N9: for (i=0;i<N;i++) BAL[i]=XW[i];

N3:;

}// end for 1

}

 

//-------------------------------------------------------

 

float FCT(

// входные параметры

 

float Bal[] // вектор предполагаемых оптимальных балансировочных параметров

 

// выходные параметры

// возвращает значение fct

 

)

{

// локальные идентификаторы

 

float VS=0.0, ALPS=0.0, BES=0.0, P=0.0, R=0.0,

Q=0.0,M=0.0,

SINAL=0.0, COSAL=0.0, PS=0.0,RS=0.0,OME=0.0,QS=0.0,

QQ=0.0,QQF=0.0,QQM=0.0,

FMAX=0.0, H,

FX[3]={0.0}, fct=0.0,

MG=0.0;

//ETA,TS,KS,XS=0.0,YS=0.0,MS=0.0,RH0=0.0;

 

int i=0;

// прототипы используемых функций

 

void AEROFLU(float,float,float,float,float,float);

 

// основной раздел

 

ETA=Bal[1];

ALPS=Bal[2];

KS=Bal[3];

TS=Bal[4];

VS=VTR;

BES=X[2];

P=X[3];

R=X[4];

Q=X[5];

H=X[10];

M=X[12];

MG=M*G;

SINAL=sin(ALPS); COSAL=cos(ALPS);

PS=P*COSAL-R*SINAL; RS=P*SINAL+R*COSAL;

OME=(G/VS)*tan(GAMMATR);

R=OME*cos(GAMMATR); Q=OME*sin(GAMMATR); QS=Q;

QQ=0.5*RH*pow(VS,2); QQF=QQ*S; QQM=QQF*LMU;

AEROFLU(VS,ALPS,BES,PS,RS,QS);

FMAX=Bal[0]*F0;

XS=cXS*QQF; YS=cYS*QQF;

MS=cMS*QQM;

FX[0]=YS+FMAX*sin(Bal[2])-MG*cos(GAMTR);

FX[1]=-XS-FMAX*cos(Bal[2])+MG*sin(GAMTR);

FX[2]=MS+RF*FMAX;

 

i=0;

while (i!=3) { fct=fct+pow(FX[i],2); i++; }

return fct;

}

 

//-------------------------------------------------------

 

TURNBAL

// АЛГОРИТМ РАСЧЕТА БАЛАНСИРОВОЧНЫХ ПАРАМЕТРОВ ПРИ ПОВОРОТЕ

 

#include "math.h"

#include "..\\Include\\extpar.h"

 

//-------------------------------------------------------

float TURNBAL(

// входные параметры

// выходные параметры

)

{

 

// локальные идентификаторы

 

const float QQ=2508056.0,

QQM=16573234.0,

CA0=0.2862,

CAAL=6.5518,

CAET=0.4162,

CAQ0=4.135,

CAQ25=-7.0,

CM0=-0.05745,

CMAL=-1.3174,

CMET=-1.648,

CMQ0=-14.7,

CMQ25=2.8,

CW0=0.01544,

W1=0.0066,

W2=0.0350,

W3=0.0412;

 

float MG,CAQ,CMQ, TUV, CAA, CA[200], UU,

Z0, Z1, Z2, Z3, Z4, DE, F, CM, CW,

XX, Y, D, AL, ET, B, HSV,

CLTS, CLKS, CLR, CCLR, CNTS, CNKS,

CNR, CCNR, ZKS, ZTS,

XSP25,M;

 

int N1,N2,j,k;

 

// основной раздел

 

M=X[12];

XSP25=(XSP-25.0)/100.0;

MG=M*G;

CAQ=CAQ0+CAQ25*XSP25;

CMQ=CMQ0+CMQ25*XSP25;

N1=100;

N2=100;

 

// ***** СA & BE *****

 

TUV=LMU/VTR;

CAA=0.4495;

CA[0]=CAA;

UU=CA0+CAQ*QTR*TUV;

Z0=(CW0*UU)/CAAL+MG/(QQ*cos(GAMMATR));

Z1=(-CW0+UU*W1)/CAAL;

Z2=(-W1+UU*W2)/CAAL;

Z3=(-W2+UU*W3)/CAAL;

Z4=-W3/CAAL;

for (j=0;j<N2;j++)

{

CA[j+1]=Z0+Z1*CA[j]+Z2*pow(CA[j],2)+Z3*pow(CA[j],3)+Z4*pow(CA[j],4);

CAA=CA[j+1];

if (fabs((CA[j+1]-CA[j]))<0.001) goto M3;

}

M3:DE=(QQ/F0)*(CW0+W1*CAA+W2*pow(CAA,2)+W3*pow(CAA,3));

 

// ***** DE & ET ******

 

F=DE*F0; CM=-(F*RF)/QQM;

XX=MG/(QQ*cos(GAMMATR))-CA0-CAQ*QTR*TUV;

Y=CM-CM0-CMQ*QTR*TUV-XSP25*CAA;

D=(CAAL+F/QQ)*CMET-CAET*CMAL;

AL=(XX*CMET-Y*CAET)/D;

ET=(Y*(CAAL+F/QQ)-XX*CMAL)/D;

UU=UU+CAET*ET;

Z0=MG/(QQ*cos(GAMMATR))+(UU*CW0)/CAAL;

B=1.0/cos(AL);

Z1=B*((-CW0-MG*sin(AL)*(1.0-cos(GAMMATR))/QQ+UU*W1)/CAAL);

Z2=B*(-W1+UU*W2)/CAAL;

Z3=B*(-W2+UU*W3)/CAAL;

Z4=-B*W3; CA[0]=CAA;

for (k=0;k<N1;k++)

{

CA[k+1]=Z0+Z1*CA[k]+Z2*pow(CA[k],2)+Z3*pow(CA[k],3)+Z4*pow(CA[k],4);

CAA=CA[k+1]; if (fabs((CA[k+1]-CA[k]))<0.0002) goto M13;

}

M13:CW=CW0+W1*CAA+W2*pow(CAA,2)+W3*pow(CAA,3);

DE=(CW*QQ/cos(AL)+MG*sin(AL)*(1.0-cos(GAMMATR)))/F0;

 

// ***** AL & ET *****

 

F=DE*F0;

CM=-(F*RF)/QQM;

XX=MG/(QQ*cos(GAMMATR))-CA0-CAQ*QTR*TUV;

Y=CM-CM0-CMQ*QTR*TUV-XSP25*CAA;

D=(CAAL+F/QQ)*CMET-CAET*CMAL;

AL=(XX*CMET-Y*CAET)/D;

ET=(Y*(CAAL+F/QQ)-XX*CMAL)/D;

//********************************************************************

DRGR0=DE; ETA0=ET; ALP0=AL;

//********************************************************************

 

// ***** TS & KS *****

 

HSV=HS/VTR;

CLTS=0.069-0.25*AL;

CLKS=-0.046;

CLR=0.304+1.74*AL+(-0.49-1.84*AL)*XSP25;

CCLR=CLR*RTR*HSV;

CNTS=-0.252;

CNKS=-0.02+0.05*AL;

CNR=-0.6-0.36*AL-1.39*pow(AL,2)+(0.433+0.42*AL)*XSP25;

CCNR=CNR*RTR*HSV;

D=CLTS*CNKS-CNTS*CLKS;

ZKS=(CCNR*CLKS-CCLR*CNKS)/D;

ZTS=(CCLR*CNTS-CCNR*CLTS)/D;

//*******************************************************

KS0=ZKS; TS0=ZTS; return 0.0002;

//*******************************************************

}

//-------------------------------------------------------

 

 

DISPCKM

// ДИСПЕТЧЕР МОДЕЛИРУЮЩЕГО КОМПЛЕКСА

 

//-------------------------------------------------------

 

#include "..\\Include\\extpar.h"

#include "..\\Include\\APR.h"

#include "..\\Include\\TLM.h"

#include "..\\Include\\ARR.h"

#include "stdio.h"

#include "string.h"

#include "math.h"

//-------------------------------------------------------

 

void main(void)

{

int PRNWTLM;

 

// основной раздел

 

void SREDA();

void OBEKT();

void FORMDYT();

void RK4(float[],float[],int,float,float*);

void DEFORMYT();

void BORT();

void AIUUS();

 

// установка путей основных файлов исходных данных

 

strcpy(FBALPATH,"..\\Obr_tlm\\Bal.txt");

strcpy(FPZPATH,"..\\Pz\\PZ");

strcpy(FOBJPATH,"..\\Obr_tlm\\Object.ris");

strcpy(FUPRPATH,"..\\Obr_tlm\\Upr.ris");

strcpy(FBINSPATH,"..\\Obr_tlm\\Bins.ris");

 

 

// инициализация начального состояния

 

NORT=1;

OPENTLMFILES();

AZPZ();

 

// расчет начального состояния

 

ARVP(); // расчет требуемых (выдерживаемых управлением) параметров на ортодромии

 

AIVSO(); // инициализация вектора состояния объекта (до расчета

// начальных балансировочных параметров)

 

SREDA(); // моделирование состояния окружающей среды

 

DISPBAL(); // расчет начальных балансировочных параметров

 

AIVSO(); // инициализация вектора состояния объекта (с учетом

// рассчитанных начальных балансировочных параметров)

 

AISB();

DRGR=DRGR0;

ETA=ETA0;

KS=KS0;

TS=TS0;

 

// основной цикл программы

// моделирование

 

begin: SREDA();

 

OBEKT(); // моделирование движения объекта

AIUUS(); // измерение ускорения и угловой скорости

SXTEK=sqrt(pow(X[9],2)+pow(X[11],2));

PSITEK=X[8];

 

// бортовые алгоритмы

 

if (!PRNP) // на ортодромии

{

if(SXTEK>=(SPR+SXTR)) // точка начала поворота

{

PRNP=1;

PSIPR=X[8];

NORT++;

if (NORT>KORT) goto end;

ARVP();

DISPBAL();

printf("\n\n NACHALO POVOROTA %f",TIME);

}

}

if (PRNP) // переход с ортодромии на ортодромию

{

if ((DPSITEK>0 && PSITEK>(PSIPR+DPSITEK)) || (DPSITEK<0 && PSITEK<(PSIPR+DPSITEK)) ) // условие выхода на следующую ортодромию

{

PRNP=0;

SPR=SXTEK;

ARVP();

DISPBAL();

printf("\n\n KONEC POVOROTA %f",TIME);

}

}

 

AONP();

AFUVRO();

 

if (!(NCICL%(int)((DT*100)/DT))) PRNWTLM=1;

if (PRNWTLM) WRITETLMFILES();

 

// интегрирование системы дифф уравнений

 

FORMDYT();

RK4(YT,DYT,NPOR,DT,&TIME);

DEFORMYT();

 

NCICL++;

goto begin;

 

end: printf("\n\n KONECHNOE VREMA %f \n\n",TIME);

 

CLOSETLMFILES();

}

 

//-------------------------------------------------------

 

 

RK4

// АЛГОРИТМ ФОРМИРОВАНИЯ / ДЕФОРМИРОВАНИЯ ВЕКТОРА ИНТЕГРИРУЕМЫХ ПАРАМЕТРОВ

 

#include "..\\Include\\extpar.h"

 

//-------------------------------------------------------

 

void FORMDYT()

{

YT[0]=X[0];

YT[1]=X[1];

YT[2]=X[2];

YT[3]=X[3];

YT[4]=X[4];

YT[5]=X[5];

YT[6]=X[6];

YT[7]=X[7];

YT[8]=X[8];

YT[9]=X[9];

YT[10]=X[10];

YT[11]=X[11];

YT[12]=X[12];

YT[13]=X[13];

YT[14]=X[14];

YT[15]=X[15];

YT[16]=X[16];

YT[17]=X[17];

YT[18]=X[18];

YT[19]=W[0];

YT[20]=W[1];

YT[21]=W[2];

YT[22]=C[0][0];

YT[23]=C[0][1];

YT[24]=C[0][2];

YT[25]=C[1][0];

YT[26]=C[1][1];

YT[27]=C[1][2];

YT[28]=C[2][0];

YT[29]=C[2][1];

YT[30]=C[2][2];

YT[31]=BFI;

YT[32]=BLAMB;

YT[33]=BVE;

YT[34]=BVN;

YT[35]=BXG;

YT[36]=BZG;

YT[37]=A[0][0];

YT[38]=A[0][1];

YT[39]=A[0][2];

YT[40]=A[1][0];

YT[41]=A[1][1];

YT[42]=A[1][2];

YT[43]=A[2][0];

YT[44]=A[2][1];

YT[45]=A[2][2];

DYT[0]=DX[0];

DYT[1]=DX[1];

DYT[2]=DX[2];

DYT[3]=DX[3];

DYT[4]=DX[4];

DYT[5]=DX[5];

DYT[6]=DX[6];

DYT[7]=DX[7];

DYT[8]=DX[8];

DYT[9]=DX[9];

DYT[10]=DX[10];

DYT[11]=DX[11];

DYT[12]=DX[12];

DYT[13]=DX[13];

DYT[14]=DX[14];

DYT[15]=DX[15];

DYT[16]=DX[16];

DYT[17]=DX[17];

DYT[18]=DX[18];

DYT[19]=DW[0];

DYT[20]=DW[1];

DYT[21]=DW[2];

DYT[22]=DC[0][0];

DYT[23]=DC[0][1];

DYT[24]=DC[0][2];

DYT[25]=DC[1][0];

DYT[26]=DC[1][1];

DYT[27]=DC[1][2];

DYT[28]=DC[2][0];

DYT[29]=DC[2][1];

DYT[30]=DC[2][2];

DYT[31]=DBFI;

DYT[32]=DBLAMB;

DYT[33]=DBVE;

DYT[34]=DBVN;

DYT[35]=DBXG;

DYT[36]=DBZG;

DYT[37]=DA[0][0];

DYT[38]=DA[0][1];

DYT[39]=DA[0][2];

DYT[40]=DA[1][0];

DYT[41]=DA[1][1];

DYT[42]=DA[1][2];

DYT[43]=DA[2][0];

DYT[44]=DA[2][1];

DYT[45]=DA[2][2];

}

void DEFORMYT()

{

X[0]=YT[0];

X[1]=YT[1];

X[2]=YT[2];

X[3]=YT[3];

X[4]=YT[4];

X[5]=YT[5];

X[6]=YT[6];

X[7]=YT[7];

X[8]=YT[8];

X[9]=YT[9];

X[10]=YT[10];

X[11]=YT[11];

X[12]=YT[12];

X[13]=YT[13];

X[14]=YT[14];

X[15]=YT[15];

X[16]=YT[16];

X[17]=YT[17];

X[18]=YT[18];

W[0]=YT[19];

W[1]=YT[20];

W[2]=YT[21];

C[0][0]=YT[22];

C[0][1]=YT[23];

C[0][2]=YT[24];

C[1][0]=YT[25];

C[1][1]=YT[26];

C[1][2]=YT[27];

C[2][0]=YT[28];

C[2][1]=YT[29];

C[2][2]=YT[30];

BFI=YT[31];

BLAMB=YT[32];

BVE=YT[33];

BVN=YT[34];

BXG=YT[35];

BZG=YT[36];

A[0][0]=YT[37];

A[0][1]=YT[38];

A[0][2]=YT[39];

A[1][0]=YT[40];

A[1][1]=YT[41];

A[1][2]=YT[42];

A[2][0]=YT[43];

A[2][1]=YT[44];

A[2][2]=YT[45];

}

 

//-------------------------------------------------------

// АЛГОРИТМ 1 ШАГА ИНТЕГРИРОВАНИЯ МЕТОДОМ РУНГЕ - КУТТА 4 ПОРЯДКА

//-------------------------------------------------------

void RK4(float YT[],float DYT[],int Npor,float DT,float *TIME)

{

// local variables

 

float A[5]={0.5,0.5,1.0,1.0,0.5};

float dx[100]={0.0}, D=0.0, C=0.0, T=0.0;

int i=0,j=0;

 

// main body

 

for (i=0;i<Npor;i++) dx[i]=0.0;

 

for (j=0;j<4;j++)

{

D=A[j+1];

C=A[j];

T=*TIME+C*DT;

for (i=0;i<Npor;i++) dx[i]=dx[i]+D*DT*DYT[i]/3.0;

}

 

for (i=0;i<Npor;i++) YT[i]=YT[i]+dx[i];

 

*TIME=T;

}

 

//-------------------------------------------------------

 

 

AEROFLU

 

// АЭРОДИНАМИКА ОБЪЕКТА (РАСЧЕТ КОЭФФИЦИЕНТОВ АЭРОДИНАМИЧЕСКИХ СИЛ И МОМЕНТОВ)

 

//-------------------------------------------------------

#include "math.h"

#include "..\\Include\\extpar.h"

//-------------------------------------------------------

 

void AEROFLU(

// входные параметры (формальные)

 

float VS, // текущая прроекция вектора скорости на оси полусвязанной СК

float ALPS, //текущий угол атаки в полусвязанной СК

float BES, // текущий угол скольжения в полусвязанной СК

float PS, float RS, float QS // текущие проекции вектора угловой скорости вращения ЛА на оси полусвязанной СК

 

// входные параметры (глобальные)

// управляющие воздействия

// вектор состояния объекта (текущая высота)

// вектор приращений компонент вектора состояния объекта (текущая скорость изменения угла атаки)

// плотность воздуха на текущей высоте RH

// G

// конструктивные параметры объекта

// выходные параметры (глобальные)

// коэффициенты аэродинамических сил и моментов

)

{

// локальные идентификаторы

 

float HSV, QQG, // вспомогательные

VA, // скорость звука

MZ, // число Маха

XSP25, // отклонение от 25%-й центровки масс ЛА

D1ALP=DX[1], // скорость изменения угла атаки

H=X[10], // текущая высота

// CA

ALFR,

CAALFR,

ALH,

CAALH,

CAET,

CAQ25,

CAQ,

CADAL,

CA,

// CW

 

CW0,

CWI,

DCWMZ,

CW,

 

// CM

 

XNFR,

XNFR25,

CMFR0,

CMFR25,

CMH,

CMQ25,

DCMQ25,

CMALFR,

DMALFR,

CMQ,

CMDAL,

CM,

// CZ

 

CZBE,

CZP,

CZR,

CZ,

// CL

 

CLBE,

CLP,

CLR,

CLKS,

CLTS,

CL,

 

// CN

 

CNBE,

CNP,

CNR,

CNKS,

CNTS,

CN;

// основной раздел

 

HSV=HS/VS;

VA=sqrt(1.4*29.27*G*(288.15-0.0065*H));

MZ=VS/VA;

QQG=0.5*RH*pow(VS,2)/G;

XSP25=(XSP-25.0)/100.0;

 

// ***** CA ( по нашему CY ) *****

 

ALFR=ALPS+0.0147*MZ+0.0373;

CAALFR=8.5*MZ-0.54;

ALH=ALPS+0.0291*MZ-0.0348-(0.483*MZ+0.107)*ALPS;

CAALH=0.78*MZ+0.286;

CAET=0.1079*MZ+0.332;

CAQ25=2.75*MZ+1.99;

CAQ=CAQ25-XSP25*(CAALFR+CAALH);

CADAL=2.67*MZ-0.35;

CA=CAALFR*ALFR+CAALH*ALH+CAET*ETA+CAQ*QS*LMU/VS+CADAL*LMU/VS*D1ALP;

 

// ***** CW ( по нашему Сх ) *****

 

CW0=-0.01706-0.00000013*H+0.0016*MZ;

CWI=-((0.85+(CA))/(pi*LAM))*pow((CA),2);

DCWMZ=0.0018-0.0066*(CA)-0.0283*(MZ-0.78);

CW=CW0+CWI+DCWMZ;

 

// ***** CM ( по нашему mz ) *****

 

XNFR=-18.5+46.7*MZ;

XNFR25=(25.0-XNFR)/100.0;

CMFR0=-0.057-0.08*MZ;

CMFR25=CMFR0+XNFR25*CAALFR*ALFR;

CMH=-3.786*CAALH*ALH-(3.904+0.0715*MZ)*CAET*ETA;

CMQ25=-19.4*MZ-1.3;

DCMQ25=0.000833*QQG+0.7*MZ-0.463;

CMALFR=XNFR25*CAALFR;

DMALFR=0.0001233*QQG+0.2*MZ-0.1333;

CMQ=CMQ25+DCMQ25+XSP25*(3.786*CAALH-CMALFR-DMALFR);

CMDAL=-14.36*MZ+4.75;

CM=CMFR25+CMH+CMQ*QS*LMU/VS+XSP25*(CA)+CMDAL*LMU/VS*D1ALP;

 

// ***** CZ ( по нашему Cz ) *****

 

CZBE=-0.5944-0.67*MZ;

CZP=-0.0086+2.0514*ALPS-0.199*MZ+2.29*MZ*ALPS;

CZR=-0.563-0.2*ALPS-0.15*MZ-XSP25*CZBE;

CZ=CZBE*BES+CZP*PS*HSV+CZR*RS*HSV+0.222*TS;

 

// ***** CL ( по нашему mx ) *****

 

CLBE=-0.175-0.4*MZ-1.85*ALPS;

CLP=-0.634-0.243*MZ-0.7*ALPS;

CLR=-0.304+2.33*ALPS-5.2*MZ*ALPS;

CLKS=-0.015-0.04*MZ;

CLTS=0.165-0.33*ALPS-0.091*MZ+0.0944*MZ*ALPS;

CL=CLBE*BES+CLP*PS*HSV+CLR*RS*HSV+CLTS*TS+CLKS*KS;

 

// ***** CN ( по нашему my ) *****

 

CNBE=-0.16-0.35*MZ+(0.93-1.74*MZ)*ALPS;

CNP=0.146+0.06*MZ+2.55*ALPS;

CNR=-0.226-0.48*MZ-0.36*ALPS-1.32*pow(ALPS,2)+3.48*MZ*pow(ALPS,2)+XSP25*CNBE;

CNKS=0.027-0.0094*MZ-0.059*ALPS;

CNTS=0.179+0.0944*MZ;

CN=CNBE*BES+CNP*PS*HSV+CNR*RS*HSV+CNKS*KS+CNTS*TS-XSP25*(CZ);

 

// запись на выход

 

cXS=CW; cYS=CA; cZS=CZ;

cLS=CL; cNS=CN; cMS=CM;

}

//-------------------------------------------------------

OBEKT

// МОДЕЛЬ ПРОСТРАНСТВЕННОГО ДВИЖЕНИЯ ОБЪЕКТА

 

//*******************************************************

 

// ВХОДНЫЕ ПАРАМЕТРЫ МОДЕЛИ :

 

// 1. КОНСТРУКТИВНЫЕ ПАРАМЕТРЫ :

 

// QT,

// S,

// M0,

// RF,

// IX,

// IY,

// IZ,

// IXY,

// LMU,

// HS,

// XSP,

// LAM

// F0.

 

// 2. ВЕКТОР СОСТОЯНИЯ МОДЕЛИ ОБЪЕКТА X[] :

 

// X[0] = V

// X[1] = ALP

// X[2] = BE

// X[3] = P

// X[4] = R

// X[5] = Q

// X[6] = TETA

// X[7] = GAMMA

// X[8] = PSI (RADIANS !)

// X[9] = SXG

// X[10] = H

// X[11] = SZG

// X[12] = M

// X[13] = TETAA

// X[14] = FIA

// X[15] = FI

// X[16] = LAMB

// X[17] = KS

// X[18] = TS

 

// 3. КОЭФФИЦИЕНТЫ А/Д СИЛ И МОМЕНТОВ :

 

// cXS,cYS,cZS, cLS,cNS,cMS

 

// 4. А/Д СИЛ И МОМЕНТЫ :

 

// XS,YS,ZS, LS,NS,MS

 

// 5. УПРАВЛЯЮЩИИЕ ВОЗДЕЙСТВИЯ С БЛОКА УПРАВЛЕНИЯ :

// DRGR,

// ETA,

// KS,

// TS.

 

// 6. ПАРАМЕТРЫ СОСТОЯНИЯ ОКРУЖАЮЩЕЙ СРЕДЫ :

 

// W[], DW[],

// RH

 

// ВЫХОДНЫЕ ПАРАМЕТРЫ МОДЕЛИ :

 

// 1. ВЕКТОР ПРИРАЩЕНИЙ КОМПОНЕНТ ВЕКТОРА СОСТОЯНИЯ МОДЕЛИ : DX[]

 

//*******************************************************

 

#include "math.h"

#include "..\\Include\\extpar.h"

 

//-------------------------------------------------------

 

void OBEKT(

 

// выходные параметры (глобальные)

// в вектор dx[] заносятся рассчитанные приращения в текущий момент времени

)

{

 

// локальные идентификаторы

 

float V, // текущая скорость полета

ALP, // текущий угол атаки

BE, // текущий угол скольжения

P, R, Q, // текущие проекции вектора угл.скорости ЛА на оси связанной СК

TET, // текущий угол тангажа

PH, // текущий угол крена

PSI, // текущий угол рыскания

H, // текущая высота полета

M, // текущая массаЛА

SINAL, SINBE, SINTE, SINPH, SINPSI, // вспомогательные

COSAL, COSBE, COSTE, COSPH, COSPSI, // вспомогательные

DD, EE, FF, GG, HH, // вспомогательные

DH, // приращение высоты ЛА

DM, // приращение массы ЛА

PS, RS, QS, // текущие проекции вектора угл.скорости ЛА на оси полусвязанной СК

VP, // путевая скорость (без ветра)

PSIL, TETAL, // углы рыскания и тангажа (без ветра)

VW, // воздушная скорость (с ветром)

PSIW, TETAW, // углы рыскания и тангажа (с ветром)

SINTEL, SINPSL, SINTEW, SINPSW,SINBEL, SINBEW, SINALL, SINALW, // вспомогательные

COSTEL, COSPSL, COSTEW, COSPSW, // вспомогательные

BES, // текущий угол скольжения в полусвязанной СК

ALPS, // текущий угол атаки в полусвязанной СК

VS, // текущая скорость в полусвязанной СК

// RH0, // плотность воздуха

QQ, // скоростной напор

QQF, QQM, QQS, // вспомогательные

FX, FY, FZ, // проекции вектора силы тяги на оси связанной СК

FSX, FSY, FSZ, // сумма сил тяги и веса в проекцияхна оси полусвязанной СК

FAX, FAY, FAZ, // результирующая сумма сил в проекцих на оси скоростной СК

DSXg, DSZg,DV, DTETAA, DFIA, D1ALP, DBE,DP, DR, DQ, DPSI, DTE,DPH,DFI,DLAMB, // приращения компонент вектора состояния

LL, NN, MM, LT, NT, MT, //

XXX, ZZZ, DDD, // вспомогательные

// Wx, Wy, Wz, // текущие проекции вектора скорости ветра на оси связанной СК

MG, // вспомогательные

TETAA,FIA, // скоростные углы наклона траектории и пути

FI, // географическая широта местоположения ЛА

axa,aya,aza,

// ax1,ay1,az1,

nx1,ny1,nz1;

 

// прототипы используемых функций

 

void AEROFLU(float,float,float,float,float,float);

 

// основной раздел

 

V=X[0];

ALP=X[1];

BE=X[2];

P=X[3];

R=X[4];

Q=X[5];

TET=X[6];

PH=X[7];

PSI=X[8];

H=X[10];

M=X[12];

MG=M*G;

TETAA=X[13];

FIA=X[14];

FI=X[15];

 

// тригонометрия основных углов

 

SINAL=sin(ALP); SINBE=sin(BE); SINTE=sin(TET); SINPH=sin(PH); SINPSI=sin(PSI);

COSAL=cos(ALP); COSBE=cos(BE); COSTE=cos(TET); COSPH=cos(PH); COSPSI=cos(PSI);

 

DD=COSAL*COSBE*COSTE;

EE=COSPH*SINPSI+SINPH*SINTE*COSPSI;

FF=SINPH*SINPSI-COSPH*SINTE*COSPSI;

GG=COSPH*COSPSI-SINPH*SINTE*SINPSI;

HH=SINPH*COSPSI+COSPH*SINTE*SINPSI;

 

// приращения координат ЛА в нормальной земной СК (по Бадеру)

 

DSXg=V*(DD*COSPSI+EE*SINBE-FF*SINAL*COSBE);

DSZg=V*(-DD*SINPSI+GG*SINBE-HH*SINAL*COSBE);

DH=V*(COSAL*COSBE*SINTE-SINBE*SINPH*COSTE-SINAL*COSBE*COSTE*COSPH);

 

// проекции вектора угл.скорости ЛА на оси полусвязанной СК

 

PS=P*COSAL-R*SINAL;

RS=P*SINAL+R*COSAL;

QS=Q;

 

// учет воздействия ветра

 

VP=sqrt(pow(DSXg,2)+pow(DSZg,2));

PSIL=atan(-DSZg/DSXg);

TETAL=atan(DH/VP);

VW=sqrt(pow((DSXg-W[0]),2)+pow((DSZg-W[2]),2));

PSIW=atan(-(DSZg-W[2])/(DSXg-W[0]));

TETAW=atan((DH-W[1])/VW);

 

// расчет углов атаки и скольжения в полусвязанной СК

 

SINPSL=sin(PSIL); SINTEL=sin(TETAL); SINPSW=sin(PSIW); SINTEW=sin(TETAW);

COSPSL=cos(PSIL); COSTEL=cos(TETAL); COSPSW=cos(PSIW); COSTEW=cos(TETAW);

 

SINBEL=EE*COSTEL*COSPSL-GG*COSTEL*SINPSL-SINTEL*COSTE*SINPH;

SINBEW=EE*COSTEW*COSPSW-GG*COSTEW*SINPSW-SINTEW*COSTE*SINPH;

BES=BE-asin(SINBEL)+asin(SINBEW);

 

SINALL=(-FF*COSTEL*COSPSL+HH*COSTEL*COSPSL-SINTEL*COSTE*COSPH)/COSBE;

SINALW=(-FF*COSTEW*COSPSW+HH*COSTEW*COSPSW-SINTEW*COSTE*COSPH)/COSBE;

ALPS=ALP-asin(SINALL)+asin(SINALW);

 

// расчет скорости в полусвязанной СК

 

VS=V-sqrt(pow(DSXg,2)+pow(DH,2)+pow(DSZg,2))+sqrt(pow((DSXg-W[0]),2)+pow((DH-W[1]),2)+pow((DSZg-W[2]),2));

 

// проекции вектора силы тяги на оси связанной СК

 

FX=DRGR*F0*pow((RH/1.225),0.75); FY=0.0; FZ=0.0;

 

// расчет аэродинамики ЛА

 

AEROFLU(VS,ALPS,BES,PS,RS,QS);

 

QQ=0.5*RH*pow(VS,2);

QQF=QQ*S; QQM=QQF*LMU; QQS=QQF*HS;

 

// аэродинамические силы и моменты (полусвязанная СК)

 

XS=cXS*QQF; YS=cYS*QQF; ZS=cZS*QQF;

LS=cLS*QQS; NS=cNS*QQS; MS=cMS*QQM;

 

// Сила тяги и веса в полусвязанной СК

 

FSX=(FX-MG*SINTE)*COSAL-(FY-MG*COSTE*COSPH)*SINAL;

FSY=(FX-MG*SINTE)*SINAL+(FY-MG*COSTE*COSPH)*COSAL;

FSZ=FZ+MG*COSTE*SINPH;

 

// проекции результирующей силы на оси скоростной СК

 

FAX=(FSX+XS)*COSBE+(FSZ+ZS)*SINBE;

FAY=FSY+YS;

FAZ=-(FSX+XS)*SINBE+(FSZ+ZS)*COSBE;

 

// проекции ускорения ЛА на оси скоростной СК

 

axa=FAX/M;

aya=FAY/M;

aza=FAZ/M;

 

// проекции ускорения ЛА на оси связанной СК

 

ax1=axa*COSAL*COSBE+aya*SINAL-aza*COSAL*SINBE;

ay1=-axa*SINAL*COSBE+aya*COSAL+aza*SINAL*SINBE;

az1=axa*SINBE+aza*COSBE;

 

// расчет приращений компонент вектора состояния

 

DV=FAX/M;

DTETAA=FAY/(M*V);

DFIA=-FAZ/(M*V);

D1ALP=(-DTETAA-PS*SINBE+QS*COSBE)/COSBE;

DBE=RS-DFIA;

 

// перерасчет аэродинамических моментов в связанную СК

 

LL=LS*COSAL+NS*SINAL; NN=-LS*SINAL+NS*COSAL; MM=MS;

 

// проекции момента дебаланса тяги двигателей на оси связанной СК

 

LT=0.0; NT=0.0; MT=FX*RF;

XXX=(IY-IZ)*Q*R-IXY*P*Q+LL+LT;

ZZZ=(IZ-IX)*P*Q+IXY*Q*R+NN+NT;

DDD=IX*IY-pow(IXY,2);

 

// расчет приращений компонент вектора состояния

 

DP=(XXX*IY+ZZZ*IXY)/DDD;

DR=(ZZZ*IX+XXX*IXY)/DDD;

DQ=((IX-IY)*R*P+IXY*(pow(P,2)-pow(R,2))+MM+MT)/IZ;

DPSI=(R*COSPH-Q*SINPH)/COSTE;

DTE=Q*COSPH+R*SINPH;

DPH=P-DPSI*SINTE;

DFI=V*cos(TETAA)*sin(FIA)/(a+H); // FI

DLAMB=V*cos(TETAA)*cos(FIA)/((a+H)*cos(FI)); // lamb

DM=-QT; // приращение массы ЛА

 

// записываем вектор приращений (в текущий момент времени) на интегрирование

 

DX[0]=DV;

DX[1]=D1ALP;

DX[2]=DBE;

DX[3]=DP;

DX[4]=DR;

DX[5]=DQ;

DX[6]=DTE;

DX[7]=DPH;

DX[8]=DPSI;

DX[9]=DSXg;

DX[10]=DH;

DX[11]=DSZg;

DX[12]=DM;

DX[13]=DTETAA;

DX[14]=DFIA;

DX[15]=DFI;

DX[16]=DLAMB;

}

 

AFUVRO

// МОДЕЛЬ АВТОПИЛОТА (ЗАКОНЫ СТАБИЛИЗАЦИИ)

 

// АЛГОРИТМ ФОРМИРОВАНИЯ УПРАВЛЯЮЩИХ ВОЗДЕЙСТВИЙ НА РУЛЕВЫЕ ОРГАНЫ

//-------------------------------------------------------

#include "..\\Include\\extpar.h"

#include "math.h"

 

//-------------------------------------------------------

 

void AFUVRO()

{

// локальные идентификаторы

 

float drgr1,eta1,eta2,SKL,k,t, // вспомогательные

V, // текущая скорость полета

Teta, // текущий угол тангажа

ALP, // текущий угол атаки

H, // текущая высота полета

Dks,Dts, // приращения отклонений рулей направления и элеронов

P, R, Q, // текущие проекции угл.скорости ЛА на оси связанной СК

BE , // текущий угол скольжения

PSI, // текущий угол рыскания

PH; // текущий угол крена

 

// основной раздел

// выбор способа управления - по объекту (идеал) или по данным с ИНС (реал)

 

if (PRNUPRBINS) // управление по показаниям БИНС

{

V=BV;

Teta=BTETA;

ALP=X[1];

BE=X[2];

H=X[10];

Q=OMEGAIZM[2];

P=OMEGAIZM[0];

R=OMEGAIZM[1];

PSI=BPSI;

PH=BGAMMA;

}

else // управление по идеальному вектору состояния

{

V=X[0];

Teta=X[6];

ALP=X[1];

BE=X[2];

H=X[10];

Q=X[5];

P=X[3];

R=X[4];

PSI=X[8];

PH=X[7];

}

drgr1=-6.0*(V-VTR);

 

// степень дросселирования двигателей

 

DRGR=(DRGR0+drgr1)*pow((1.225/RH),0.75);

 

// руль высоты

 

eta1=0.0;

eta2=eta1+(0.7*(Q-QTR)+3.5*(Teta-TETA0)+4.0*(ALP-ALP0)+0.02*(H-HTR));

 

// ограничения по отклонению 10 градусов

 

if (eta2>(10.0/GR)) eta2=10.0/GR;

if (eta2<(-10.0/GR)) eta2=-10.0/GR;

ETA=ETA0+eta2;

 

// ограничения по отклонению 16 и 25 градусов

 

if (ETA>(16.0/GR)) ETA=16.0/GR;

if (ETA<(-25.0/GR)) ETA=-25.0/GR;

 

// рули направления и элероны

 

SKL=1.0;

if (PRNP) SKL=0.0;

if (!PRNUPR) { Dks=0.0; Dts=0.0; } // откл САУ

else

{ // подкл САУ

 

k=X[17]; // ks

t=X[18]; // ts

Dks=-(1/50.0)*k-pow(5.0,2)*(0.741*P-2.63*(R-RTR)+1.718*(PH-GAMMATR)+3.92*(BE-BETR)-4.3*SKL*(PSI-(PSIGTR/GR))+0.384*k+0.18*t);

Dts=-(1/40.0)*t-pow(3.7,2)*(-0.301*P-2.82*(R-RTR)-0.3*(PH-GAMMATR)-3.93*(BE-BETR)+3.79*SKL*(PSI-(PSIGTR/GR))-0.384*k+0.343*t);

}

 

DX[17]=Dks;

DX[18]=Dts;

 

KS=KS0-X[17];

TS=TS0-X[18];

 

}

//-------------------------------------------------------

 

AONP

 

// АЛГОРИТМ ОПРЕДЕЛЕНИЯ НАВИГАЦИОННЫХ ПАРАМЕТРОВ

//-------------------------------------------------------

#include "..\\Include\\extpar.h"

#include "..\\Include\\common.h"

#include "math.h"

 

//-------------------------------------------------------

void AONP()

{

float uy,uz,f1,f2,

OMEGAX,OMEGAY,OMEGAZ;

 

float OMEGAG[3][3],OMEGAS[3][3];

float L1[3][3],L2[3][3];

float w[3][3], wx,wy,wz;

float Nx,Ny,Nz;

int i,j;

float H=X[10];

 

BVZ=DX[10];

 

BV=sqrt(pow(BVE,2)+pow(BVN,2));//+pow(BVZ,2));

 

BTETA=asin(C[2][0]);

BGAMMA=asin(-C[2][2]/sqrt(1-pow(C[2][0],2)));

BPSI=atan(C[1][0]/fabs(C[0][0]));

 

uy=0.0;

uz=0.0;

 

f1=0.0;

f2=0.0;

 

OMEGAX=-BVN*f1;

OMEGAY=uy+BVE*f2;

OMEGAZ=uz+tan(BFI)*BVE*f2;

 

OMEGAG[0][0]=0.0; OMEGAG[0][1]=-OMEGAZ; OMEGAG[0][2]=OMEGAY;

 

OMEGAG[1][0]=OMEGAZ; OMEGAG[1][1]=0.0; OMEGAG[1][2]=-OMEGAX;

 

OMEGAG[2][0]=-OMEGAY; OMEGAG[2][1]=OMEGAX; OMEGAG[2][2]=0.0;

 

OMEGAS[0][0]=0.0; OMEGAS[0][1]=-OMEGAIZM[2]; OMEGAS[0][2]=OMEGAIZM[1];

 

OMEGAS[1][0]=OMEGAIZM[2]; OMEGAS[1][1]=0.0; OMEGAS[1][2]=-OMEGAIZM[0];

 

OMEGAS[2][0]=-OMEGAIZM[1]; OMEGAS[2][1]=OMEGAIZM[0]; OMEGAS[2][2]=0.0;

 

for (i=0;i<3;i++) // умножаем матрицу на -1

{

for (j=0;j<3;j++) { OMEGAG[i][j]=-OMEGAG[i][j]; }

 

}

 

MatrixMult(OMEGAG,C,L1);

MatrixMult(C,OMEGAS,L2);

 

MatrixSum(L1,L2,DC);

 

DBFI=BVN*f1;

DBLAMB=sin(BFI)*BVE*f2;

 

wx=-DBFI;

wy=DBLAMB*cos(BFI);

wz=DBLAMB*sin(BFI);

 

w[0][0]=0.0; w[0][1]=-wz; w[0][2]=wy;

 

w[1][0]=wz; w[1][1]=0.0; w[1][2]=-wx;

 

w[2][0]=-wy; w[2][1]=wx; w[2][2]=0.0;

 

MatrixMult(A,w,DA);

 

Nx=AIZM[0]*C[0][0]+AIZM[1]*C[0][1]+AIZM[2]*C[0][2];

Ny=AIZM[0]*C[1][0]+AIZM[1]*C[1][1]+AIZM[2]*C[1][2];

Nz=AIZM[0]*C[2][0]+AIZM[1]*C[2][1]+AIZM[2]*C[2][2];

 

DBVE=BVN*(wz+2*uz)-BVZ*(wy+2*uy)+Nx;

DBVN=-BVE*(wz+2*uz)+BVZ*wx+Ny+G*((H/a)*(e2-5*0.0346775)+0.0346775*e2*pow(A[2][2],2))*A[2][1]*A[2][2];

 

DBXG=BVE;

DBZG=-BVN;

}

//-------------------------------------------------------

 

 

ARVP

 

// АЛГОРИТМ РАСЧЕТА ТРЕБУЕМЫХ (ВЫДЕРЖИВАЕМЫХ ЗАКОНОМ

 

// УПРАВЛЕНИЯ ) ПАРАМЕТРОВ

//-------------------------------------------------------

#include "..\\Include\\extpar.h"

#include "math.h"

 

//-------------------------------------------------------

void ARVP()

{

// локальные идентификаторы

 

int SIGNBETATR;

int SIGNGAMMATR;

float K;

 

// основной раздел

 

if (PRNP)

{

if (( PSIG[NORT-1] - PSIG[NORT-2]) >0) SIGNBETATR = SIGNGAMMATR = -1; // поворот налево

else SIGNBETATR = SIGNGAMMATR = 1; // поворот направо

 

K=fabs((PSIG[NORT-1] - PSIG[NORT-2])/90);

 

BETR=SIGNBETATR*K*0.02;

GAMMATR=SIGNGAMMATR*K*0.2;

}

else

{

BETR=0.0;

GAMMATR=0.0;

}

 

PSIGTR=PSIG[NORT-1];

 

DPSITEK=(PSIGTR-PSIG[NORT-2])/GR;

 

SXTR=SX[NORT-1];

 

PTR=0.0;

 

RTR=(G/VTR)*tan(GAMMATR)*cos(GAMMATR);

 

QTR=(G/VTR)*tan(GAMMATR)*sin(GAMMATR);

}

//-------------------------------------------------------

 

 

SREDA

 

// МОДЕЛЬ ВОЗДЕЙСТВИЯ ВЕТРА

 

//-------------------------------------------------------

#include "..\\Include\\extpar.h"

#include "stdlib.h"

#include "math.h"

//-------------------------------------------------------

 

void SREDA()

{

// локальные идентификаторы

 

const float KW=8.6, b=1.26, a=-0.747;

float n;

 

// прототипы используемых функций

 

float Generator(void);

 

// основной раздел

 

// МОДЕЛЬ ВЕТРА

 

if (PRNWIND)

{

n=Generator();

DW[0]=a*W[0]+b*n*KW;

 

n=Generator();

DW[1]=a*W[1]+b*n*KW;

 

n=Generator();

DW[2]=a*W[2]+b*n*KW;

}

 

// ПЛОТНОСТЬ ВОЗДУХА НА ТЕКУЩЕЙ ВЫСОТЕ

 

RH=ro0*pow((1-X[10]/44332.0),4.255);

}

//-------------------------------------------------------

 

// процедура - генератор случайных норм. распр. чисел в интервале [-1;1]

 

float Generator(void)

{

// основной раздел

 

return (rand()*6.0/100000.0)-1;

 

}

//-------------------------------------------------------

 

 

TLM

// АЛГОРИТМЫ ЗАПИСИ ПАРАМЕТРОВ СОСТОЯНИЯ В ФАЙЛЫ ТЕЛЕМЕТРИИ

 

//-------------------------------------------------------

#include "stdio.h"

#include "..\\Include\\Extpar.h"

//-------------------------------------------------------

 





Читайте также:
Расчет длины развертки детали: Рассмотрим ситуацию, которая нередко возникает на...
Экономика как подсистема общества: Может ли общество развиваться без экономики? Как побороть бедность и добиться...
Группы красителей для волос: В индустрии красоты колористами все красители для волос принято разделять на четыре группы...
Тест Тулуз-Пьерон (корректурная проба): получение информации о более общих характеристиках работоспособности, таких как...

Рекомендуемые страницы:


Поиск по сайту

©2015-2020 poisk-ru.ru
Все права принадлежать их авторам. Данный сайт не претендует на авторства, а предоставляет бесплатное использование.
Дата создания страницы: 2019-05-16 Нарушение авторских прав и Нарушение персональных данных


Поиск по сайту:

Обратная связь
0.516 с.