Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 838 → Rev 966

/branches/KalmanFilter MikeW/main.c
1,75 → 1,210
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
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"
 
/*****************************************************************************
(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 MotorControl(void);
extern void RemoteControl(void);
extern void SendMotorData(void);
extern void CalculateAverage(void);
extern void GetMeasurements(void);
extern void AttitudeEstimation(void);
extern void PID_Regler(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]
81,11 → 216,11
**************************************************************************** */
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);
if (number > 5)
{
number = 5;
}
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length);
}
 
/* ****************************************************************************
100,15 → 235,14
**************************************************************************** */
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
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:
120,20 → 254,43
**************************************************************************** */
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);
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: main */ /*!
Description: Hauptprogramm
Functionname: GenerateDefaults */ /*!
Description: Generate the Default Paramter
 
@return void
@pre -
140,184 → 297,213
@post -
@author H. Buss / I. Busker
**************************************************************************** */
int main (void)
void GenerateDefaults()
{
unsigned int timer;
//unsigned int timer2 = 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;
beeptime = 2000;
StickGier = 0; StickRoll = 0; StickNick = 0; PPM_in[K_GAS] = 0;
Kafi_Init();
Timer_Init();
UART_Init();
InitGPS();
rc_sum_init();
ADC_Init();
i2c_init();
/* Init the MM3 */
MM3_Init();
sei();
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);
}
}
 
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
GRN_ON;
#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);
}
 
/* Init EE_Parameter */
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");
}
ROT_ON
Delay_ms(1000);
SetNeutral();
ROT_OFF
Delay_ms(1000);
SetNeutral();
ROT_ON
beeptime = 2000;
DebugIn.Analog[1] = 1000;
DebugIn.Digital[0] = 0x55;
/* 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();
}
/* Init the OSD */
// InitOSD();
/* ****************************************************************************
Functionname: InitPorts */ /*!
Description: Init the IO Ports
 
I2CTimeout = 5000;
while (1)
{
static i32_t OldTime = 0;
if (UpdateMotor) // ReglerIntervall
{
UpdateMotor=0;
/* 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++;
/* Set the cycle Time to 110ms */
if (OldTime != 0)
{
#ifdef USE_Extended_MM3_Measurement_Model
while (((Count8Khz - OldTime) *10) / 8 < 160);
#else
while (((Count8Khz - OldTime) *10) / 8 < 110);
#endif
DebugOut.Analog[7] = ((Count8Khz - OldTime) *10) / 8;
}
OldTime = Count8Khz;
/* Average the Measurements */
CalculateAverage();
/* Do the Kalman Filterimg */
AttitudeEstimation();
/* MotorControl */
MotorControl();
/* Call the PID Control */
PID_Regler();
SendMotorData();
if(PcZugriff)
{
PcZugriff--;
}
if(SenderOkay)
{
SenderOkay--;
}
if(!I2CTimeout)
{
I2CTimeout = 5;
i2c_reset();
if((BeepMuster == 0xffff) && MotorenEin)
{
beeptime = 10000;
BeepMuster = 0x0080;
}
}
else
{
I2CTimeout--;
}
@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++;
}
if(SIO_DEBUG)
{
DatenUebertragung();
BearbeiteRxDaten();
}
else BearbeiteRxDaten();
if(CheckDelay(timer))
{
if(UBat < EE_Parameter.UnterspannungsWarnung)
{
if(BeepMuster == 0xffff)
{
beeptime = 6000;
BeepMuster = 0x0300;
}
}
timer = SetDelay(100);
}
}
return (1);
}
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;*/
}