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

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/main.c - Rev 966 → 970