main.cpp

来自「this the code of bootstation」· C++ 代码 · 共 98 行

#include "stdafx.h"

#include "global.h"
#include "main.h"
//#include "..\Bodenstation2Dlg.h"

void Task5_Aufruf(int* Time, float* vorne, float* hinten, float* oben,
				  float* unten, float* rechts, float* links, float* kompass, float* beschl, 
				  float* gyro, unsigned char* CamDat, int FoundBlo)
{
//Initialisierungen
	Zeit = *Time;
	memcpy(Messung_v, vorne, sizeof(Messung_v));
	memcpy(Messung_h, hinten, sizeof(Messung_h));
	memcpy(Messung_r, rechts, sizeof(Messung_r));
	memcpy(Messung_l, links, sizeof(Messung_l));
	memcpy(Messung_o, oben, sizeof(Messung_o));
	memcpy(Messung_u, unten, sizeof(Messung_u));
	memcpy(Messung_g, gyro, sizeof(Messung_g));
	memcpy(Messung_k, kompass, sizeof(Messung_k));
	memcpy(CamDaten, CamDat, sizeof(CamDaten));
	FoundBlobs = FoundBlo;
	
	//-----------------

	MittelwertFilter(Messung_v, MittelwertGefiltert_v);		//Messwert filtern
	MittelwertFilter(Messung_h, MittelwertGefiltert_h);		//Messwert filtern
	MittelwertFilter(Messung_o, MittelwertGefiltert_o);		//Messwert filtern
	MittelwertFilter(Messung_u, MittelwertGefiltert_u);		//Messwert filtern
	MittelwertFilter(Messung_r, MittelwertGefiltert_r);		//Messwert filtern
	MittelwertFilter(Messung_l, MittelwertGefiltert_l);		//Messwert filtern
	
	
	Var3[4] = 0.0;
//	Kalman_Messwertfusion(Var2, Messung_k[4], Messung_g[4]/25, 10, 1, Var3[0]);
	Var3[3] = Var3[4];
//	Kalman_Messwertfusion(Var2, Messung_k[3], Messung_g[3]/25, 10, 1, Var3[3]);
	Var3[2] = Var3[3];
//	Kalman_Messwertfusion(Var2, Messung_k[2], Messung_g[2]/25, 10, 1, Var3[2]);
	Var3[1] = Var3[2];
//	Kalman_Messwertfusion(Var2, Messung_k[1], Messung_g[1]/25, 10, 1, Var3[1]);
	Var3[0] = Var3[1];
//	Kalman_Messwertfusion(Var2, Messung_k[0], Messung_g[0]/25, 10, 1, Var3[0]);	
	Kovar_Richtung = Var3[0];


	if (Sonderfall())
	{

	}
	else
	{
		if (Regel_Sperrzeit > 0){ Regel_Sperrzeit--; Richtung_ist[0] =0; }
						else
						{

	 						short HoeheRampe = 0;
//							char FixBlob[4];
							float CamMassPoint;

							if (Zeit > 900)
								HoeheRampe = -7;
					
							CamMassPoint = (CamDaten[1] - 72) * 0.0057 * MittelwertGefiltert_u[0];
							
							if (CamDaten[1] == 0)
							{
	
							}
							else
							{
	
								Soll_Drift[1] = Soll_Drift[0];
								Soll_Drift[0] = (CamDaten[0] - 44)*0.0057 * 40;
							}

//							HoeheSoll = SollwertHoehe(0.9, MittelwertGefiltert_u, HoeheRampe);						
							FlugRegelung(0.9, MittelwertGefiltert_u[0],
										0, 0,
										Richtung_soll, Messung_k[0], Soll_Drift[0]);
						}
			Var1[0] = Soll_Drift[0] + 50;
			Var1[1] = Var1[0];
			Var1[2] = Var1[0];
			Var1[3] = Var1[0];
			Var1[4] = Var1[0];

		
	}
}


bool Sonderfall()
{
	return false;
}