В данном разделе приведен перечень используемых в процессе реализации моделей, оговоренных в пояснительной записке, глобальных идентификаторов.
Таблица 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"
//-------------------------------------------------------
// п