WebSVN - FlightCtrl - Diff - Rev 980 and 985 - /branches/KalmanFilter MikeW/main.c


/*
Copyright 2008, by Michael Walter
 
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/
 
/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "main.h"
#include "kafi.h"
#include "mymath.h"
#include "mm3.h"
#include "FlightControl.h"
 
/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/
 
#define sin45 -0.707106
#define cos45 0.707106
 
/*****************************************************************************
  VARIABLES
*****************************************************************************/
extern int RCQuality;
int RemoteLinkLost = 0;
int InvalidAttitude = 0;
extern unsigned long maxDistance;
unsigned char EEPromArray[E2END+1] EEMEM;
 
void TestRemote(void);
void IMU_Main(void);
void TestBattery(void);
void SendDebugData(void);
void InitPorts(void);
void GenerateDefaults(void);
void TestIC2Link(void);
void GetAirPressureOffset(void);
void DeployRescue(void);
 
extern void InitOSD(void);
extern void InitGPS(void);
extern void SendOSD(void);
extern void RemoteControl(void);  
extern void SendMotorData(void);
extern void GetMeasurements(void);
extern void AttitudeEstimation(void);
extern void PD_Regler(void);
extern void SetNeutral(void);
 
/* ****************************************************************************
Functionname:     main                      */ /*!
Description:      Hauptprogramm
 
  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
int main (void)
{
  /* Controler Init */
  InitPorts();
  Kafi_Init();
  Timer_Init();
  UART_Init();
  InitGPS();
  rc_sum_init();
  ADC_Init();
  i2c_init();
#ifdef USE_COMPASS
  MM3_Init();
#endif
  /* Enable Interrupts */
  sei();
 
  /* Generate Default Values */
  GenerateDefaults ();
  /* Get Air Pressure Offset */
  GetAirPressureOffset();
  /* Determine Gyro / ACC Offsets */
  SetNeutral();
 
  DebugIn.Analog[1] = 1000;
  DebugIn.Digital[0] = 0x55;
 
#ifdef USE_COMPASS
  /* Calibrate the MM3 Compass? */
  if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) &&
    (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) &&
    (MotorenEin == 0))
  {
    printf("\n\rCalibrating Compass");
    MM3_Calibrate();
  }
#endif 
 
#ifdef USE_OSD
  /* Init the Bob-4 OnScreen Display */
  InitOSD();
#endif
 
  /* Start the main Task */
  IMU_Main();
  return (1);
}
 
 
/* ****************************************************************************
  Functionname:     IMU_Main                      */ /*!
  Description:      
 
  @param[in]        
 
  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void IMU_Main()
{
  ROT_ON
  I2CTimeout = 5000;
 
  while (1)
  {
    static i32_t OldTime = 0;
    if (UpdateMotor)
    {
      UpdateMotor=0;
 
#ifdef USE_OSD
      /* G e n e r a t e   O S D   D a t a */
      static char CntOSD = 0;
      if (CntOSD % 6 == 1)
      {
        SendOSD();
      }
      CntOSD++;
#endif
 
      /* Set the cycle Time to 120ms / 125ms */
      if (OldTime != 0)
      {
        while (((Count8Khz - OldTime) *10) / 8 < 120);
        DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8;
      }
      OldTime = Count8Khz;
     
      /* GetMeasurements()*/
      GetMeasurements();
 
      /* EstimateFlightAttitude */
      FlightAttitudeEstimation();
 
      /* Set Nominal Value */
      RemoteControl();  
 
      /* PD Control */
      PD_Regler();
     
      /* Send Motor Data */
      SendMotorData();
     
      /* TestRemote */
      TestRemote();
     
      /* Test IC2- / RC-Link */
      TestIC2Link();
    }
    /* Send Debug Data over RS232 */
    SendDebugData();
    /* Check the Batery for Undervoltage */
    TestBattery();
    /* DeployRescue */
    DeployRescue();
  }
}
 
/* ****************************************************************************
Functionname:     DeployRescue                      */ /*!
Description:       Deploy a rescue parachute using a servo
 
  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
void DeployRescue()
{
 
  /* Yaw or pitch are greater than 90 Deg abs */
  if (((abs(status.iTheta10) > 900) || (abs(status.iPhi10) > 900)) &&
     ((abs(AverageRoll) > 400) ||
      (abs(AverageNick) > 400) ||
      (abs(AverageGier) > 400) ))
  {
    //MotorenEin = 0;
    Delay_ms(500);
    InvalidAttitude = 1;
  }
 
  if (InvalidAttitude)
  {
    ServoValue = 150;
  }
  else
  {
    ServoValue = 0;
  }
}
 
/* ****************************************************************************
Functionname:     ReadParameterSet                      */ /*!
Description:      -- Parametersatz aus EEPROM lesen ---
                  number [0..5]
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
  if (number > 5)
  {
    number = 5;
  }
  eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
}
 
/* ****************************************************************************
Functionname:     WriteParameterSet                      */ /*!
Description:      -- Parametersatz ins EEPROM schreiben ---
                  number [0..5]
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
{
  if(number > 5)
  {
    number = 5;
  }
  eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
  eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number);      // diesen Parametersatz als aktuell merken
}
 
/* ****************************************************************************
Functionname:     GetActiveParamSetNumber                      */ /*!
Description:      
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
unsigned char GetActiveParamSetNumber(void)
{
  unsigned char set;
  set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);
  if(set > 5)
  {
    set = 2;  
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set);      // diesen Parametersatz als aktuell merken
  }
  return(set);
}
 
/* ****************************************************************************
Functionname:     GetAirPressureOffset                      */ /*!
Description:      
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void GetAirPressureOffset(void)
{
 unsigned int timer;
 ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
  printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
  if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
  {
    printf("\n\rAbgleich Luftdrucksensor..");
    timer = SetDelay(1000);  
    SucheLuftruckOffset();
    while (!CheckDelay(timer));
    printf("OK\n\r");
  }
}
 
/* ****************************************************************************
Functionname:     GenerateDefaults                      */ /*!
Description:      Generate the Default Paramter
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  GenerateDefaults()
{
  VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
  VersionInfo.Nebenversion = VERSION_NEBENVERSION;
  VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
 
#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben
  if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
  {
    printf("\n\rInit. EEPROM: Generiere Default-Parameter...");
    DefaultKonstanten1();
    for (unsigned char i=0;i<6;i++)  
    {
      if(i==2) DefaultKonstanten2(); // Kamera
      if(i==3) DefaultKonstanten3(); // Beginner
      if(i>3)  DefaultKonstanten2(); // Kamera
      WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
    }
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
  }
}
 
 
/* ****************************************************************************
Functionname:     InitPorts                      */ /*!
Description:      Init the IO Ports
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  InitPorts()
{
  unsigned int timer = 0;
  DDRB  = 0x00;
  PORTB = 0x00;
  for(timer = 0; timer < 1000; timer++); // verzögern
  DDRC  = 0x81; // SCL
  PORTC = 0xff; // Pullup SDA
  DDRB  = 0x1B; // LEDs und Druckoffset
  PORTB = 0x01; // LED_Rot
  DDRD  = 0x3E; // Speaker & TXD & J3 J4 J5
  DDRD  |=0x80; // J7
  PORTD = 0xF7; // LED
 
  MCUSR &=~(1<<WDRF);
  WDTCSR |= (1<<WDCE)|(1<<WDE);
  WDTCSR = 0;
}
 
/* ****************************************************************************
Functionname:     TestIC2Link                      */ /*!
Description:      Test IC2- / RC-Link
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void TestIC2Link()
{
  if(PcZugriff)
  {
    PcZugriff--;
  }
 
  if(SenderOkay)  
  {
    SenderOkay--;
  }
 
  if(!I2CTimeout)
  {
    I2CTimeout = 5;
    i2c_reset();
    if((BeepMuster == 0xffff) && MotorenEin)
    {
      beeptime = 10000;
      BeepMuster = 0x0080;
    }
  }
  else
  {
    I2CTimeout--;
  }
}
 
 
/* ****************************************************************************
Functionname:     SendDebugData                      */ /*!
Description:      Send Debug Data over RS232
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void SendDebugData()
{
  if(SIO_DEBUG)
  {
    DatenUebertragung();
    BearbeiteRxDaten();
  }
  else
  {
    BearbeiteRxDaten();
  }
}
 
 
/* ****************************************************************************
Functionname:     TestBattery                      */ /*!
Description:      Check the Battery Voltage
 
  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void  TestBattery()
{
  unsigned int timer = 0;
  if(CheckDelay(timer))
  {  /* Voltage is Below Threshhod ? */
    if(UBat < EE_Parameter.UnterspannungsWarnung)
    {    
      if(BeepMuster == 0xffff)
      {
        beeptime = 6000;
        BeepMuster = 0x0300;
      }
    }
    timer = SetDelay(100);  
  }
  DebugOut.Analog[15] =  UBat;
}
 
/* ****************************************************************************
  Functionname:     TestRemote                      */ /*!
  Description:      
 
  @param[in]        
 
  @return           void
  @pre              -
  @post             -
  @author         Michael Walter  
**************************************************************************** */
void TestRemote()
{
  /*--- (SYMBOLIC) CONSTANTS ---*/
 
  /*--- VARIABLES ---*/
  static unsigned int TimeSinceRC_BelowThreshhold = 0;
  static unsigned int TimeSinceRC_AboveThreshhold = 0;
 
  if (MotorenEin == 1)
  {
    if (RemoteLinkLost == 0)
    {
      if (RCQuality < 60)
      {
        if (TimeSinceRC_BelowThreshhold < 10000)
        {
          TimeSinceRC_BelowThreshhold++;
        }
      }
      else
      {
        TimeSinceRC_BelowThreshhold = 0;
      }
      if ((TimeSinceRC_BelowThreshhold > 500) || /* aprox. 5 seconds */
        (SenderOkay < 100))
      {
        RemoteLinkLost = 1;
        TimeSinceRC_AboveThreshhold = 0;
      }
    }
    else
    {
      if (RCQuality > 80)
      {
        if (TimeSinceRC_AboveThreshhold < 10000)
        {
          TimeSinceRC_AboveThreshhold++;
        }
      }
      else
      {
        TimeSinceRC_AboveThreshhold = 0;
      }
      if (TimeSinceRC_AboveThreshhold > 100) /* aprox. 1 seconds */
      {
        RemoteLinkLost = 0;
        TimeSinceRC_BelowThreshhold = 0;    
      }
    }
  }
  else
  {
    RemoteLinkLost = 0;
    TimeSinceRC_BelowThreshhold = 0;
    TimeSinceRC_AboveThreshhold = 0;
  }
  /*DebugOut.Analog[14] = TimeSinceRC_BelowThreshhold;*/
  /*DebugOut.Analog[15] = TimeSinceRC_AboveThreshhold;*/
}
 

Rev 980	Rev 985
1	/*	1	/*
2	Copyright 2008, by Michael Walter	2	Copyright 2008, by Michael Walter
3		3
4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser	4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but	5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public	7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
8	License along with this program. If not, see <http://www.gnu.org/licenses/>.	8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
9		9
10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that	10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de	11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12	unless it is stated otherwise.	12	unless it is stated otherwise.
13	*/	13	*/
14		14
15	/*****************************************************************************	15	/*****************************************************************************
16	INCLUDES	16	INCLUDES
17	**************************************************************************** */	17	**************************************************************************** */
18	#include "main.h"	18	#include "main.h"
19	#include "kafi.h"	19	#include "kafi.h"
20	#include "mymath.h"	20	#include "mymath.h"
21	#include "mm3.h"	21	#include "mm3.h"
22	#include "FlightControl.h"	22	#include "FlightControl.h"
23		23
24	/*****************************************************************************	24	/*****************************************************************************
25	(SYMBOLIC) CONSTANTS	25	(SYMBOLIC) CONSTANTS
26	*****************************************************************************/	26	*****************************************************************************/
27		27
28	#define sin45 -0.707106	28	#define sin45 -0.707106
29	#define cos45 0.707106	29	#define cos45 0.707106
30		30
31	/*****************************************************************************	31	/*****************************************************************************
32	VARIABLES	32	VARIABLES
33	*****************************************************************************/	33	*****************************************************************************/
34	extern int RCQuality;	34	extern int RCQuality;
35	int RemoteLinkLost;	35	int RemoteLinkLost = 0;
-		36	int InvalidAttitude = 0;
36	extern unsigned long maxDistance;	37	extern unsigned long maxDistance;
37	unsigned char EEPromArray[E2END+1] EEMEM;	38	unsigned char EEPromArray[E2END+1] EEMEM;
38		39
39	void TestRemote(void);	40	void TestRemote(void);
40	void IMU_Main(void);	41	void IMU_Main(void);
41	void TestBattery(void);	42	void TestBattery(void);
42	void SendDebugData(void);	43	void SendDebugData(void);
43	void InitPorts(void);	44	void InitPorts(void);
44	void GenerateDefaults(void);	45	void GenerateDefaults(void);
45	void TestIC2Link(void);	46	void TestIC2Link(void);
46	void GetAirPressureOffset(void);	47	void GetAirPressureOffset(void);
47	void DeployRescue(void);	48	void DeployRescue(void);
48		49
49	extern void InitOSD(void);	50	extern void InitOSD(void);
50	extern void InitGPS(void);	51	extern void InitGPS(void);
51	extern void SendOSD(void);	52	extern void SendOSD(void);
52	extern void RemoteControl(void);	53	extern void RemoteControl(void);
53	extern void SendMotorData(void);	54	extern void SendMotorData(void);
54	extern void GetMeasurements(void);	55	extern void GetMeasurements(void);
55	extern void AttitudeEstimation(void);	56	extern void AttitudeEstimation(void);
56	extern void PD_Regler(void);	57	extern void PD_Regler(void);
57	extern void SetNeutral(void);	58	extern void SetNeutral(void);
58		59
59	/* ****************************************************************************	60	/* ****************************************************************************
60	Functionname: main / /!	61	Functionname: main / /!
61	Description: Hauptprogramm	62	Description: Hauptprogramm
62		63
63	@return void	64	@return void
64	@pre -	65	@pre -
65	@post -	66	@post -
66	@author Michael Walter	67	@author Michael Walter
67	**************************************************************************** */	68	**************************************************************************** */
68	int main (void)	69	int main (void)
69	{	70	{
70	/* Controler Init */	71	/* Controler Init */
71	InitPorts();	72	InitPorts();
72	Kafi_Init();	73	Kafi_Init();
73	Timer_Init();	74	Timer_Init();
74	UART_Init();	75	UART_Init();
75	InitGPS();	76	InitGPS();
76	rc_sum_init();	77	rc_sum_init();
77	ADC_Init();	78	ADC_Init();
78	i2c_init();	79	i2c_init();
79	#ifdef USE_COMPASS	80	#ifdef USE_COMPASS
80	MM3_Init();	81	MM3_Init();
81	#endif	82	#endif
82	/* Enable Interrupts */	83	/* Enable Interrupts */
83	sei();	84	sei();
84		85
85	/* Generate Default Values */	86	/* Generate Default Values */
86	GenerateDefaults ();	87	GenerateDefaults ();
87	/* Get Air Pressure Offset */	88	/* Get Air Pressure Offset */
88	GetAirPressureOffset();	89	GetAirPressureOffset();
89	/* Determine Gyro / ACC Offsets */	90	/* Determine Gyro / ACC Offsets */
90	SetNeutral();	91	SetNeutral();
91		92
92	DebugIn.Analog[1] = 1000;	93	DebugIn.Analog[1] = 1000;
93	DebugIn.Digital[0] = 0x55;	94	DebugIn.Digital[0] = 0x55;
94		95
95	#ifdef USE_COMPASS	96	#ifdef USE_COMPASS
96	/* Calibrate the MM3 Compass? */	97	/* Calibrate the MM3 Compass? */
97	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) &&	98	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) &&
98	(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) &&	99	(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) &&
99	(MotorenEin == 0))	100	(MotorenEin == 0))
100	{	101	{
101	printf("\n\rCalibrating Compass");	102	printf("\n\rCalibrating Compass");
102	MM3_Calibrate();	103	MM3_Calibrate();
103	}	104	}
104	#endif	105	#endif
105		106
106	#ifdef USE_OSD	107	#ifdef USE_OSD
107	/* Init the Bob-4 OnScreen Display */	108	/* Init the Bob-4 OnScreen Display */
108	InitOSD();	109	InitOSD();
109	#endif	110	#endif
110		111
111	/* Start the main Task */	112	/* Start the main Task */
112	IMU_Main();	113	IMU_Main();
113	return (1);	114	return (1);
114	}	115	}
115		116
116		117
117	/* ****************************************************************************	118	/* ****************************************************************************
118	Functionname: IMU_Main / /!	119	Functionname: IMU_Main / /!
119	Description:	120	Description:
120		121
121	@param[in]	122	@param[in]
122		123
123	@return void	124	@return void
124	@pre -	125	@pre -
125	@post -	126	@post -
126	@author Michael Walter	127	@author Michael Walter
127	**************************************************************************** */	128	**************************************************************************** */
128	void IMU_Main()	129	void IMU_Main()
129	{	130	{
130	ROT_ON	131	ROT_ON
131	I2CTimeout = 5000;	132	I2CTimeout = 5000;
132		133
133	while (1)	134	while (1)
134	{	135	{
135	static i32_t OldTime = 0;	136	static i32_t OldTime = 0;
136	if (UpdateMotor)	137	if (UpdateMotor)
137	{	138	{
138	UpdateMotor=0;	139	UpdateMotor=0;
139		140
140	#ifdef USE_OSD	141	#ifdef USE_OSD
141	/* G e n e r a t e O S D D a t a */	142	/* G e n e r a t e O S D D a t a */
142	static char CntOSD = 0;	143	static char CntOSD = 0;
143	if (CntOSD % 6 == 1)	144	if (CntOSD % 6 == 1)
144	{	145	{
145	SendOSD();	146	SendOSD();
146	}	147	}
147	CntOSD++;	148	CntOSD++;
148	#endif	149	#endif
149		150
150	/* Set the cycle Time to 120ms / 125ms */	151	/* Set the cycle Time to 120ms / 125ms */
151	if (OldTime != 0)	152	if (OldTime != 0)
152	{	153	{
153	while (((Count8Khz - OldTime) *10) / 8 < 120);	154	while (((Count8Khz - OldTime) *10) / 8 < 120);
154	DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8;	155	DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8;
155	}	156	}
156	OldTime = Count8Khz;	157	OldTime = Count8Khz;
157		158
158	/* GetMeasurements()*/	159	/* GetMeasurements()*/
159	GetMeasurements();	160	GetMeasurements();
160		161
161	/* EstimateFlightAttitude */	162	/* EstimateFlightAttitude */
162	FlightAttitudeEstimation();	163	FlightAttitudeEstimation();
163		164
164	/* Set Nominal Value */	165	/* Set Nominal Value */
165	RemoteControl();	166	RemoteControl();
166		167
167	/* PD Control */	168	/* PD Control */
168	PD_Regler();	169	PD_Regler();
169		170
170	/* Send Motor Data */	171	/* Send Motor Data */
171	SendMotorData();	172	SendMotorData();
172		173
173	/* TestRemote */	174	/* TestRemote */
174	TestRemote();	175	TestRemote();
175		176
176	/* Test IC2- / RC-Link */	177	/* Test IC2- / RC-Link */
177	TestIC2Link();	178	TestIC2Link();
178	}	179	}
179	/* Send Debug Data over RS232 */	180	/* Send Debug Data over RS232 */
180	SendDebugData();	181	SendDebugData();
181	/* Check the Batery for Undervoltage */	182	/* Check the Batery for Undervoltage */
182	TestBattery();	183	TestBattery();
183	/* DeployRescue */	184	/* DeployRescue */
184	DeployRescue();	185	DeployRescue();
185	}	186	}
186	}	187	}
187		188
188	/* ****************************************************************************	189	/* ****************************************************************************
189	Functionname: DeployRescue / /!	190	Functionname: DeployRescue / /!
190	Description: Deploy a rescue parachute using a servo	191	Description: Deploy a rescue parachute using a servo
191		192
192	@return void	193	@return void
193	@pre -	194	@pre -
194	@post -	195	@post -
195	@author Michael Walter	196	@author Michael Walter
196	**************************************************************************** */	197	**************************************************************************** */
197	void DeployRescue()	198	void DeployRescue()
198	{	199	{
199	static int InvalidAttitude = 0;	-
200	/* Yaw or pitch are greater than 90 Deg abs */	200	/* Yaw or pitch are greater than 90 Deg abs */
201	if (((abs(status.iTheta10) > 900) \|\| (abs(status.iPhi10) > 900)) &&	201	if (((abs(status.iTheta10) > 900) \|\| (abs(status.iPhi10) > 900)) &&
202	((abs(AverageRoll) > 400) \|\|	202	((abs(AverageRoll) > 400) \|\|
203	(abs(AverageNick) > 400) \|\|	203	(abs(AverageNick) > 400) \|\|
204	(abs(AverageGier) > 400) ))	204	(abs(AverageGier) > 400) ))
205	{	205	{
206	//MotorenEin = 0;	206	//MotorenEin = 0;
207	Delay_ms(500);	207	Delay_ms(500);
208	InvalidAttitude = 1;	208	InvalidAttitude = 1;
209	}	209	}
210		210
211	if (InvalidAttitude)	211	if (InvalidAttitude)
212	{	212	{
213	ServoValue = 150;	213	ServoValue = 150;
214	}	214	}
215	else	215	else
216	{	216	{
217	ServoValue = 0;	217	ServoValue = 0;
218	}	218	}
219	}	219	}
220		220
221	/* ****************************************************************************	221	/* ****************************************************************************
222	Functionname: ReadParameterSet / /!	222	Functionname: ReadParameterSet / /!
223	Description: -- Parametersatz aus EEPROM lesen ---	223	Description: -- Parametersatz aus EEPROM lesen ---
224	number [0..5]	224	number [0..5]
225		225
226	@return void	226	@return void
227	@pre -	227	@pre -
228	@post -	228	@post -
229	@author H. Buss / I. Busker	229	@author H. Buss / I. Busker
230	**************************************************************************** */	230	**************************************************************************** */
231	void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)	231	void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
232	{	232	{
233	if (number > 5)	233	if (number > 5)
234	{	234	{
235	number = 5;	235	number = 5;
236	}	236	}
237	eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);	237	eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
238	}	238	}
239		239
240	/* ****************************************************************************	240	/* ****************************************************************************
241	Functionname: WriteParameterSet / /!	241	Functionname: WriteParameterSet / /!
242	Description: -- Parametersatz ins EEPROM schreiben ---	242	Description: -- Parametersatz ins EEPROM schreiben ---
243	number [0..5]	243	number [0..5]
244		244
245	@return void	245	@return void
246	@pre -	246	@pre -
247	@post -	247	@post -
248	@author H. Buss / I. Busker	248	@author H. Buss / I. Busker
249	**************************************************************************** */	249	**************************************************************************** */
250	void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)	250	void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
251	{	251	{
252	if(number > 5)	252	if(number > 5)
253	{	253	{
254	number = 5;	254	number = 5;
255	}	255	}
256	eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);	256	eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
257	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken	257	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken
258	}	258	}
259		259
260	/* ****************************************************************************	260	/* ****************************************************************************
261	Functionname: GetActiveParamSetNumber / /!	261	Functionname: GetActiveParamSetNumber / /!
262	Description:	262	Description:
263		263
264	@return void	264	@return void
265	@pre -	265	@pre -
266	@post -	266	@post -
267	@author H. Buss / I. Busker	267	@author H. Buss / I. Busker
268	**************************************************************************** */	268	**************************************************************************** */
269	unsigned char GetActiveParamSetNumber(void)	269	unsigned char GetActiveParamSetNumber(void)
270	{	270	{
271	unsigned char set;	271	unsigned char set;
272	set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);	272	set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]);
273	if(set > 5)	273	if(set > 5)
274	{	274	{
275	set = 2;	275	set = 2;
276	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken	276	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken
277	}	277	}
278	return(set);	278	return(set);
279	}	279	}
280		280
281	/* ****************************************************************************	281	/* ****************************************************************************
282	Functionname: GetAirPressureOffset / /!	282	Functionname: GetAirPressureOffset / /!
283	Description:	283	Description:
284		284
285	@return void	285	@return void
286	@pre -	286	@pre -
287	@post -	287	@post -
288	@author H. Buss / I. Busker	288	@author H. Buss / I. Busker
289	**************************************************************************** */	289	**************************************************************************** */
290	void GetAirPressureOffset(void)	290	void GetAirPressureOffset(void)
291	{	291	{
292	unsigned int timer;	292	unsigned int timer;
293	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	293	ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
294	printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());	294	printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber());
295	if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)	295	if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
296	{	296	{
297	printf("\n\rAbgleich Luftdrucksensor..");	297	printf("\n\rAbgleich Luftdrucksensor..");
298	timer = SetDelay(1000);	298	timer = SetDelay(1000);
299	SucheLuftruckOffset();	299	SucheLuftruckOffset();
300	while (!CheckDelay(timer));	300	while (!CheckDelay(timer));
301	printf("OK\n\r");	301	printf("OK\n\r");
302	}	302	}
303	}	303	}
304		304
305	/* ****************************************************************************	305	/* ****************************************************************************
306	Functionname: GenerateDefaults / /!	306	Functionname: GenerateDefaults / /!
307	Description: Generate the Default Paramter	307	Description: Generate the Default Paramter
308		308
309	@return void	309	@return void
310	@pre -	310	@pre -
311	@post -	311	@post -
312	@author H. Buss / I. Busker	312	@author H. Buss / I. Busker
313	**************************************************************************** */	313	**************************************************************************** */
314	void GenerateDefaults()	314	void GenerateDefaults()
315	{	315	{
316	VersionInfo.Hauptversion = VERSION_HAUPTVERSION;	316	VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
317	VersionInfo.Nebenversion = VERSION_NEBENVERSION;	317	VersionInfo.Nebenversion = VERSION_NEBENVERSION;
318	VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;	318	VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
319		319
320	#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben	320	#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben
321	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)	321	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION)
322	{	322	{
323	printf("\n\rInit. EEPROM: Generiere Default-Parameter...");	323	printf("\n\rInit. EEPROM: Generiere Default-Parameter...");
324	DefaultKonstanten1();	324	DefaultKonstanten1();
325	for (unsigned char i=0;i<6;i++)	325	for (unsigned char i=0;i<6;i++)
326	{	326	{
327	if(i==2) DefaultKonstanten2(); // Kamera	327	if(i==2) DefaultKonstanten2(); // Kamera
328	if(i==3) DefaultKonstanten3(); // Beginner	328	if(i==3) DefaultKonstanten3(); // Beginner
329	if(i>3) DefaultKonstanten2(); // Kamera	329	if(i>3) DefaultKonstanten2(); // Kamera
330	WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);	330	WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
331	}	331	}
332	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting	332	eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting
333	eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);	333	eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION);
334	}	334	}
335	}	335	}
336		336
337		337
338	/* ****************************************************************************	338	/* ****************************************************************************
339	Functionname: InitPorts / /!	339	Functionname: InitPorts / /!
340	Description: Init the IO Ports	340	Description: Init the IO Ports
341		341
342	@return void	342	@return void
343	@pre -	343	@pre -
344	@post -	344	@post -
345	@author H. Buss / I. Busker	345	@author H. Buss / I. Busker
346	**************************************************************************** */	346	**************************************************************************** */
347	void InitPorts()	347	void InitPorts()
348	{	348	{
349	unsigned int timer = 0;	349	unsigned int timer = 0;
350	DDRB = 0x00;	350	DDRB = 0x00;
351	PORTB = 0x00;	351	PORTB = 0x00;
352	for(timer = 0; timer < 1000; timer++); // verzögern	352	for(timer = 0; timer < 1000; timer++); // verzögern
353	DDRC = 0x81; // SCL	353	DDRC = 0x81; // SCL
354	PORTC = 0xff; // Pullup SDA	354	PORTC = 0xff; // Pullup SDA
355	DDRB = 0x1B; // LEDs und Druckoffset	355	DDRB = 0x1B; // LEDs und Druckoffset
356	PORTB = 0x01; // LED_Rot	356	PORTB = 0x01; // LED_Rot
357	DDRD = 0x3E; // Speaker & TXD & J3 J4 J5	357	DDRD = 0x3E; // Speaker & TXD & J3 J4 J5
358	DDRD \|=0x80; // J7	358	DDRD \|=0x80; // J7
359	PORTD = 0xF7; // LED	359	PORTD = 0xF7; // LED
360		360
361	MCUSR &=~(1<<WDRF);	361	MCUSR &=~(1<<WDRF);
362	WDTCSR \|= (1<<WDCE)\|(1<<WDE);	362	WDTCSR \|= (1<<WDCE)\|(1<<WDE);
363	WDTCSR = 0;	363	WDTCSR = 0;
364	}	364	}
365		365
366	/* ****************************************************************************	366	/* ****************************************************************************
367	Functionname: TestIC2Link / /!	367	Functionname: TestIC2Link / /!
368	Description: Test IC2- / RC-Link	368	Description: Test IC2- / RC-Link
369		369
370	@return void	370	@return void
371	@pre -	371	@pre -
372	@post -	372	@post -
373	@author H. Buss / I. Busker	373	@author H. Buss / I. Busker
374	**************************************************************************** */	374	**************************************************************************** */
375	void TestIC2Link()	375	void TestIC2Link()
376	{	376	{
377	if(PcZugriff)	377	if(PcZugriff)
378	{	378	{
379	PcZugriff--;	379	PcZugriff--;
380	}	380	}
381		381
382	if(SenderOkay)	382	if(SenderOkay)
383	{	383	{
384	SenderOkay--;	384	SenderOkay--;
385	}	385	}
386		386
387	if(!I2CTimeout)	387	if(!I2CTimeout)
388	{	388	{
389	I2CTimeout = 5;	389	I2CTimeout = 5;
390	i2c_reset();	390	i2c_reset();
391	if((BeepMuster == 0xffff) && MotorenEin)	391	if((BeepMuster == 0xffff) && MotorenEin)
392	{	392	{
393	beeptime = 10000;	393	beeptime = 10000;
394	BeepMuster = 0x0080;	394	BeepMuster = 0x0080;
395	}	395	}
396	}	396	}
397	else	397	else
398	{	398	{
399	I2CTimeout--;	399	I2CTimeout--;
400	}	400	}
401	}	401	}
402		402
403		403
404	/* ****************************************************************************	404	/* ****************************************************************************
405	Functionname: SendDebugData / /!	405	Functionname: SendDebugData / /!
406	Description: Send Debug Data over RS232	406	Description: Send Debug Data over RS232
407		407
408	@return void	408	@return void
409	@pre -	409	@pre -
410	@post -	410	@post -
411	@author H. Buss / I. Busker	411	@author H. Buss / I. Busker
412	**************************************************************************** */	412	**************************************************************************** */
413	void SendDebugData()	413	void SendDebugData()
414	{	414	{
415	if(SIO_DEBUG)	415	if(SIO_DEBUG)
416	{	416	{
417	DatenUebertragung();	417	DatenUebertragung();
418	BearbeiteRxDaten();	418	BearbeiteRxDaten();
419	}	419	}
420	else	420	else
421	{	421	{
422	BearbeiteRxDaten();	422	BearbeiteRxDaten();
423	}	423	}
424	}	424	}
425		425
426		426
427	/* ****************************************************************************	427	/* ****************************************************************************
428	Functionname: TestBattery / /!	428	Functionname: TestBattery / /!
429	Description: Check the Battery Voltage	429	Description: Check the Battery Voltage
430		430
431	@return void	431	@return void
432	@pre -	432	@pre -
433	@post -	433	@post -
434	@author H. Buss / I. Busker	434	@author H. Buss / I. Busker
435	**************************************************************************** */	435	**************************************************************************** */
436	void TestBattery()	436	void TestBattery()
437	{	437	{
438	unsigned int timer = 0;	438	unsigned int timer = 0;
439	if(CheckDelay(timer))	439	if(CheckDelay(timer))
440	{ /* Voltage is Below Threshhod ? */	440	{ /* Voltage is Below Threshhod ? */
441	if(UBat < EE_Parameter.UnterspannungsWarnung)	441	if(UBat < EE_Parameter.UnterspannungsWarnung)
442	{	442	{
443	if(BeepMuster == 0xffff)	443	if(BeepMuster == 0xffff)
444	{	444	{
445	beeptime = 6000;	445	beeptime = 6000;
446	BeepMuster = 0x0300;	446	BeepMuster = 0x0300;
447	}	447	}
448	}	448	}
449	timer = SetDelay(100);	449	timer = SetDelay(100);
450	}	450	}
451	DebugOut.Analog[15] = UBat;	451	DebugOut.Analog[15] = UBat;
452	}	452	}
453		453
454	/* ****************************************************************************	454	/* ****************************************************************************
455	Functionname: TestRemote / /!	455	Functionname: TestRemote / /!
456	Description:	456	Description:
457		457
458	@param[in]	458	@param[in]
459		459
460	@return void	460	@return void
461	@pre -	461	@pre -
462	@post -	462	@post -
463	@author Michael Walter	463	@author Michael Walter
464	**************************************************************************** */	464	**************************************************************************** */
465	void TestRemote()	465	void TestRemote()
466	{	466	{
467	/--- (SYMBOLIC) CONSTANTS ---/	467	/--- (SYMBOLIC) CONSTANTS ---/
468		468
469	/--- VARIABLES ---/	469	/--- VARIABLES ---/
470	static unsigned int TimeSinceRC_BelowThreshhold = 0;	470	static unsigned int TimeSinceRC_BelowThreshhold = 0;
471	static unsigned int TimeSinceRC_AboveThreshhold = 0;	471	static unsigned int TimeSinceRC_AboveThreshhold = 0;
472		472
473	if (MotorenEin == 1)	473	if (MotorenEin == 1)
474	{	474	{
475	if (RemoteLinkLost == 0)	475	if (RemoteLinkLost == 0)
476	{	476	{
477	if (RCQuality < 60)	477	if (RCQuality < 60)
478	{	478	{
479	if (TimeSinceRC_BelowThreshhold < 10000)	479	if (TimeSinceRC_BelowThreshhold < 10000)
480	{	480	{
481	TimeSinceRC_BelowThreshhold++;	481	TimeSinceRC_BelowThreshhold++;
482	}	482	}
483	}	483	}
484	else	484	else
485	{	485	{
486	TimeSinceRC_BelowThreshhold = 0;	486	TimeSinceRC_BelowThreshhold = 0;
487	}	487	}
488	if ((TimeSinceRC_BelowThreshhold > 500) \|\| /* aprox. 5 seconds */	488	if ((TimeSinceRC_BelowThreshhold > 500) \|\| /* aprox. 5 seconds */
489	(SenderOkay < 100))	489	(SenderOkay < 100))
490	{	490	{
491	RemoteLinkLost = 1;	491	RemoteLinkLost = 1;
492	TimeSinceRC_AboveThreshhold = 0;	492	TimeSinceRC_AboveThreshhold = 0;
493	}	493	}
494	}	494	}
495	else	495	else
496	{	496	{
497	if (RCQuality > 80)	497	if (RCQuality > 80)
498	{	498	{
499	if (TimeSinceRC_AboveThreshhold < 10000)	499	if (TimeSinceRC_AboveThreshhold < 10000)

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/main.c - Rev 980 → 985