Kras LR2.cpp

#include <fstream>
#include <conio.h>
#include <stdio.h>
#define _USE_MATH_DEFINES 1;
#include <math.h>

#pragma warning(disable : 4996)

// Объявление глобальных переменных
int N, dN, Tv;	 // Параметры циклов
double C[10], D[10];// Матрицы ориентации и перехода от связанной СК к инерциальной
double k, ta, kr;	 // Измеряемые углы курса,тангажа, крена
double wx, wy, wz, nx, ny, nz;	// Измеряемые угловые скорости и ускорения
double ks, tas, krs;		// Накопления измерений углов курса,тангажа, крена
double wxs, wys, wzs, nxs, nys, nzs;	// Накопления измерений угловых скоростей и ускорений
double ksr, tasr, krsr;		// Усредненные измерения углов курса,тангажа, крена
double p11, p12, p13, q11, q12, q13;	// Координаты векторов в первой системе
double p21, p22, p23, q21, q22, q23;	// Координаты векторов во второй системе
double e111, e112, e113, e121, e122, e123, e131, e132, e133; // Орты в первой системе
double e211, e212, e213, e221, e222, e223, e231, e232, e233; // Орты во второй системе
double m11, m12, m21, m22; // Модули векторов
double gr, F, H, W, Wv, Wg, g0, g;// Физические константы и параметры расположения БИНС
double b1, b2, b3;	// Координаты векторных произведений
FILE* Fk, * Ft, *Ft2;	// Файловые потоки


int main(void)
{
	gr = 180. / M_PI;		// Количество градусов в радиане
	F = 55.8 / gr;			// Широта (рад)
	H = 200;				// Высота над уровнем моря, м
	W = 7.2921158553E-5;	// Угловая скорость вращения Земли (рад/сек)
	Wv = W * sin(F);		// Вертикальная проекция угловой скорости вращения Земли
	Wg = W * cos(F);		// Горизонтальная проекция угловой скорости вращения Земли
	g0 = 9.780327;			// Экваториальное ускорение силы тяжести
	g = g0 * (1 + 0.0053024 * pow(sin(F), 2) - 0.0000058 * pow(sin(2 * F), 2)) - 3.686e-6 * H;	// Ускорение силы тяжести
	Tv = 1.;				// Начальное значение времени выставки и шаг его изменения
	dN = Tv * 50;			// Начальное значение кол-ва измерений для выставки и шаг его изменения
	p21 = 0.; p22 = g; p23 = 0;// Координаты вектора ускорения силы тяжести в сопровождающем трёхграннике
	q21 = Wg; q22 = Wv; q23 = 0;// Координаты вектора угловой скорости Земли в сопровождающем трёхграннике

		//Открытие файла информации с БИМС-Т
	if ((Fk = fopen("Data.res", "r")) == NULL) {
		perror("Ошибка открытия файла DATA.RES на чтение");
		_getch;
		return 0;
	}

	if ((Ft = fopen("out_bins.txt", "w")) == NULL) {
		perror("Ошибка открытия файл out_bins.txt на запись");
		_getch;
		return 0;
	}

	if ((Ft2 = fopen("out_sr.txt", "w")) == NULL) {
		perror("Ошибка открытия файл out_sr.txt на запись");
		_getch;
		return 0;
	}

	printf("Начало обработки.\n");

	fprintf(Ft2, "iter Курс Тангаж Крен Wx Wy Wz Nx Ny Nz\n");
// Цикл варьирования количества наблюдений для выставки 
// (предел цикла не более, чем количество строк в исходном файле с данными)
	N = dN;
	while (N <= 4621) {
		// Сброс накоплений
		ks = tas = krs = wxs = wys = wzs = nxs = nys = nzs = 0;
		// Считывание данных из файла и вычисление средних значений ускорения, угловой скорости и углов ориентации
		for (int i = 1; i <= N; i++) {
			fscanf(Fk, "%*8s%lf%lf%lf%lf%lf%lf%lf%lf%lf", &k, &ta, &kr, &wx, &wy, &wz, &nx, &ny, &nz);
			// Изменение ориентации вертикальной оси чувствительности ДУС
			wy = -wy;
			// Накопление измерений
			wxs += wx; wys += wy; wzs += wz;
			nxs += nx; nys += ny; nzs += nz;
			ks += k; tas += ta; krs += kr;
		}

		fprintf(Ft2, "%10.0f %15.8f% 15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f\n", 
					  N / 50., ks / N, tas / N, krs / N, wxs / N, wys / N, wzs / N, nxs / N, nys / N, nzs / N);

		// Координаты вектора угловой скорости вращения Земли в приборной системе
		q11 = wxs / N; q12 = wys / N; q13 = wzs / N;
		// Координаты вектора ускорения силы тяжести в приборной системе
		p11 = g * nxs / N; p12 = g * nys / N; p13 = g * nzs / N;
		// Углы ориентации
		ksr = ks / N; tasr = tas / N; krsr = krs / N;

		//Вычисление ортов для первой системы
		m11 = sqrt(p11 * p11 + p12 * p12 + p13 * p13);
		e121 = p11 / m11; e122 = p12 / m11; e123 = p13 / m11;
		b1 = q12 * p13 - q13 * p12; b2 = q13 * p11 - q11 * p13; b3 = q11 * p12 - q12 * p11;
		m12 = sqrt(b1 * b1 + b2 * b2 + b3 * b3);
		e131 = b1 / m12; e132 = b2 / m12; e133 = b3 / m12;
		e111 = e122 * e133 - e123 * e132; e112 = e123 * e131 - e121 * e133;
		e113 = e121 * e132 - e122 * e131;

		//Вычисление ортов для второй системы;
		m21 = sqrt(p21 * p21 + p22 * p22 + p23 * p23);
		e221 = p21 / m21; e222 = p22 / m21; e223 = p23 / m21;
		b1 = q22 * p23 - q23 * p22; b2 = q23 * p21 - q21 * p23; b3 = q21 * p22 - q22 * p21;
		m22 = sqrt(b1 * b1 + b2 * b2 + b3 * b3);
		e231 = b1 / m22; e232 = b2 / m22; e233 = b3 / m22;
		e211 = e222 * e233 - e223 * e232; e212 = e223 * e231 - e221 * e233;
		e213 = e221 * e232 - e222 * e231;

		//Вычисление матрицы ориентации
		C[1] = e211 * e111 + e221 * e121 + e231 * e131;
		C[2] = e211 * e112 + e221 * e122 + e231 * e132;
		C[3] = e211 * e113 + e221 * e123 + e231 * e133;
		C[4] = e212 * e111 + e222 * e121 + e232 * e131;
		C[5] = e212 * e112 + e222 * e122 + e232 * e132;
		C[6] = e212 * e113 + e222 * e123 + e232 * e133;
		C[7] = e213 * e111 + e223 * e121 + e233 * e131;
		C[8] = e213 * e112 + e223 * e122 + e233 * e132;
		C[9] = e213 * e113 + e223 * e123 + e233 * e133;

		//Вычисление курса, тангажа, крена
		k = atan2(C[7], C[1]);
		ta = asin(C[4]);
		kr = -atan2(C[6], C[5]);

		// Вывод результатов
		printf("%10.1f%15.3f%15.3f%15.3f\n", N / 50., k * gr, ta * gr , kr * gr);
		fprintf(Ft, "%10.1f%15.3f%15.3f%15.3f\n", N / 50., k * gr, ta * gr, kr * gr);

		// Сброс указателя файлового потока в начало
		fseek(Fk, 0L, SEEK_SET);
		// Увеличение количества измерений, используемых для выставки
		N += dN;
	}

	printf("Обработка завершена.");

	fclose(Fk);
	fclose(Ft);
	fclose(Ft2);
	_getch();
}