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

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/main.c - Rev 972 → 973