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

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/main.c @ 1606 - Rev 974 → 980