global.h

来自「this the code of bootstation」· C头文件 代码 · 共 197 行

#ifndef _global_h_
#define _global_h_

#pragma warning(disable: 4305)


#include <math.h>
#include <fstream>
#include <iostream>
#include <string>
using namespace std;

extern float Var1[5];
extern float Var2[5];
extern float Var3[5];

//allgemeine globale Parameter
//-----------------------------------------------------------------------------------------
#define Pi 			3.141
#define true 		1
#define false 		0
#define bit			bool
#define n_max		1000
//#define SchrittProTakt 5

//Timer Grundwert (maxWert - Schritte f黵 gew黱schten Taskwechsel (30ms) || (40ms))
//Taktteiler: 8 == 800ns pro Z鋒lwert (Task 1..4 -> 30ms; Task 5..6 -> 40ms)
#define Taskzeit_30ms  0.03
#define Taskzeit_40ms  0.04
#define Timer_Wert_30 0xFFFF-(Taskzeit_30ms/0.0000008)	//Basiszeit Timer 800ns
#define Timer_Wert_40 0xFFFF-(Taskzeit_40ms/0.0000008)

#define K_x 0		//Position x Koordinate im Array
#define K_y 1		//Position y Koordinate im Array
#define BufferLenght 16

#define min_Seite_dreh			2		//minimaler Seitenabstand bei Drehungen
//Standrard-Flug
#define Fwd				0
#define Turn 			1
#define Rev				2
//Koordinaten-Flug
#define Stop			3
#define Koor_r_Turn 	4
#define Koor_l_Turn		5
#define Koor_Fwd_Rev	6
//ReturnToOrigin
#define ToOrigin		7
#define FollowObject	8

//Modellparameter

#define AufrufeProZyklus	 6		//6 Aufrufe des Verhaltensmodel pro Zyklus (6x case)
#define Tau					 0.1	//Zeitkoinstante Motor und Servo Tr鋑heit
#define FmaxMotor			 0.85	//maximale Motorkraft in N
#define Gewicht				 1		//Gewicht in kg
#define Dreh_Radius			 1.8		//Radius der Drehachse
#define Umfang				 9.42 	//Umfang des Luftschiffes
#define Luft_x				 0.6	//Luftwiderstand x Richtung
#define Luft_y				 0.8	//Luftwiderstand y Richtung
#define Luft_z				 0.8	//Luftwiderstand z Richtung
#define Dreh_Traegheit		 0.2	//Drehtr鋑heit 
#define Abtrieb				 0.05	//Abtriebsfaktor
#define Gravitation			 9.81	//Gravitationskonstante

//Spaltenposition in Datendatei, die vom Zeppelin gesendet wird
#define s_Zeit		0
#define s_vorne		1
#define s_hinten	2
#define s_oben		3
#define s_unten		4
#define s_rechts	5
#define s_links		6
#define s_Gyro		7

#define BLOB_LENGTH 5
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************
//main
extern const float	Prozesszyklus;	//Prozesszykus [s]
extern int	 		Zeit;			//globale Zeit seit Systemstart (wird je Prozesszyklus um Prozesszyklus erh鰄t)
extern int			Regel_Sperrzeit;

extern float Messung_v[5];				//Messwert vorne
extern float Messung_h[5];				//Messwert hinten
extern float Messung_o[5];				//Messwert oben
extern float Messung_u[5];				//Messwert unten
extern float Messung_r[5];				//Messwert rechts
extern float Messung_l[5];				//Messwert links
extern float Messung_g[5];				//Messwert Gyroskop
extern float Messung_k[5];				//Messwert Kompass
extern float Richtung_ist[5];

extern const float	max_Sensor_v;	//maximaler Sensormessbereich
extern const float	max_Sensor_h;	//maximaler Sensormessbereich
extern const float	max_Sensor_o;	//maximaler Sensormessbereich
extern const float	max_Sensor_u;	//maximaler Sensormessbereich
extern const float	max_Sensor_r;	//maximaler Sensormessbereich
extern const float	max_Sensor_l;	//maximaler Sensormessbereich
extern float 		ServoWinkel;	//auf diesen Wert wird der Servo angesteuert
extern float 		HauptMotorLeistung;	//mit diesem Wert wird der Moror angesteuert

//-----------------------------------------------------------------------------------------
extern 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);
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************
//Kalman
//Parameter
//-----------------------------------------------------------------------------------------
extern float Gain_vorne;	//Kalmangain f黵 die Ultraschallmessungen
extern float Gain_hinten;	//
extern float Gain_oben;		//
extern float Gain_unten;	//
extern float Gain_rechts;	//
extern float Gain_links;	//
extern float Q;				//Systemrauschen einfacher Kalman_Filter (Ultraschallmessung)
extern float Q_v;			//Modellvarianz Kalman_Datenfusion Geschwindigkeit
extern float P_v[1];		//Fehler-Kovarianz bei Kalman_Datenfusion Geschwindigkeit
extern float Q_Winkel;		//Modellvarianz Winkelberechnung (GyroIntegration)
extern float P_Winkel[1];	//Fehler-Kovarianz Kalman_Datenfusion Winkelkorrektur
extern float Kovar_Richtung;//Kovarianz f黵 die Messwertfusion Richtung
//-----------------------------------------------------------------------------------------

//Funktionen
//-----------------------------------------------------------------------------------------
//Einfacher Kalmanfilter (Messwertbewertung)
extern void Kalman_Filter(float Daten[5], float Messwert, float Gain, float P_min[1]);
//Kalman-Datenfusion
//躡ergabewerte: Ergebnis der Fusion[0], Modelldynamik, Messwert, 
//Messwert g黮tig (G),  Fehlervarianz, Rauschen Modell
extern void Kalman_Datenfusion(float Ergebnis[5], float ModellDynamik, float Messwert, 
						bit G, float P[1], float Q_Modell);
//Datenfusion von Sensormesswerten und Sensormessdynamik
//鋒nlich wie Datenfusion
//躡ergabewerte: Ergebnis der Fusion [0], Messwert, Messdynamik, 
//Rauschen Messwerte, Rauschen Messdynamik, Fehlerkovarianz
//Ergibnis muss ein 5er Feld sein
extern void Kalman_Messwertfusion(float* Ergebnis,  float Messwert, float Messdynamik, 
						float Q_Messwert, float Q_Messdynamik, float& P);

//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//***********************************************************************************
extern int FindFixBlob(char* FixBlob, char* Daten, char FoundBlobs);
extern unsigned char	CamDaten[128];			//Kamera Daten
extern unsigned char	FoundBlobs;
extern unsigned char FixBlob[4];
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************
//Regelung
//FlugRegelung --ermittelt aus den gegebenen Soll- Istwerten die Kennwerte aller Aktoren und steuert sie
extern void FlugRegelung(float HoeheSoll, float HoeheIst, float FlugSoll, float FlugIst, 
				  float RichtungSoll, float RichtungIst, int Drift);
extern float RichtungsRegler(float Sollwert, float Istwert);
extern float HoehenRegler(float Sollwert, float Istwert);	
extern float FlugRegler(float Sollwert, float Istwert);
extern void ServoRegelung(float HoehenRegler, float FlugRegler);	//Servo- und Motorregelung;

//PID Regler (I_alt ist Zeiger auf letzten I-Anteil der berechnung und wird aktuallisiert)
//e ist Zeiger auf Array[2], mit [0]->e und [1]-> e aus letztem Aufruf (wird hier verschoben)
//Es gilt der Prozesszyklus als Aufrufzyklus!
extern float PIDRegler(float P, float I, float D, float* I_alt, float* e);

extern float Richtung_soll;
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************
//RegelungVorstufe
extern float SollwertHoehe(float MaxHoehe, float* Messwerte, short Rampe);

//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************
//Datenfilter
extern float 		MittelwertGefiltert_v[5];
extern float 		MittelwertGefiltert_h[5];	//Mittelwert hinten
extern float 		MittelwertGefiltert_o[5];	//Mittelwert oben
extern float 		MittelwertGefiltert_u[5];	//Mittelwert unten
extern float 		MittelwertGefiltert_r[5];	//Mittelwert rechts
extern float 		MittelwertGefiltert_l[5];	//Mittelwert links

//speichert den mittleren Wert aus den 黚ergebenen Daten im MittelwertGefiltert Array (5er)
extern void MittelwertFilter(float* Daten, float* MittelwertGefiltert);

//schiebt die Daten eines i Arrays um eins weiter ([0]->[1] etc.)
extern void DatenShift(float* Daten, int i);
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//*****************************************************************************************

#endif