Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 837 → Rev 838

/branches/KalmanFilter MikeW/Bob4_OSD.c
0,0 → 1,354
/*
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.
*/
 
#include "main.h"
#include "kafi.h"
 
extern void UART2Print(void);
void InitOSD(void);
void SendOSD(void);
 
extern gpsInfo_t actualPos; // measured position (last gps record)
extern gpsInfo_t targetPos; // measured position (last gps record)
 
extern signed int GPS_Nick;
extern signed int GPS_Roll;
 
extern int CurrentAltitude, LastAltitude, targetPosValid, RCQuality;
extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d;
extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45;
extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P;
extern unsigned long maxDistance;
 
unsigned char UART2PrintAbgeschlossen = 1;
void UART2Print(void);
 
char DisplayBuff[80] = "";
unsigned char DispPtr = 0;
 
char OSDBuff[80] = "";
unsigned char OSDPtr = 0;
 
 
#define OSD_printf(format, args...) { OSDPtr = 0; _printf_P(OUT_OSD,PSTR(format) , ## args);UART2Print();}
#define OSD_printf_(format, args...) { _printf_P(OUT_OSD,PSTR(format) , ## args);}
 
 
/* For OSD_printf to work, Putchar() has to be extendet as follows.
char Putchar(char zeichen)
{
if(PrintZiel == OUT_LCD)
{
DisplayBuff[DispPtr++] = zeichen; return(1);
}
else if (PrintZiel == OUT_OSD)
{
OSDBuff[OSDPtr++] = zeichen; return(1);
}
else
{
return(uart_putchar(zeichen));
}
}
*/
 
 
/* ****************************************************************************
Functionname: OsdClear */ /*!
Description:
 
@param[in]
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void OsdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) OSDBuff[i] = ' ';
}
 
/* ****************************************************************************
Functionname: LcdClear */ /*!
Description:
 
@param[in]
 
@return void
@pre -
@post -
@author
**************************************************************************** */
void LcdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
}
 
 
/* ****************************************************************************
Functionname: InitOSD */ /*!
Description: Init the Bob-4H OSD
 
@param[in]
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void InitOSD()
{
/* Clear OSD */
OSD_printf ("\33[2J");
Delay_ms_Mess(300);
OSD_printf ("\33[2J");
/* Set OSD Pixel Clock */
Delay_ms_Mess(300);
OSD_printf ("\33[23;6v");
/* Set OSD Pixel Width */
Delay_ms_Mess(300);
OSD_printf ("\33[25;448v");
/* Set OSD Font */
Delay_ms_Mess(300);
OSD_printf ("\33[0z");
}
 
 
 
/* ****************************************************************************
Functionname: UART2Print */ /*!
Description: Send OSD Data to the Bob-4H OSD
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void UART2Print()
{
OSDBuff[OSDPtr] = '\r';
if (UART2PrintAbgeschlossen == 1)
{
UART2PrintAbgeschlossen = 0;
UDR1 = OSDBuff[0];
}
}
 
/* ****************************************************************************
Functionname: SIGNAL */ /*!
Description: Send OSD Data to the Bob-4H OSD
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
SIGNAL(SIG_USART1_TRANS)
{
static unsigned int ptr = 0;
unsigned char tmp_tx;
if(!UART2PrintAbgeschlossen)
{
ptr++; // die [0] wurde schon gesendet
tmp_tx = OSDBuff[ptr];
if((tmp_tx == '\r') /* tmp_tx == 0 */)
{
ptr = 0;
UART2PrintAbgeschlossen = 1;
}
UDR1 = tmp_tx;
}
else ptr = 0;
}
 
 
/* ****************************************************************************
Functionname: SendOSD */ /*!
Description: Display OnScreen Display Data on a Bob-4H
 
@param[in]
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void SendOSD()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
//static int dx, dy, x1,y1,x2,y2;
//f32_t sinPhi_P_cosTheta_P;
//static int x1_old, y1_old, x2_old, y2_old;
static int iState = 0;
switch (iState)
{
case 0:
OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10);
iState++;
break;
case 1: /* Display the battery voltage an the RC signal level */
OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality);
iState++;
break;
case 2: /* Display the current altitude */
if (targetPosValid == 1)
{
int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude);
OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude);
}
else
{
OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude);
}
iState++;
break;
case 3: /* Draw an artificial horizon. Part 1 */
#if 0
sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P;
Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
dx = c_cos_8192(Phi45) / 128;
dy = c_sin_8192(Phi45) / 128;
x1 = 180 - dx;
y1 = 120 + 2 * Theta45 + dy;
x2 = 180 + dx;
y2 = y1 - 2 * dy;
OSDPtr = 0;
OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
#endif
iState++;
break;
case 4: /* Draw an artificial horizon. Part 2 */
#if 0
OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
UART2Print();
x1_old = x1;
y1_old = y1;
x2_old = x2;
y2_old = y2;
#endif
iState++;
break;
case 5: /* Display velocity over ground */
if (actualPos.state > 1)
{
OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100);
}
else
{
OSD_printf ("\33[0;20HVel:-.-");
}
iState++;
break;
case 6: /* Display distance from target */
if (targetPosValid == 1)
{
OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10);
}
else
{
OSD_printf ("\33[1;20HDst:-.-");
}
iState++;
break;
case 7: /* Draw an artificial horizon. Part 1 */
#if 0
sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P;
Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
dx = c_cos_8192(Phi45) / 128;
dy = c_sin_8192(Phi45) / 128;
x1 = 180 - dx;
y1 = 120 + 2 * Theta45 + dy;
x2 = 180 + dx;
y2 = y1 - 2 * dy;
OSDPtr = 0;
OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
#endif
iState++;
break;
case 8: /* Draw an artificial horizon. Part 2 */
#if 0
OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
UART2Print();
x1_old = x1;
y1_old = y1;
x2_old = x2;
y2_old = y2;
#endif
iState++;
break;
case 9: /* Display the GPS control outputs */
OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p, roll_gain_p, nick_gain_d, roll_gain_d);
iState++;
break;
case 10:
OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);
iState++;
break;
case 11:/* Draw a ^ to indicate the target direction */
{
static int LastGierOffset = 0;
int GierOffset = (TargetGier - status.iPsi10) / 10;
if (GierOffset > 180)
{
GierOffset -= 360;
}
if (GierOffset < -180)
{
GierOffset += 360;
}
GierOffset /= 14;
OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset);
LastGierOffset = GierOffset;
iState++;
break;
}
case 12: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */
if (Override == 0)
{
OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll);
}
else
{
OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll);
}
iState++;
break;
case 13:
#if 0 /* Display the Horizon */
OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");
#endif
iState=0;
break;
default:
iState = 0;
}
}
 
/branches/KalmanFilter MikeW/KalmanFc.c
0,0 → 1,1408
/*
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.
*/
 
#include "kafi.h"
#include "KalmanFc.h"
#include "main.h"
#include "mm3.h"
 
#include "main.h"
#include "eeprom.c"
 
#define MAX_GAS 250
#define MIN_GAS 15
 
#define sin45 sin(45.F / 180.F * PI)
#define cos45 cos(45.F / 180.F * PI)
 
extern void GPSupdate(void);
 
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;
int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0;
int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;
int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0;
 
f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F;
f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F;
f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta;
f32_t sinPhi, cosPhi, sinTheta, cosTheta;
f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P;
 
#ifdef USE_Extended_MM3_Measurement_Model
f32_t sinPsi_P, cosPsi_P;
extern f32_t MM3_Hx, MM3_Hy, MM3_Hz;
#endif
 
int DiffAltitude = 0, DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, NominalAltitude = 0, InitialAltitude = 0;
int KompassValue = 0, SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
int GierMischanteil;
extern int GasMischanteil;
 
extern char Notlandung;
extern unsigned long maxDistance;
 
unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
unsigned char MotorWert[5];
 
extern signed int GPS_Nick, GPS_Roll;
 
int GasMischanteil;
char MotorenEin = 0;
char Notlandung = 0;
 
unsigned char SenderOkay = 0;
unsigned int I2CTimeout = 100;
 
struct mk_param_struct EE_Parameter;
 
/* ****************************************************************************
Functionname: SendMotorData */ /*!
Description: Senden der Motorwerte per I2C-Bus
 
@return void
@pre -
@post -
@author H. Buss / I. Busker
**************************************************************************** */
void SendMotorData(void)
{
if(!MotorenEin)
{
Motor_Hinten = 0;
Motor_Vorne = 0;
Motor_Rechts = 0;
Motor_Links = 0;
if(MotorTest[0]) Motor_Vorne = MotorTest[0];
if(MotorTest[1]) Motor_Hinten = MotorTest[1];
if(MotorTest[2]) Motor_Links = MotorTest[2];
if(MotorTest[3]) Motor_Rechts = MotorTest[3];
}
 
//Start I2C Interrupt Mode
twi_state = 0;
motor = 0;
i2c_start();
}
 
 
/* ****************************************************************************
Functionname: MotorControl */ /*!
Description:
 
@return void
@pre -
@post -
@author H. Buss / I. Busker
**************************************************************************** */
void MotorControl(void)
{
static unsigned int NotGasZeit;
static unsigned char delay_neutral = 0;
static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
static unsigned int modell_fliegt = 0;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
if(GasMischanteil < 0)
{
GasMischanteil = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay < 100)
{
if(!PcZugriff)
{
if(BeepMuster == 0xffff)
{
beeptime = 15000;
BeepMuster = 0x0c00;
}
}
if(NotGasZeit)
{
NotGasZeit--;
}
else
{
MotorenEin = 0;
Notlandung = 0;
}
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMischanteil = 100; //EE_Parameter.NotGas;
Notlandung = 1;
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
}
else
{
MotorenEin = 0;
}
}
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(SenderOkay > 140)
{
Notlandung = 0;
NotGasZeit = 200 * 50; //EE_Parameter.NotGasZeit * 50;
if(GasMischanteil > 40)
{
if(modell_fliegt < 0xffff)
{
modell_fliegt++;
}
}
 
if((GasMischanteil > 200) && MotorenEin == 0)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// auf Nullwerte kalibrieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
{
if(++delay_neutral > 50) // nicht sofort
{
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750))
{
SucheLuftruckOffset();
}
}
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SetNeutral();
}
}
else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gas ist unten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(GasMischanteil < 35)
{
// Starten
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 100)
{
MotorenEin = 1;
modell_fliegt = 1;
delay_einschalten = 100;
}
}
else
{
delay_einschalten = 0;
}
//Auf Neutralwerte setzen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Auschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
{
if(++delay_ausschalten > 50) // nicht sofort
{
MotorenEin = 0;
modell_fliegt = 0;
delay_ausschalten = 50;
}
}
else
{
delay_ausschalten = 0;
}
}
}
}
 
 
/* ****************************************************************************
Functionname: SetNeutral */ /*!
Description:
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void SetNeutral(void)
{
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
{
if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750))
{
SucheLuftruckOffset();
}
}
LastAltitude = CurrentAltitude;
Delay_ms_Mess(300);
ANALOG_OFF;
AdNeutralNick = AdWertNick_Raw;
AdNeutralRoll = AdWertRoll_Raw;
AdNeutralGier = AdWertGier_Raw;
NeutralAccY = AdWertAccRoll_Raw;
NeutralAccX = AdWertAccNick_Raw;
NeutralAccZ = AdWertAccHoch_Raw;
 
Roll_X_Offset = 0.F;
Roll_Y_Offset = 0.F;
Roll_Z_Offset = 0.F;
ACC_X_Offset = 0.F;
ACC_Y_Offset = 0.F;
ACC_Z_Offset = 0.F;
 
AccumulatedACC_X = 0;
AccumulatedACC_X_cnt = 0;
AccumulatedACC_Y = 0;
AccumulatedACC_Y_cnt = 0;
AccumulatedACC_Z = 0;
AccumulatedACC_Z_cnt = 0;
AccumulatedRoll_X = 0;
AccumulatedRoll_X_cnt = 0;
AccumulatedRoll_Y = 0;
AccumulatedRoll_Y_cnt = 0;
AccumulatedRoll_Z = 0;
AccumulatedRoll_Z_cnt = 0;
AveragerACC_X = 0;
AveragerACC_Y = 0;
AveragerACC_Z = 0;
AccumulatedACC_X = 0;
AccumulatedACC_X_cnt = 0;
AccumulatedACC_Y = 0;
AccumulatedACC_Y_cnt = 0;
AccumulatedACC_Z = 0;
AccumulatedACC_Z_cnt = 0;
ANALOG_ON;
 
SummeRoll = 0;
SummeNick = 0;
 
sollGier = status.iPsi10;
InitialAltitude = AdWertAirPressure_Raw;
beeptime = 50;
}
 
 
/* ****************************************************************************
Functionname: CalculateAverage */ /*!
Description:
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void CalculateAverage(void)
{
ANALOG_OFF;
AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt;
AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt;
AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt;
 
/* Get Pressure Differenz */
CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt;
AccumulatedAirPressure_cnt = 0;
AccumulatedAirPressure = 0;
static char AirPressureCnt = 0;
if (AirPressureCnt % 25 == 1)
{
DeltaAltitude = CurrentAltitude - LastAltitude;
LastAltitude = CurrentAltitude;
}
AirPressureCnt++;
 
if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10))
{
Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_X)));
Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y)));
if (abs(StickGier) < 15 || MotorenEin == 0)
{
Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F * (float) ( MAX(-60, MIN(60,AverageRoll_Z)));
}
}
 
#if 0
DebugOut.Analog[3] = AdWertGier_Raw;
DebugOut.Analog[4] = AdWertRoll_Raw;
DebugOut.Analog[5] = AdWertNick_Raw;
#endif
AverageRoll_X -= Roll_X_Offset;
AverageRoll_Y -= Roll_Y_Offset;
AverageRoll_Z -= Roll_Z_Offset;
 
AccumulatedRoll_X = 0;
AccumulatedRoll_X_cnt = 0;
AccumulatedRoll_Y = 0;
AccumulatedRoll_Y_cnt = 0;
AccumulatedRoll_Z = 0;
AccumulatedRoll_Z_cnt = 0;
 
#if 0
ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt);
ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt);
ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt);
#endif
 
AveragerACC_X = AccumulatedACC_X / AccumulatedACC_X_cnt;
AveragerACC_Y = AccumulatedACC_Y / AccumulatedACC_Y_cnt;
AveragerACC_Z = AccumulatedACC_Z / AccumulatedACC_Z_cnt;
AccumulatedACC_X = 0;
AccumulatedACC_X_cnt = 0;
AccumulatedACC_Y = 0;
AccumulatedACC_Y_cnt = 0;
AccumulatedACC_Z = 0;
AccumulatedACC_Z_cnt = 0;
ANALOG_ON;
 
if (Roll_X_Off > 60)
{
Roll_X_Off = 60;
}
if (Roll_X_Off < -60)
{
Roll_X_Off = -60;
}
if (Roll_Y_Off > 60)
{
Roll_Y_Off = 60;
}
if (Roll_Y_Off < -60)
{
Roll_Y_Off = -60;
}
 
if (Roll_Z_Off > 60)
{
Roll_Z_Off = 60;
}
if (Roll_Z_Off < -60)
{
Roll_Z_Off = -60;
}
}
 
 
/* ****************************************************************************
Functionname: PID_Regler */ /*!
Description:
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void PID_Regler(void)
{
int StickNick45,StickRoll45;
int DiffNick,DiffRoll, DiffGier;
int motorwert = 0;
int pd_ergebnis;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData--)
{
StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
} // Ende neue Funken-Werte
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Notlandung)
{
StickGier = 0;
StickNick = 0;
StickRoll = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mischer und PID-Regler
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
/* G P S U p d a t e */
static char CntGPS = 0;
if (CntGPS % 10 == 1)
{
//GPSupdate();
}
CntGPS++;
 
StickRoll45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick));
StickNick45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick));
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(GasMischanteil < 20)
{
sollGier = status.iPsi10;
SummeRoll = 0;
SummeNick = 0;
}
/* Gier-Anteil */
if (abs(StickGier) > 4)
{
sollGier = status.iPsi10 + 4 * StickGier;
}
DiffGier = (sollGier - status.iPsi10);
GierMischanteil = (int) (-4 * DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10;
#define MUL_G 0.8
if(GierMischanteil > MUL_G * GasMischanteil)
{
GierMischanteil = MUL_G * GasMischanteil;
}
if(GierMischanteil < -MUL_G * GasMischanteil)
{
GierMischanteil = -MUL_G * GasMischanteil;
}
if(GierMischanteil > 50)
{
GierMischanteil = 50;
}
if(GierMischanteil < -50)
{
GierMischanteil = -50;
}
 
int scale_p = 7;
int scale_d = 10;
 
/* N i c k - A c h s e */
DiffNick = -(status.iTheta10 + StickNick45);
/* R o l l - A c h s e */
DiffRoll = -(status.iPhi10 + StickRoll45);
 
SummeNick += DiffNick;
if(SummeNick > 10000)
{
SummeNick = 10000;
}
if(SummeNick < -10000)
{
SummeNick = -10000;
}
 
SummeRoll += DiffRoll;
if(SummeRoll > 10000)
{
SummeRoll = 10000;
}
if(SummeRoll < -10000)
{
SummeRoll = -10000;
}
 
pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000);
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
 
/* M o t o r V o r n */
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Vorne = motorwert;
 
/* M o t o r H e c k */
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Hinten = motorwert;
 
pd_ergebnis = ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000);
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r L i n k s */
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Links = motorwert;
/* M o t o r R e c h t s */
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Rechts = motorwert;
 
#if 1
DebugOut.Analog[0] = status.iTheta10;
DebugOut.Analog[1] = status.iPhi10;
DebugOut.Analog[2] = status.iPsi10 / 10;
//DebugOut.Analog[3] = KompassValue;
//DebugOut.Analog[4] = (sollGier - status.iPsi10 ) / 10;
//DebugOut.Analog[5] = AverageRoll_Z;
//DebugOut.Analog[6] = AverageRoll_X;
 
//DebugOut.Analog[9] = CurrentAltitude;
//DebugOut.Analog[10] = DiffAltitude;
//DebugOut.Analog[8] = AverageRoll_X;
//DebugOut.Analog[9] = AverageRoll_Y;
//DebugOut.Analog[10] = AverageRoll_Z;
 
//DebugOut.Analog[11] = GasMischanteil;
//DebugOut.Analog[13] = AverageRoll_X;
//DebugOut.Analog[14] = AverageRoll_Y;
//DebugOut.Analog[15] = AdWertAirPressure_Raw;
#endif
}
 
 
/*****************************************************************************
VARIABLES
*****************************************************************************/
 
/* Kalman filter */
kafi_t *p_kafi;
mat_Matrix_t *matP0;
mat_Matrix_t *matQ;
mat_Matrix_t *matR;
mat_Matrix_t *matXd; /* estimated state */
mat_Matrix_t *matXs; /* predicted state */
mat_Matrix_t *matPhi; /* transition matrix to predict new state from current state */
mat_Matrix_t *matB; /* control matrix to predict new state from ext. control vector U */
mat_Matrix_t *matU; /* control vector */
mat_Matrix_t *matC; /* Jacobi matrix */
mat_Matrix_t *matY; /* real measurements */
mat_Matrix_t *matdY; /* innovation */
mat_Matrix_t *matYs; /* predicted measurements */
mat_Matrix_t *matP; /* error covariance matrix */
mat_Matrix_t *matPDiag; /* diagonal covariance matrix */
char *fYValid; /* measurement validity flag */
 
 
void Kafi_Init()
{
/* create kalman filter and associated matrizes */
p_kafi = KAFICreate();
KAFIInit(p_kafi);
/* get pointers to all matrices */
KAFIGetP0(p_kafi, &matP0);
KAFIGetQ(p_kafi, &matQ);
KAFIGetR(p_kafi, &matR);
KAFIGetXd(p_kafi, &matXd);
KAFIGetXs(p_kafi, &matXs);
KAFIGetPhi(p_kafi, &matPhi);
KAFIGetB(p_kafi, &matB);
KAFIGetU(p_kafi, &matU);
KAFIGetC(p_kafi, &matC);
KAFIGetY(p_kafi, &matY);
KAFIGetYs(p_kafi, &matYs);
KAFIGetPDiag(p_kafi, &matPDiag);
KAFIGetYValid(p_kafi, &fYValid);
KAFIGetdY(p_kafi,&matdY);
trResetKalmanFilter();
}
 
/* ****************************************************************************
Functionname: trResetKalmanFilter */ /*!
Description:
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
 
void trResetKalmanFilter()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
ui32_t i, j;
/* set phi to identity matrix */
for ( j = 0; j < p_kafi->uiDimX; j++ )
{
for ( i = 0; i < p_kafi->uiDimX; i++ )
{
matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F);
}
}
/* set diagonal of measurement noise covariance R */
matSetDiagonal(matR, _ax, _ax, 10.0F);
matSetDiagonal(matR, _ay, _ay, 10.0F);
matSetDiagonal(matR, _az, _az, 10.0F);
#ifdef USE_Extended_MM3_Measurement_Model
matSetDiagonal(matR, _mx, _mx, 100.0F);
matSetDiagonal(matR, _my, _my, 100.0F);
matSetDiagonal(matR, _mz, _mz, 100.0F);
#else
matSetDiagonal(matR, _compass, _compass, 5.0F);
#endif
/* set diagonal of system noise covariance Q */
matSetDiagonal(matQ, _Phi, _Phi, 0.00001F);
matSetDiagonal(matQ, _Theta, _Theta, 0.00001F);
matSetDiagonal(matQ, _Psi, _Psi, 0.00005F);
/* set diagonal init. estimation error (covariance) P0 */
matSetDiagonal(matP0, _Phi, _Phi_, 0.00001F);
matSetDiagonal(matP0, _Theta, _Theta, 0.00001F);
matSetDiagonal(matP0, _Psi, _Psi, 0.00001F);
matSetFull(matXd, _Phi, 0, 0.0F);
matSetFull(matXd, _Theta, 0, 0.0F);
matSetFull(matXd, _Psi, 0, 0.0F);
matSetFull(matXs, _Phi, 0, 0.0F);
matSetFull(matXs, _Theta, 0, 0.0F);
matSetFull(matXs, _Psi, 0, 0.0F);
memset( &status, 0, sizeof(status) );
KAFISetP0(p_kafi);
}
 
 
 
 
/* ****************************************************************************
Functionname: trInnovate */ /*!
Description: Update the current System State based on the current Observation
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void trInnovate()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
/*estimate the new system state (Xd), nominal case */
KAFIInnovation(p_kafi);
KAFIUpdatePDiag(p_kafi);
KAFIUpdateP(p_kafi);
return;
}
 
 
 
/* ****************************************************************************
Functionname: trUpdatePhi */ /*!
Description: Setup matPhi, used to calculate the predicted state
from the current state
 
@param[in]
@param[in] fCycleTime
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void trUpdatePhi()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
matSetFull(matPhi, _Phi, _Phi, 1.0F);
matSetFull(matPhi, _Theta, _Theta, 1.0F);
matSetFull(matPhi, _Psi, _Psi, 1.0F);
}
 
 
 
#ifdef USE_Extended_MM3_Measurement_Model
/* ****************************************************************************
Functionname: trUpdateJacobiMatrix */ /*!
Description:
 
@param[in] void
@return void
@pre -
@post -
@author Michael Walter
*****************************************************************************/
void trUpdateJacobiMatrix(void)
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*
Simplified Version
Pre_ax = du - sinTheta * g;
Pre_ay = dv + cosTheta_sinPhi * g;
Pre_az = dw + cosTheta_cosPhi * g;
*/
/*--- VARIABLES ---*/
f32_t Phi, Theta, Psi;
f32_t sinPsi, cosPsi;
 
f32_t g = 9.81F;
f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */
Phi = status.Phi;
Theta = status.Theta;
Psi = status.Psi;
sinPhi = sin(Phi);
cosPhi = cos(Phi);
sinTheta = sin(Theta);
cosTheta = cos(Theta);
sinPsi = sin(Psi);
cosPsi = cos(Psi);
//Pre_ax = du - sinTheta * g;
//matSetFull(matC, _ax, _Phi , 0);
matSetFull(matC, _ax, _Theta, -cosTheta * g);
//matSetFull(matC, _ax, _Psi , 0);
//Pre_ay = dv + cosTheta_sinPhi * g;
matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
//matSetFull(matC, _ay, _Psi , 0);
//Pre_az = dw + cosTheta_cosPhi * g;
matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g);
matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
//matSetFull(matC, _az, _Psi , 0);
//Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
//matSetFull(matC, _mx, _Phi , (sin45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi) )* Pre_Hx + ( sin45 * cosPhi * cosTheta) * Pre_Hz );
//matSetFull(matC, _mx, _Theta, (cos45 * (-sinTheta * cosPsi) + sin45 * (sinPhi * cosTheta * cosPsi) )* Pre_Hx + (-cos45 * cosTheta - sin45 * sinPhi * sinTheta) * Pre_Hz);
//matSetFull(matC, _mx, _Psi , (-cos45 * (cosTheta * sinPsi) + sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi) )* Pre_Hx );
 
//Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz;
//matSetFull(matC, _my, _Phi , (cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi)) * Pre_Hx + ( cos45 * cosPhi * cosTheta) * Pre_Hz );
//matSetFull(matC, _my, _Theta, (sin45 * (sinTheta * cosPsi) + cos45 * (sinPhi * cosTheta * cosPsi)) * Pre_Hx + (sin45 * cosTheta - cos45 * sinPhi * sinTheta) * Pre_Hz );
//matSetFull(matC, _my, _Psi , (sin45 * (cosTheta * sinPsi) + cos45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi)) * Pre_Hx );
 
//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
//matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz);
//matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz );
//matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx );
 
/* Runtime Optimised Version */
f32_t cos45_sinTheta_cosPsi = cos45 * (sinTheta * cosPsi) ;
f32_t sin45_sinPhi_sinTheta = sin45 * sinPhi * sinTheta;
f32_t sin45_cosPhi_cosTheta = sin45 * cosPhi * cosTheta;
f32_t cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi = cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi);
f32_t cos45_cosTheta_sinPsi = cos45 * (cosTheta * sinPsi) ;
f32_t sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi = sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi);
f32_t sin45_sinPhi_cosTheta_cosPsi = sin45 * (sinPhi * cosTheta * cosPsi);
f32_t tmp1= (cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi)* Pre_Hx + (sin45_cosPhi_cosTheta) * Pre_Hz;
 
//Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz;
matSetFull(matC, _mx, _Phi , tmp1);
matSetFull(matC, _mx, _Theta, (-cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi)* Pre_Hx + (-cos45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz);
matSetFull(matC, _mx, _Psi , (-cos45_cosTheta_sinPsi+ sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi)* Pre_Hx );
 
//Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz;
matSetFull(matC, _my, _Phi , tmp1);
matSetFull(matC, _my, _Theta, (cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi) * Pre_Hx + (sin45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz );
matSetFull(matC, _my, _Psi , (cos45_cosTheta_sinPsi + sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi) * Pre_Hx );
 
//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz);
matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz );
matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx );
}
#else
/* ****************************************************************************
Functionname: trUpdateJacobiMatrix */ /*!
Description:
 
@param[in] void
@return void
@pre -
@post -
@author Michael Walter
*****************************************************************************/
void trUpdateJacobiMatrix(void)
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*
Simplified Version
Pre_ax = du - sinTheta * g;
Pre_ay = dv + cosTheta_sinPhi * g;
Pre_az = dw + cosTheta_cosPhi * g;
*/
/*--- VARIABLES ---*/
f32_t Phi, Theta, Psi;
f32_t g = 9.81F;
Phi = status.Phi;
Theta = status.Theta;
Psi = status.Psi;
sinPhi = sin(Phi);
cosPhi = cos(Phi);
sinTheta = sin(Theta);
cosTheta = cos(Theta);
//Pre_ax = du - sinTheta * g;
//matSetFull(matC, _ax, _Phi , 0);
matSetFull(matC, _ax, _Theta, -cosTheta * g);
//matSetFull(matC, _ax, _Psi , 0);
//Pre_ay = dv + cosTheta_sinPhi * g;
matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g);
matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g);
//matSetFull(matC, _ay, _Psi , 0);
//Pre_az = dw + cosTheta_cosPhi * g;
matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g);
matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g);
//matSetFull(matC, _az, _Psi , 0);
matSetFull(matC, _compass, _Psi , 1.0F);
}
#endif
 
 
#ifdef USE_Extended_MM3_Measurement_Model
/*****************************************************************************
Functionname: trPredictMeasurement */ /*!
Description: Predict Measurements: AX, AY, AZ, HX, HY, HZ
 
@param[in] void
@return void
@pre -
@post -
@author Michael Walter
*****************************************************************************/
void trPredictMeasurement(void)
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*
Simplified Version
Pre_ax = du - sinTheta * g;
Pre_ay = dv + cosTheta_sinPhi * g;
Pre_az = dw + cosTheta_cosPhi * g;
*/
/*--- VARIABLES ---*/
f32_t g = 9.81F;
f32_t Pre_ax, Pre_ay, Pre_az;
f32_t Phi, Theta, Psi;
 
f32_t Pre_mx;
f32_t Pre_my;
f32_t Pre_mz;
 
f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */
f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */
 
f32_t cosTheta_cosPsi, sinPhi_cosTheta, cosPhi_sinPsi, sinPhi_sinTheta_cosPsi, cos45_cosTheta_cosPsi;
f32_t cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi, cos45_sinPhi_cosTheta, cos45_sinTheta;
matGetFull(matXs, _Phi, 0, &Phi);
matGetFull(matXs, _Theta, 0, &Theta);
matGetFull(matXs, _Psi, 0, &Psi);
sinPhi_P = sin(Phi);
cosPhi_P = cos(Phi);
sinTheta_P = sin(Theta);
cosTheta_P = cos(Theta);
sinPsi_P = sin(Psi);
cosPsi_P = cos(Psi);
 
cosTheta_cosPsi = cosTheta_P * cosPsi_P;
sinPhi_cosTheta = sinPhi_P * cosTheta_P;
cosPhi_sinPsi = cosPhi_P * sinPsi_P;
sinPhi_sinTheta_cosPsi = sinPhi_P * sinTheta_P * cosPsi_P;
 
cos45_cosTheta_cosPsi = cos45 * (cosTheta_cosPsi);
cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi = cos45 * (-cosPhi_sinPsi + sinPhi_sinTheta_cosPsi);
cos45_sinPhi_cosTheta = cos45 * sinPhi_cosTheta;
cos45_sinTheta = cos45 * sinTheta_P;
 
/* Simplified Version */
Pre_ax = -sinTheta_P * g;
Pre_ay = cosTheta_P * sinPhi_P * g;
Pre_az = cosTheta_P * cosPhi_P * g;
matSetFull(matYs, _ax, 0, Pre_ax);
matSetFull(matYs, _ay, 0, Pre_ay);
matSetFull(matYs, _az, 0, Pre_az);
//Pre_mx = (cos45 * (cosTheta_P * cosPsi_P) + sin45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P) )* Pre_Hx + (-cos45 * sinTheta_P + sin45 * sinPhi_P * cosTheta_P) * Pre_Hz;
//Pre_my = (-sin45 * (cosTheta_P * cosPsi_P) + cos45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P)) * Pre_Hx + (sin45 * sinTheta_P + cos45 * sinPhi_P * cosTheta_P) * Pre_Hz;
//Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
 
Pre_mx = (cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi)* Pre_Hx + (-cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
Pre_my = (-cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi) * Pre_Hx + (cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz;
Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz;
 
//DebugOut.Analog[3] = Pre_mx * 100;
//DebugOut.Analog[4] = Pre_my * 100;
//DebugOut.Analog[5] = Pre_mz * 100;
matSetFull(matYs, _mx, 0, Pre_mx);
matSetFull(matYs, _my, 0, Pre_my);
matSetFull(matYs, _mz, 0, Pre_mz);
}
 
#else
/*****************************************************************************
Functionname: trPredictMeasurement */ /*!
Description: Predict Measurements: AX, AY, AZ, Compass
 
@param[in] void
@return void
@pre -
@post -
@author Michael Walter
*****************************************************************************/
void trPredictMeasurement(void)
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*
Simplified Version
Pre_ax = du - sinTheta * g;
Pre_ay = dv + cosTheta_sinPhi * g;
Pre_az = dw + cosTheta_cosPhi * g;
*/
/*--- VARIABLES ---*/
f32_t g = 9.81F;
f32_t Pre_ax, Pre_ay, Pre_az;
f32_t Phi, Theta, Psi;
matGetFull(matXs, _Phi, 0, &Phi);
matGetFull(matXs, _Theta, 0, &Theta);
matGetFull(matXs, _Psi, 0, &Psi);
sinPhi_P = sin(Phi);
cosPhi_P = cos(Phi);
sinTheta_P = sin(Theta);
cosTheta_P = cos(Theta);
/* Simplified Version */
Pre_ax = -sinTheta_P * g;
Pre_ay = cosTheta_P * sinPhi_P * g;
Pre_az = cosTheta_P * cosPhi_P * g;
matSetFull(matYs, _ax, 0, Pre_ax);
matSetFull(matYs, _ay, 0, Pre_ay);
matSetFull(matYs, _az, 0, Pre_az);
matSetFull(matYs, _compass, 0, Psi);
}
#endif
 
#ifdef USE_Extended_MM3_Measurement_Model
/* ****************************************************************************
Functionname: trMeasure */ /*!
Description:
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void trMeasure()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
 
f32_t ADC_ax = 0.0F;
f32_t ADC_ay = 0.0F;
f32_t ADC_az = 0.0F;
fYValid[_ax] = 1;
fYValid[_ay] = 1;
fYValid[_az] = 1;
 
ADC_ax = AdWertAccNick * 0.047F;
ADC_ay = -AdWertAccRoll * 0.047F;
ADC_az = AdWertAccHoch * 0.047F;
 
matSetFull(matY, _ax, 0, ADC_ax);
matSetFull(matY, _ay, 0, ADC_ay);
matSetFull(matY, _az, 0, ADC_az);
//if (MM3_Ready == 1)
if (1)
{
MM3_Heading();
matSetFull(matY, _mx, 0, MM3_Hx);
matSetFull(matY, _my, 0, MM3_Hy);
matSetFull(matY, _mz, 0, MM3_Hz);
 
fYValid[_mx] = 1;
fYValid[_my] = 1;
fYValid[_mz] = 1;
//MM3_Ready = 0;
DebugOut.Analog[13] = MM3_Hx * 100;
DebugOut.Analog[14] = MM3_Hy * 100;
DebugOut.Analog[15] = MM3_Hz * 100;
}
else
{
fYValid[_mx] = 0;
fYValid[_my] = 0;
fYValid[_mz] = 0;
}
}
#else
/* ****************************************************************************
Functionname: trMeasure */ /*!
Description:
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void trMeasure()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
 
f32_t ADC_ax = 0.0F;
f32_t ADC_ay = 0.0F;
f32_t ADC_az = 0.0F;
fYValid[_ax] = 1;
fYValid[_ay] = 1;
fYValid[_az] = 1;
 
ADC_ax = AdWertAccNick * 0.047F;
ADC_ay = -AdWertAccRoll * 0.047F;
ADC_az = AdWertAccHoch * 0.047F;
 
matSetFull(matY, _ax, 0, ADC_ax);
matSetFull(matY, _ay, 0, ADC_ay);
matSetFull(matY, _az, 0, ADC_az);
static uint8_t updCompass = 0;
f32_t Psi, Compass;
if (!updCompass--)
{
updCompass = 10;
KompassValue = MM3_Heading();
}
Compass = KompassValue / 180.F * PI;
matGetFull(matXs, _Psi, 0, &Psi);
 
if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi))
{
Compass += 2 * PI;
}
else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi))
{
Compass -= 2 * PI;
}
matSetFull(matY, _compass, 0, Compass);
 
/* Roll and Yaw angle are smaller 8 Deg*/
if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80))
{
fYValid[_compass] = 1;
}
else
{
fYValid[_compass] = 0;
}
}
#endif
 
/* ****************************************************************************
Functionname: trUpdateBU */ /*!
Description: Update control matrix B and vector U
 
@param[in]
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void trUpdateBU()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
/*--- VARIABLES ---*/
/*
dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p
dTheta = | 0 cosPhi -sinPhi | * q
dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w
*/
f32_t ADC_p = 0.0F;
f32_t ADC_q = 0.0F;
f32_t ADC_r = 0.0F;
/* store predicted state vector in current state */
matGetFull(matXd, _Phi , 0, &Phi);
matGetFull(matXd, _Theta , 0, &Theta);
sinPhi = sin(Phi);
cosPhi = cos(Phi);
tanTheta = tan(Theta);
secTheta = 1.0F / cos(Theta);
matSetFull(matB, _Phi, _p, 1.0F);
matSetFull(matB, _Phi, _q, sinPhi * tanTheta );
matSetFull(matB, _Phi, _r, cosPhi * tanTheta );
//matSetFull(matB, _Theta, _p, 0.0F);
matSetFull(matB, _Theta, _q, cosPhi );
matSetFull(matB, _Theta, _r, -sinPhi );
//matSetFull(matB, _Psi, _p, 0.0F);
matSetFull(matB, _Psi, _q, sinPhi * secTheta );
matSetFull(matB, _Psi, _r, cosPhi * secTheta );
 
ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime;
ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime;
ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime;
matSetFull(matU, _p, 0, ADC_p );
matSetFull(matU, _q, 0, ADC_q);
matSetFull(matU, _r, 0, ADC_r);
}
 
 
void AttitudeEstimation()
{
trPredictMeasurement();
trUpdateJacobiMatrix();
/* Extract measurements and store them in vector matY.
Measurement validity is indicated by fYValid[] */
trMeasure();
/* Innovation: calculate Xd, and Pd */
trInnovate();
/* Update transition matrix Phi */
trUpdatePhi();
/* Update control matrix B */
trUpdateBU();
/* Predict new state Xs and Ps */
KAFIPrediction(p_kafi);
/* store innovated state vector in current state */
matGetFull(matXs, _Phi , 0, &status.Phi);
matGetFull(matXs, _Theta , 0, &status.Theta);
matGetFull(matXs, _Psi , 0, &status.Psi);
trLimitAngles();
}
 
 
 
/* ****************************************************************************
Functionname: trEstimateVelocity */ /*!
Description:
 
@param[in]
 
@return void
@pre -
@post -
@author
**************************************************************************** */
void trEstimateVelocity()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
 
/*--- VARIABLES ---*/
}
 
 
/* ****************************************************************************
Functionname: trLimitAngles */ /*!
Description:
 
@param[in]
 
@return void
@pre -
@post -
@author
**************************************************************************** */
void trLimitAngles()
{
/*--- (SYMBOLIC) CONSTANTS ---*/
 
/*--- VARIABLES ---*/
f32_t Psi;
 
if (status.Phi > 2.0F * PI)
{
status.Phi -= 2.0F * PI;
matSetFull(matXs, _Phi, 0, status.Phi);
}
if (status.Phi < -2.0F * PI)
{
status.Phi += 2.0F * PI;
matSetFull(matXs, _Phi, 0, status.Phi);
}
if (status.Theta > 2.0F * PI)
{
status.Theta -= 2.0F * PI;
matSetFull(matXs, _Theta, 0, status.Theta);
}
if (status.Theta < -2.0F * PI)
{
status.Theta += 2.0F * PI;
matSetFull(matXs, _Theta, 0, status.Theta);
}
if (status.Psi > 2.0F * PI)
{
status.Psi -= 2.0F * PI;
sollGier -= 3600;
matGetFull(matXs, _Psi, 0, &Psi);
matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI);
matGetFull(matXd, _Psi, 0, &Psi);
matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI);
}
if (status.Psi < 0.0F * PI)
{
status.Psi += 2.0F * PI;
sollGier += 3600;
matGetFull(matXs, _Psi, 0, &Psi);
matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI);
matGetFull(matXd, _Psi, 0, &Psi);
matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI);
}
status.iPhi10 = (int) (status.Phi * 573.0F);
status.iTheta10 = (int) (status.Theta * 573.0F);
status.iPsi10 = (int) (status.Psi * 573.0F);
if ((sollGier - status.iPsi10) > 3600)
{
sollGier -= 3600;
}
 
if ((sollGier - status.iPsi10) < -3600)
{
sollGier += 3600;
}
}
 
 
/branches/KalmanFilter MikeW/KalmanFc.h
0,0 → 1,38
/*
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.
*/
 
extern int MesswertNick,MesswertRoll,MesswertGier;
extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier;
extern int NeutralAccX, NeutralAccY, NeutralAccZ;
extern int GierMischanteil,GasMischanteil;
extern int AdWertNick_Raw, AdWertRoll_Raw, AdWertGier_Raw;
extern int AdWertAccHoch_Raw, AdWertAccRoll_Raw,AdWertAccNick_Raw, AdWertAccHoch_Raw;
extern int AccumulatedACC_X, AccumulatedACC_Y, AccumulatedACC_Z;
extern int AccumulatedACC_X_cnt, AccumulatedACC_Y_cnt, AccumulatedACC_Z_cnt;
extern int AccumulatedRoll_X, AccumulatedRoll_Y, AccumulatedRoll_Z, AccumulatedAirPressure;
extern int AccumulatedRoll_X_cnt, AccumulatedRoll_Y_cnt, AccumulatedRoll_Z_cnt, AccumulatedAirPressure_cnt;
extern unsigned int AdWertAirPressure_Raw;
 
extern int DiffNick,DiffRoll;
extern unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
extern unsigned char MotorWert[5];
extern unsigned char SenderOkay;
extern int StickNick,StickRoll,StickGier;
 
void CalculateAverage(void);
void GetRadioValues(void);
void SendMotorData(void);
void PID_Regler(void);
void SetNeutral(void);
/branches/KalmanFilter MikeW/Settings.h
--- _Settings.h (nonexistent)
+++ _Settings.h (revision 838)
@@ -0,0 +1,18 @@
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Debug-Interface
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#define SIO_DEBUG 1 // Soll der Debugger aktiviert sein?
+#define MIN_DEBUG_INTERVALL 500 // in diesem Intervall werden Degugdaten ohne Aufforderung gesendet
+
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Sender
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ #define K_NICK 0
+ #define K_ROLL 1
+ #define K_GAS 2
+ #define K_GIER 3
+ #define K_POTI1 4
+ #define K_POTI2 5
+ #define K_POTI3 6
+ #define K_POTI4 7
+
/branches/KalmanFilter MikeW/analog.c
0,0 → 1,150
/*
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.
*/
 
#include "main.h"
#include "KalmanFc.h"
 
int UBat = 100;
int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;
 
int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0;
int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0;
 
unsigned int AdWertAirPressure_Raw = 1023;
 
/* ****************************************************************************
Functionname: ADC_Init */ /*!
Description:
 
@return void
@pre -
@post -
@author H. Buss / I. Busker
**************************************************************************** */
void ADC_Init(void)
{
ADMUX = 0;//Referenz ist extern
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
//Free Running Mode, Division Factor 128, Interrupt on
}
 
/* ****************************************************************************
Functionname: SucheLuftruckOffset */ /*!
Description:
 
@return void
@pre -
@post -
@author H. Buss / I. Busker
**************************************************************************** */
void SucheLuftruckOffset(void)
{
unsigned int off;
off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
if(off > 20) off -= 10;
OCR0A = off;
Delay_ms_Mess(100);
if(AdWertAirPressure_Raw < 850) off = 0;
for(; off < 250;off++)
{
OCR0A = off;
Delay_ms_Mess(50);
printf(".");
if(AdWertAirPressure_Raw < 900) break;
}
eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
Delay_ms_Mess(300);
}
 
/* ****************************************************************************
Functionname: SIGNAL */ /*!
Description:
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
SIGNAL(SIG_ADC)
{
static unsigned char kanal=0,state = 0;
ANALOG_OFF;
switch(state++)
{
case 0:
AdWertGier = ADC;
AdWertGier_Raw = AdWertGier;
AccumulatedRoll_Z += (ADC - AdNeutralGier);
AccumulatedRoll_Z_cnt++;
kanal = 1;
break;
case 1:
AdWertRoll = ADC;
AdWertRoll_Raw = AdWertRoll;
AccumulatedRoll_X += (ADC - AdNeutralRoll);
AccumulatedRoll_X_cnt++;
kanal = 2;
break;
case 2:
AdWertNick = ADC;
AdWertNick_Raw = AdWertNick;
AccumulatedRoll_Y += (ADC - AdNeutralNick);
AccumulatedRoll_Y_cnt++;
kanal = 4;
break;
case 3:
UBat = (3 * UBat + ADC / 3) / 4;
kanal = 6;
break;
case 4:
AdWertAccRoll = NeutralAccY - ADC;
AdWertAccRoll_Raw = ADC;
AccumulatedACC_Y += (NeutralAccY - ADC);
AccumulatedACC_Y_cnt++;
kanal = 7;
break;
case 5:
AdWertAccNick = ADC - NeutralAccX;
AdWertAccNick_Raw = ADC;
AccumulatedACC_X += (ADC - NeutralAccX);
AccumulatedACC_X_cnt++;
kanal = 5;
break;
case 6:
AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
AdWertAccHoch_Raw = ADC;
AccumulatedACC_Z += (ADC - NeutralAccZ);
AccumulatedACC_Z_cnt++;
kanal = 3;
break;
case 7:
AdWertAirPressure_Raw = ADC;
AccumulatedAirPressure += ADC;
AccumulatedAirPressure_cnt++;
kanal = 0;
state = 0;
break;
default:
kanal = 0;
state = 0;
break;
}
ADMUX = kanal;
ANALOG_ON;
}
/branches/KalmanFilter MikeW/analog.h
0,0 → 1,10
extern int UBat;
extern int AdWertNick, AdWertRoll, AdWertGier;
extern int AdWertAccRoll,AdWertAccNick,AdWertAccHoch;
extern int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az;
 
void ADC_Init(void);
void SucheLuftruckOffset(void);
 
#define ANALOG_OFF ADCSRA=0
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE)
/branches/KalmanFilter MikeW/eeprom.c
0,0 → 1,164
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Konstanten
// + 0-250 -> normale Werte
// + 251 -> Poti1
// + 252 -> Poti2
// + 253 -> Poti3
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void DefaultKonstanten1(void)
{
EE_Parameter.Kanalbelegung[K_NICK] = 1;
EE_Parameter.Kanalbelegung[K_ROLL] = 2;
EE_Parameter.Kanalbelegung[K_GAS] = 3;
EE_Parameter.Kanalbelegung[K_GIER] = 4;
EE_Parameter.Kanalbelegung[K_POTI1] = 5;
EE_Parameter.Kanalbelegung[K_POTI2] = 6;
EE_Parameter.Kanalbelegung[K_POTI3] = 7;
EE_Parameter.Kanalbelegung[K_POTI4] = 8;
EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1
EE_Parameter.Hoehe_P = 10; // Wert : 0-32
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50
EE_Parameter.Stick_P = 4; //2 // Wert : 1-6
EE_Parameter.Stick_D = 8; //8 // Wert : 0-64
EE_Parameter.Gier_P = 14; // Wert : 1-20
EE_Parameter.Gas_Min = 15; // Wert : 0-32
EE_Parameter.Gas_Max = 250; // Wert : 33-250
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.KompassWirkung = 128; // Wert : 0-250
EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250
EE_Parameter.Gyro_I = 150; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.I_Faktor = 0;
EE_Parameter.UserParam1 = 0; //zur freien Verwendung
EE_Parameter.UserParam2 = 0; //zur freien Verwendung
EE_Parameter.UserParam3 = 0; //zur freien Verwendung
EE_Parameter.UserParam4 = 0; //zur freien Verwendung
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickRefresh = 5;
EE_Parameter.LoopGasLimit = 50;
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag
EE_Parameter.LoopHysterese = 50;
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
EE_Parameter.AchsKopplung1 = 100;
EE_Parameter.AchsGegenKopplung1 = 70;
EE_Parameter.WinkelUmschlagNick = 100;
EE_Parameter.WinkelUmschlagRoll = 100;
EE_Parameter.GyroAccAbgleich = 10; // 1/k
memcpy(EE_Parameter.Name, "Sport\0", 12);
}
void DefaultKonstanten2(void)
{
EE_Parameter.Kanalbelegung[K_NICK] = 1;
EE_Parameter.Kanalbelegung[K_ROLL] = 2;
EE_Parameter.Kanalbelegung[K_GAS] = 3;
EE_Parameter.Kanalbelegung[K_GIER] = 4;
EE_Parameter.Kanalbelegung[K_POTI1] = 5;
EE_Parameter.Kanalbelegung[K_POTI2] = 6;
EE_Parameter.Kanalbelegung[K_POTI3] = 7;
EE_Parameter.Kanalbelegung[K_POTI4] = 8;
EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1
EE_Parameter.Hoehe_P = 10; // Wert : 0-32
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
EE_Parameter.Stick_P = 4; //2 // Wert : 1-6
EE_Parameter.Stick_D = 0; //8 // Wert : 0-64
EE_Parameter.Gier_P = 10; // Wert : 1-20
EE_Parameter.Gas_Min = 15; // Wert : 0-32
EE_Parameter.Gas_Max = 250; // Wert : 33-250
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.KompassWirkung = 128; // Wert : 0-250
EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250
EE_Parameter.Gyro_I = 175; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.I_Faktor = 0;
EE_Parameter.UserParam1 = 0; // zur freien Verwendung
EE_Parameter.UserParam2 = 0; // zur freien Verwendung
EE_Parameter.UserParam3 = 0; // zur freien Verwendung
EE_Parameter.UserParam4 = 0; // zur freien Verwendung
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickRefresh = 5;
EE_Parameter.LoopGasLimit = 50;
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag
EE_Parameter.LoopHysterese = 50;
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
EE_Parameter.AchsKopplung1 = 100; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt
EE_Parameter.AchsGegenKopplung1 = 80;
EE_Parameter.WinkelUmschlagNick = 100;
EE_Parameter.WinkelUmschlagRoll = 100;
EE_Parameter.GyroAccAbgleich = 16; // 1/k
memcpy(EE_Parameter.Name, "Kamera\0", 12);
}
 
void DefaultKonstanten3(void)
{
EE_Parameter.Kanalbelegung[K_NICK] = 1;
EE_Parameter.Kanalbelegung[K_ROLL] = 2;
EE_Parameter.Kanalbelegung[K_GAS] = 3;
EE_Parameter.Kanalbelegung[K_GIER] = 4;
EE_Parameter.Kanalbelegung[K_POTI1] = 5;
EE_Parameter.Kanalbelegung[K_POTI2] = 6;
EE_Parameter.Kanalbelegung[K_POTI3] = 7;
EE_Parameter.Kanalbelegung[K_POTI4] = 8;
EE_Parameter.GlobalConfig = CFG_HOEHENREGELUNG | CFG_DREHRATEN_BEGRENZER | CFG_ACHSENKOPPLUNG_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 100; // Wert : 0-250 251 -> Poti1
EE_Parameter.Hoehe_P = 10; // Wert : 0-32
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
EE_Parameter.Stick_P = 3; //2 // Wert : 1-6
EE_Parameter.Stick_D = 0; //8 // Wert : 0-64
EE_Parameter.Gier_P = 8; // Wert : 1-20
EE_Parameter.Gas_Min = 15; // Wert : 0-32
EE_Parameter.Gas_Max = 250; // Wert : 33-250
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64
EE_Parameter.KompassWirkung = 128; // Wert : 0-250
EE_Parameter.Gyro_P = 200; //80 // Wert : 0-250
EE_Parameter.Gyro_I = 175; // Wert : 0-250
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation
EE_Parameter.I_Faktor = 0;
EE_Parameter.UserParam1 = 0; // zur freien Verwendung
EE_Parameter.UserParam2 = 0; // zur freien Verwendung
EE_Parameter.UserParam3 = 0; // zur freien Verwendung
EE_Parameter.UserParam4 = 0; // zur freien Verwendung
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag
EE_Parameter.ServoNickRefresh = 5;
EE_Parameter.LoopGasLimit = 50;
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag
EE_Parameter.LoopHysterese = 50;
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
EE_Parameter.AchsKopplung1 = 100; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt
EE_Parameter.AchsGegenKopplung1 = 80;
EE_Parameter.WinkelUmschlagNick = 100;
EE_Parameter.WinkelUmschlagRoll = 100;
EE_Parameter.GyroAccAbgleich = 16; // 1/k
memcpy(EE_Parameter.Name, "Beginner\0", 12);
}
/branches/KalmanFilter MikeW/fc.c
0,0 → 1,12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
 
 
 
 
/branches/KalmanFilter MikeW/fc.h
0,0 → 1,76
/*#######################################################################################
Flight Control
#######################################################################################*/
 
#ifndef _FC_H
#define _FC_H
extern unsigned int I2CTimeout;
 
void Piep(unsigned char Anzahl);
 
 
extern unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
extern unsigned char MotorWert[5];
extern unsigned char SenderOkay;
extern int StickNick,StickRoll,StickGier;
extern char MotorenEin;
extern void DefaultKonstanten1(void);
extern void DefaultKonstanten2(void);
extern void DefaultKonstanten3(void);
 
#define STRUCT_PARAM_LAENGE 65
struct mk_param_struct
{
unsigned char Kanalbelegung[8]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3
unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv
unsigned char Hoehe_MinGas; // Wert : 0-100
unsigned char Luftdruck_D; // Wert : 0-250
unsigned char MaxHoehe; // Wert : 0-32
unsigned char Hoehe_P; // Wert : 0-32
unsigned char Hoehe_Verstaerkung; // Wert : 0-50
unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250
unsigned char Stick_P; // Wert : 1-6
unsigned char Stick_D; // Wert : 0-64
unsigned char Gier_P; // Wert : 1-20
unsigned char Gas_Min; // Wert : 0-32
unsigned char Gas_Max; // Wert : 33-250
unsigned char GyroAccFaktor; // Wert : 1-64
unsigned char KompassWirkung; // Wert : 0-32
unsigned char Gyro_P; // Wert : 10-250
unsigned char Gyro_I; // Wert : 0-250
unsigned char UnterspannungsWarnung; // Wert : 0-250
unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust
unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen
unsigned char UfoAusrichtung; // X oder + Formation
unsigned char I_Faktor; // Wert : 0-250
unsigned char UserParam1; // Wert : 0-250
unsigned char UserParam2; // Wert : 0-250
unsigned char UserParam3; // Wert : 0-250
unsigned char UserParam4; // Wert : 0-250
unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag
unsigned char ServoNickRefresh; //
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag
unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag
unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung)
unsigned char AchsGegenKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick Gegenkoppelt (NickRollGegenkopplung)
unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt
unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt
unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung)
unsigned char Driftkomp;
//------------------------------------------------
unsigned char LoopConfig; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
unsigned char ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen
unsigned char Reserved[4];
char Name[12];
};
 
 
extern struct mk_param_struct EE_Parameter;
#endif //_FC_H
 
/branches/KalmanFilter MikeW/flight.pnproj
0,0 → 1,0
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="menu.h"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="GPS.c"></File><File path="gps.h"></File><File path="eeprom.c"></File><File path="kafi.c"></File><File path="kafi.h"></File><File path="mat.h"></File><File path="matmatrix.c"></File><File path="mymath.c"></File><File path="mm3.h"></File><File path="mm3.c"></File><File path="KalmanFc.h"></File><File path="KalmanFc.c"></File><File path="main.c"></File><File path="Bob4_OSD.c"></File></Project>
/branches/KalmanFilter MikeW/flight.pnps
0,0 → 1,0
<pd><ViewState><e p="Flight-Ctrl" x="true"></e></ViewState></pd>
/branches/KalmanFilter MikeW/gps.c
0,0 → 1,547
/*
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.
*/
 
#include "main.h"
#include "kafi.h"
 
#include "mymath.h"
#include <math.h>
 
int GPSTracking = 0;
int targetPosValid = 0;
int homePosValid = 0;
uint8_t gpsState;
gpsInfo_t actualPos; // measured position (last gps record)
gpsInfo_t targetPos; // measured position (last gps record)
gpsInfo_t homePos; // measured position (last gps record)
 
NAV_STATUS_t navStatus;
NAV_POSLLH_t navPosLlh;
NAV_POSUTM_t navPosUtm;
NAV_VELNED_t navVelNed;
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
long distanceNS = 0;
long distanceEW = 0;
long velocityNS = 0;
long velocityEW = 0;
unsigned long maxDistance = 0;
 
int roll_gain = 0;
int nick_gain = 0;
int nick_gain_p = 0;
int nick_gain_d = 0;
int roll_gain_p = 0;
int roll_gain_d = 0;
int Override = 0;
int TargetGier = 0;
extern int sollGier;
char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered
uint8_t CK_A, CK_B; // UBX checksum bytes
uint8_t ignorePacket; // true when previous packet was not processed
unsigned short msgLen;
uint8_t msgID;
 
void GPSscanData (void);
void GPSupdate(void);
 
 
/* ****************************************************************************
Functionname: GPSupdate */ /*!
Description:
 
@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void GPSupdate(void)
{
float SIN_H, COS_H;
long max_p = 0;
long max_d = 0;
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 ||
abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 ||
abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 15)
{
Override = 1;
}
else
{
Override = 0;
}
 
/* Set Home Position */
if ((actualPos.state > 2) &&
(homePosValid == 0))
{
/* Save Current Position */
homePos.north = actualPos.north;
homePos.east = actualPos.east;
homePos.altitude = actualPos.altitude ;
homePosValid = 1;
}
 
/* Set Target Position */
if ((actualPos.state > 2) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100))
{
/* Save Current Position */
targetPos.north = actualPos.north;
targetPos.east = actualPos.east;
targetPos.altitude = actualPos.altitude ;
targetPosValid = 1;
 
if(BeepMuster == 0xffff)
{
beeptime = 5000;
BeepMuster = 0x0100;
}
}
 
if ((GPSTracking == 1) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100))
{
GPSTracking = 0;
beeptime = 5000;
BeepMuster = 0x0080;
}
/* The System is in Tracking State*/
if ((GPSTracking == 0) &&
(targetPosValid == 1) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) &&
(actualPos.state > 2))
{
GPSTracking = 1;
beeptime = 5000;
BeepMuster = 0x0c00;
}
max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);
 
#if 0
DebugOut.Analog[11] = max_p;
DebugOut.Analog[12] = max_d;
#endif
 
static int GPSTrackingCycles = 0;
long maxGainPos = 140;
long maxGainVel = 140;
 
/* Slowly ramp up gain */
if (GPSTrackingCycles < 1000)
{
GPSTrackingCycles++;
}
maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
if (actualPos.state > 2 )
{
distanceNS = actualPos.north - targetPos.north; // in 0.1m
distanceEW = actualPos.east - targetPos.east; // in 0.1m
velocityNS = actualPos.velNorth; // in 0.1m/s
velocityEW = actualPos.velEast; // in 0.1m/s
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
// Limit GPS_Nick to 25
nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m
nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s
roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s
 
TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
}
 
if ((GPSTracking == 1) &&
(actualPos.state > 2) &&
(Override == 0))
{
/* Calculate Distance to Target */
SIN_H = (float) sin(status.Psi);
COS_H =(float) cos(status.Psi);
 
/* PD-Regler */
nick_gain = nick_gain_p + nick_gain_d;
roll_gain = -(roll_gain_p + roll_gain_d);
GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);
 
GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));
/* Turn the mikrokopter slowly towards the target position */
{
int OffsetGier;
if (maxDistance / 10 > 2)
{
OffsetGier = 2;
}
else
{
OffsetGier = 0;
}
if (TargetGier < 0)
{
TargetGier += 3600;
}
if (TargetGier > sollGier)
{
if (TargetGier - sollGier < 1800)
{
sollGier += OffsetGier;
}
else
{
sollGier -= OffsetGier;
}
}
else
{
if (sollGier - TargetGier < 1800)
{
sollGier -= OffsetGier;
}
else
{
sollGier += OffsetGier;
}
}
}
#if 0
/* Go round in a square */
if (maxDistance / 10 < 10)
{
static int iState = 0;
switch (iState)
{
case 0:
targetPos.north += 400;
targetPos.east += 0;
GPSTrackingCycles = 0;
iState++;
break;
case 1:
targetPos.north += 0;
targetPos.east += 400;
GPSTrackingCycles = 0;
iState++;
break;
case 2:
targetPos.north -= 400;
targetPos.east += 0;
GPSTrackingCycles = 0;
iState++;
break;
case 3:
targetPos.north += 0;
targetPos.east -= 400;
GPSTrackingCycles = 0;
iState=0;
break;
default:
iState=0;
break;
}
beeptime = 5000;
BeepMuster = 0x0c00;
}
#endif
}
else
{
GPS_Nick = 0;
GPS_Roll = 0;
GPSTrackingCycles = 0;
}
}
 
/* ****************************************************************************
Functionname: InitGPS */ /*!
Description:
 
@return void
@pre -
@post -
@author
**************************************************************************** */
void InitGPS(void)
{
// USART1 Control and Status Register A, B, C and baud rate register
uint8_t sreg = SREG;
//uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
 
// disable all interrupts before reconfiguration
cli();
 
// disable RX-Interrupt
UCSR1B &= ~(1 << RXCIE1);
// disable TX-Interrupt
UCSR1B &= ~(1 << TXCIE1);
// disable DRE-Interrupt
UCSR1B &= ~(1 << UDRIE1);
 
// set direction of RXD1 and TXD1 pins
// set RXD1 (PD2) as an input pin
PORTD |= (1 << PORTD2);
DDRD &= ~(1 << DDD2);
 
// set TXD1 (PD3) as an output pin
PORTD |= (1 << PORTD3);
DDRD |= (1 << DDD3);
// USART0 Baud Rate Register
// set clock divider
UBRR1H = 0;
UBRR1L = BAUDRATE;
// enable double speed operation
//UCSR1A |= (1 << U2X1);
UCSR1A = _B0(U2X1);
 
// enable receiver and transmitter
UCSR1B = (1 << TXEN1) | (1 << RXEN1);
// set asynchronous mode
UCSR1C &= ~(1 << UMSEL11);
UCSR1C &= ~(1 << UMSEL10);
// no parity
UCSR1C &= ~(1 << UPM11);
UCSR1C &= ~(1 << UPM10);
// 1 stop bit
UCSR1C &= ~(1 << USBS1);
// 8-bit
UCSR1B &= ~(1 << UCSZ12);
UCSR1C |= (1 << UCSZ11);
UCSR1C |= (1 << UCSZ10);
// flush receive buffer explicit
while ( UCSR1A & (1<<RXC1) ) UDR1;
 
// enable interrupts at the end
// enable RX-Interrupt
UCSR1B |= (1 << RXCIE1);
// enable TX-Interrupt
UCSR1B |= (1 << TXCIE1);
// enable DRE interrupt
//UCSR1B |= (1 << UDRIE1);
 
// restore global interrupt flags
SREG = sreg;
gpsState = GPS_EMPTY;
}
 
 
/* ****************************************************************************
Functionname: InitGPS */ /*!
Description: Copy GPS paket data to actualPos
 
@return void
@pre -
@post -
@author
**************************************************************************** */
void GPSscanData (void)
{
if (navStatus.packetStatus == 1) // valid packet
{
actualPos.state = navStatus.GPSfix;
if ((actualPos.state > 1) && (GPSTracking == 0))
{
GRN_FLASH;
}
navStatus.packetStatus = 0;
}
 
if (navPosUtm.packetStatus == 1) // valid packet
{
actualPos.north = navPosUtm.NORTH/10L;
actualPos.east = navPosUtm.EAST/10L;
actualPos.altitude = navPosUtm.ALT/100;
actualPos.ITOW = navPosUtm.ITOW;
navPosUtm.packetStatus = 0;
}
/*
if (navPosLlh.packetStatus == 1)
{
actualPos.longi = navPosLlh.LON;
actualPos.lati = navPosLlh.LAT;
actualPos.height = navPosLlh.HEIGHT;
navPosLlh.packetStatus = 0;
}
*/
if (navVelNed.packetStatus == 1)
{
actualPos.velNorth = navVelNed.VEL_N;
actualPos.velEast = navVelNed.VEL_E;
actualPos.velDown = navVelNed.VEL_D;
actualPos.groundSpeed = navVelNed.GSpeed;
navVelNed.packetStatus = 0;
}
}
 
/* ****************************************************************************
Functionname: SIGNAL */ /*!
Description:
 
@return void
@pre -
@post -
@author
**************************************************************************** */
SIGNAL (SIG_USART1_RECV)
{
uint8_t c;
uint8_t re;
re = (UCSR1A & (_B1(FE1) | _B1(DOR1))); // any error occured ?
c = UDR1;
if (re == 0)
{
switch (gpsState)
{
case GPS_EMPTY:
if (c == SYNC_CHAR1)
{
gpsState = GPS_SYNC1;
}
break;
case GPS_SYNC1:
if (c == SYNC_CHAR2)
{
gpsState = GPS_SYNC2;
}
else if (c != SYNC_CHAR1)
{
gpsState = GPS_EMPTY;
}
break;
case GPS_SYNC2:
if (c == CLASS_NAV)
{
gpsState = GPS_CLASS;
}
else
{
gpsState = GPS_EMPTY;
}
break;
case GPS_CLASS: // msg ID seen: init packed receive
msgID = c;
CK_A = CLASS_NAV + c;
CK_B = CLASS_NAV + CK_A;
gpsState = GPS_LEN1;
 
switch (msgID)
{
case MSGID_STATUS:
ubxP = (char*)&navStatus;
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
ubxSp = (char*)&navStatus.packetStatus;
ignorePacket = navStatus.packetStatus;
break;
/*
case MSGID_POSLLH:
ubxP = (char*)&navPosLlh;
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
ubxSp = (char*)&navPosLlh.packetStatus;
ignorePacket = navPosLlh.packetStatus;
break;
*/
case MSGID_POSUTM:
ubxP = (char*)&navPosUtm;
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
ubxSp = (char*)&navPosUtm.packetStatus;
ignorePacket = navPosUtm.packetStatus;
break;
case MSGID_VELNED:
ubxP = (char*)&navVelNed;
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
ubxSp = (char*)&navVelNed.packetStatus;
ignorePacket = navVelNed.packetStatus;
break;
default:
ignorePacket = 1;
ubxSp = (char*)0;
}
break;
case GPS_LEN1: // first len byte
msgLen = c;
CK_A += c;
CK_B += CK_A;
gpsState = GPS_LEN2;
break;
case GPS_LEN2: // second len byte
msgLen = msgLen + (c * 256);
CK_A += c;
CK_B += CK_A;
gpsState = GPS_FILLING; // next data will be stored in packet struct
break;
case GPS_FILLING:
CK_A += c;
CK_B += CK_A;
 
if ( !ignorePacket && ubxP < ubxEp)
{
*ubxP++ = c;
}
 
if (--msgLen == 0)
{
gpsState = GPS_CKA;
}
break;
case GPS_CKA:
if (c == CK_A)
{
gpsState = GPS_CKB;
}
else
{
gpsState = GPS_EMPTY;
}
break;
case GPS_CKB:
if (c == CK_B && ubxSp) // No error -> packet received successfully
{
*ubxSp = 1; // set packetStatus in struct
GPSscanData();
}
gpsState = GPS_EMPTY; // ready for next packet
break;
default:
gpsState = GPS_EMPTY; // ready for next packet
}
}
else // discard any data if error occured
{
gpsState = GPS_EMPTY;
}
}
 
 
 
 
/branches/KalmanFilter MikeW/gps.h
0,0 → 1,97
#define _B1(bit) (1 << (bit))
#define _B0(bit) (0 << (bit))
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
 
//#define BAUDRATE 129 // = 9600 baud
//#define BAUDRATE 64 // = 19200 baud
#define BAUDRATE 32 // = 38400 baud
//#define BAUDRATE 21 // = 57600 baud
//#define BAUDRATE 10 // = 115200 baud
 
#define UBX_NOFIX 0
#define UBX_2DFIX 2
#define UBX_3DFIX 3
 
#define GPS_EMPTY 0
#define GPS_SYNC1 1
#define GPS_SYNC2 2
#define GPS_CLASS 3
#define GPS_LEN1 4
#define GPS_LEN2 5
#define GPS_FILLING 6
#define GPS_CKA 7
#define GPS_CKB 8
 
/* some uBlox UBX format defines */
#define SYNC_CHAR1 0xb5
#define SYNC_CHAR2 0x62
#define CLASS_NAV 0x01
#define MSGID_POSLLH 0x02
#define MSGID_STATUS 0x03
#define MSGID_POSUTM 0x08
#define MSGID_VELNED 0x12
 
typedef struct {
long north; // in cm (+ = north)
long east; // in cm (+ = east)
long altitude; // in cm
long velNorth;
long velEast;
long velDown;
long groundSpeed;
long heading;
 
unsigned long ITOW;
uint8_t state; // status of data: 0 = invlid; 1 = valid
uint8_t noSV; // number of sats
} gpsInfo_t;
typedef struct {
unsigned long ITOW; // time of week
uint8_t GPSfix; // GPSfix Type, range 0..6
uint8_t Flags; // Navigation Status Flags
uint8_t DiffS; // Differential Status
uint8_t res; // reserved
unsigned long TTFF; // Time to first fix (millisecond time tag)
unsigned long MSSS; // Milliseconds since Startup / Reset
uint8_t packetStatus;
} NAV_STATUS_t;
 
typedef struct {
unsigned long ITOW; // time of week
long LON; // longitude in 1e-07 deg
long LAT; // lattitude
long HEIGHT; // height in mm
long HMSL; // height above mean sea level im mm
unsigned long Hacc; // horizontal accuracy in mm
unsigned long Vacc; // vertical accuracy in mm
uint8_t packetStatus;
} NAV_POSLLH_t;
 
typedef struct {
unsigned long ITOW; // time of week
long EAST; // cm UTM Easting
long NORTH; // cm UTM Nording
long ALT; // cm altitude
uint8_t ZONE; // UTM zone number
uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
uint8_t packetStatus;
} NAV_POSUTM_t;
 
typedef struct {
unsigned long ITOW; // ms GPS Millisecond Time of Week
long VEL_N; // cm/s NED north velocity
long VEL_E; // cm/s NED east velocity
long VEL_D; // cm/s NED down velocity
unsigned long Speed; // cm/s Speed (3-D)
unsigned long GSpeed; // cm/s Ground Speed (2-D)
long Heading; // deg (1e-05) Heading 2-D
unsigned long SAcc; // cm/s Speed Accuracy Estimate
unsigned long CAcc; // deg Course / Heading Accuracy Estimate
uint8_t packetStatus;
} NAV_VELNED_t;
 
 
 
 
/branches/KalmanFilter MikeW/kafi.c
0,0 → 1,15
/*****************************************************************************
INCLUDES
**************************************************************************** */
#include "kafi.h"
 
 
/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/
 
 
 
/*****************************************************************************
VARIABLES
*****************************************************************************/
/branches/KalmanFilter MikeW/kafi.h
0,0 → 1,127
/*
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 "mat.h"
 
/*****************************************************************************
(SYMBOLIC) CONSTANTS
*****************************************************************************/
 
 
/* *************************************************************************** */
/* Uses the Compass to Estimate Theta, Phi, Psi */
//#define USE_Extended_MM3_Measurement_Model
/* *************************************************************************** */
 
/* kalman filter model extensions change number of state variables */
#define KAFI_DIM_X (3) /* number of rows of state vector */
 
#ifdef USE_Extended_MM3_Measurement_Model
#define KAFI_DIM_Y (6) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, HX, HY, HZ)*/
#else
#define KAFI_DIM_Y (4) /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
#endif
 
#define KAFI_DIM_U (3) /* number of rows of control vector */
 
#ifdef USE_Extended_MM3_Measurement_Model
#define fCycleTime 16.0F;
#else
#define fCycleTime 10.5F;
#endif
 
/*typedef*/ enum
{
_p = 0,
_q,
_r,
c_control_variables /* number of control variables */
} /*trControlVariables_e*/;
 
 
#ifdef USE_Extended_MM3_Measurement_Model
/*typedef*/ enum
{
_ax = 0,
_ay,
_az,
_mx,
_my,
_mz,
c_observation_variables /* number of observation variables */
} /*trObservationVariables_e*/;
#else
/*typedef*/ enum
{
_ax = 0,
_ay,
_az,
_compass,
c_observation_variables /* number of observation variables */
} /*trObservationVariables_e*/;
#endif
 
 
/*typedef*/ enum
{
_Phi = 0,
_Theta,
_Psi,
//_du,
//_dv,
//_dw,
c_state_variables /* number of state variables */
} /*trStateVariables_e*/;
 
 
typedef struct
{
f32_t Phi;
f32_t Theta;
f32_t Psi;
i32_t iPhi10;
i32_t iTheta10;
i32_t iPsi10;
f32_t dPhi;
f32_t dTheta;
f32_t dPsi;
f32_t du;
f32_t dv;
f32_t dw;
f32_t X;
f32_t Y;
f32_t Z;
} status_t;
 
status_t status;
 
 
void trUpdatePhi(void);
void trUpdateBU(void);
void trMeasure(void);
void trInnovate(void);
 
void trResetKalmanFilter(void);
void AttitudeEstimation(void);
 
int KAFIInit(kafi_t *pkafi);
void Kafi_Init(void);
void trEstimateVelocity(void);
void trLimitAngles(void);
void trControl(void);
 
 
/branches/KalmanFilter MikeW/main.c
0,0 → 1,323
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "kafi.h"
#include "mymath.h"
#include "mm3.h"
#define sin45 -0.707106
#define cos45 0.707106
 
unsigned char EEPromArray[E2END+1] EEMEM;
extern void InitOSD(void);
extern void InitGPS(void);
 
extern void SendOSD(void);
extern void MotorControl(void);
extern void SendMotorData(void);
extern void CalculateAverage(void);
extern void AttitudeEstimation(void);
extern void PID_Regler(void);
extern void SetNeutral(void);
 
/* ****************************************************************************
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: main */ /*!
Description: Hauptprogramm
 
@return void
@pre -
@post -
@author H. Buss / I. Busker
**************************************************************************** */
int main (void)
{
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;
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();
 
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--;
}
}
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);
}
 
/branches/KalmanFilter MikeW/main.h
0,0 → 1,94
#ifndef _MAIN_H
#define _MAIN_H
//Hier die Quarz Frequenz einstellen
#if defined (__AVR_ATmega32__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
#endif
 
#if defined (__AVR_ATmega644__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
#endif
 
// neue Hardware
#define ROT_OFF PORTB |= 0x01;
#define ROT_ON PORTB &=~0x01;
#define ROT_FLASH PORTB ^= 0x01
#define GRN_OFF PORTB &=~0x02
#define GRN_ON PORTB |= 0x02
#define GRN_FLASH PORTB ^= 0x02
 
#define F_CPU SYSCLK
//#ifndef F_CPU
//#error ################## F_CPU nicht definiert oder ungültig #############
//#endif
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#define EEPROM_ADR_VALID 1
#define EEPROM_ADR_ACTIVE_SET 2
#define EEPROM_ADR_LAST_OFFSET 3
 
#define PID_MM3_X_OFF 10 // word
#define PID_MM3_Y_OFF 12 // word
#define PID_MM3_Z_OFF 14 // word
#define PID_MM3_X_RANGE 16 // word
#define PID_MM3_Y_RANGE 18 // word
#define PID_MM3_Z_RANGE 20 // word
 
#define EEPROM_ADR_PARAM_BEGIN 100
 
#define CFG_HOEHENREGELUNG 0x01
#define CFG_HOEHEN_SCHALTER 0x02
#define CFG_HEADING_HOLD 0x04
#define CFG_KOMPASS_AKTIV 0x08
#define CFG_KOMPASS_FIX 0x10
#define CFG_GPS_AKTIV 0x20
#define CFG_ACHSENKOPPLUNG_AKTIV 0x40
#define CFG_DREHRATEN_BEGRENZER 0x80
 
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))
 
extern unsigned char SenderOkay;
extern unsigned char GetActiveParamSetNumber(void);
extern unsigned char EEPromArray[];
 
void ReadParameterSet (unsigned char number, unsigned char *buffer, unsigned char length);
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length);
 
 
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <avr/boot.h>
#include <avr/wdt.h>
 
#include "old_macros.h"
 
#include "_Settings.h"
#include "printf_P.h"
#include "timer0.h"
#include "uart.h"
#include "analog.h"
#include "twimaster.h"
#include "menu.h"
#include "rc.h"
#include "fc.h"
#include "gps.h"
 
 
#ifndef EEMEM
#define EEMEM __attribute__ ((section (".eeprom")))
#endif
 
#endif //_MAIN_H
 
 
 
 
 
 
/branches/KalmanFilter MikeW/makefile
0,0 → 1,415
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
HAUPT_VERSION = 0
NEBEN_VERSION = 66
VERSION_INDEX = 2
 
VERSION_KOMPATIBEL = 6 # PC-Kompatibilität
#-------------------------------------------------------------------
 
ifeq ($(MCU), atmega32)
# FUSE_SETTINGS= -u -U lfuse:w:0xff:m -U hfuse:w:0xcf:m
 
HEX_NAME = MEGA32
endif
 
ifeq ($(MCU), atmega644)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
#FUSE_SETTINGS = -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
 
# -u bei neuen Controllern wieder einspielen
HEX_NAME = MEGA644
endif
 
ifeq ($(MCU), atmega644p)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644p
endif
 
ifeq ($(F_CPU), 16000000)
QUARZ = 16MHZ
endif
 
ifeq ($(F_CPU), 20000000)
QUARZ = 20MHZ
endif
 
 
# Output format. (can be srec, ihex, binary)
FORMAT = ihex
 
# Target file name (without extension).
 
ifeq ($(VERSION_INDEX), 0)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)a
endif
ifeq ($(VERSION_INDEX), 1)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)b
endif
ifeq ($(VERSION_INDEX), 2)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)c
endif
ifeq ($(VERSION_INDEX), 3)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)d
endif
ifeq ($(VERSION_INDEX), 4)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)e
endif
ifeq ($(VERSION_INDEX), 5)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION)f
endif
 
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
OPT = 3
 
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart.c printf_P.c timer0.c analog.c Bob4_OSD.c KalmanFc.c
SRC += twimaster.c rc.c fc.c gps.c kafi.c matmatrix.c mymath.c mm3.c
 
 
##########################################################################################################
 
 
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC =
 
 
 
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS =
 
 
# Optional compiler flags.
# -g: generate debugging information (for GDB, or for COFF conversion)
# -O*: optimization level
# -f...: tuning, see gcc manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create assembler listing
CFLAGS = -O$(OPT) \
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \
-Wall -Wstrict-prototypes \
-Wa,-adhlns=$(<:.c=.lst) \
$(patsubst %,-I%,$(EXTRAINCDIRS))
 
 
# Set a "language standard" compiler flag.
# Unremark just one line below to set the language standard to use.
# gnu99 = C99 + GNU extensions. See GCC manual for more information.
#CFLAGS += -std=c89
#CFLAGS += -std=gnu89
#CFLAGS += -std=c99
CFLAGS += -std=gnu99
 
CFLAGS += -DVERSION_HAUPTVERSION=$(HAUPT_VERSION) -DVERSION_NEBENVERSION=$(NEBEN_VERSION) -DVERSION_KOMPATIBEL=$(VERSION_KOMPATIBEL) -DVERSION_INDEX=$(VERSION_INDEX)
 
 
# Optional assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create listing
# -gstabs: have the assembler create line number information; note that
# for use in COFF files, additional information about filenames
# and function names needs to be present in the assembler source
# files -- see avr-libc docs [FIXME: not yet described there]
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs
 
 
 
# Optional linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
 
# Additional libraries
 
# Minimalistic printf version
#LDFLAGS += -Wl,-u,vfprintf -lprintf_min
 
# Floating point printf version (requires -lm below)
#LDFLAGS += -Wl,-u,vfprintf -lprintf_flt
 
# -lm = math library
LDFLAGS += -lm
 
 
##LDFLAGS += -T./linkerfile/avr5.x
 
 
 
# Programming support using avrdude. Settings and variables.
 
# Programming hardware: alf avr910 avrisp bascom bsd
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
#
# Type: avrdude -c ?
# to get a full listing.
#
#AVRDUDE_PROGRAMMER = stk200
AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = ponyser
#falls Ponyser ausgewählt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
 
 
#AVRDUDE_PORT = com1 # programmer connected to serial device
AVRDUDE_PORT = lpt1 # programmer connected to parallel port
 
#AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS)
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
 
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
 
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
# see avrdude manual.
#AVRDUDE_ERASE += -y
 
# Uncomment the following if you do /not/ wish a verification to be
# performed after programming the device.
AVRDUDE_FLAGS += -V
 
# Increase verbosity level. Please use this when submitting bug
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude>
# to submit bug reports.
#AVRDUDE_FLAGS += -v -v
 
# ---------------------------------------------------------------------------
# Define directories, if needed.
DIRAVR = c:/winavr
DIRAVRBIN = $(DIRAVR)/bin
DIRAVRUTILS = $(DIRAVR)/utils/bin
DIRINC = .
DIRLIB = $(DIRAVR)/avr/lib
 
 
# Define programs and commands.
SHELL = sh
 
CC = avr-gcc
 
OBJCOPY = avr-objcopy
OBJDUMP = avr-objdump
SIZE = avr-size
 
# Programming support using avrdude.
AVRDUDE = avrdude
 
REMOVE = rm -f
COPY = cp
 
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
ELFSIZE = $(SIZE) -A $(TARGET).elf
 
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = -------- begin --------
MSG_END = -------- end --------
MSG_SIZE_BEFORE = Size before:
MSG_SIZE_AFTER = Size after:
MSG_COFF = Converting to AVR COFF:
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
MSG_FLASH = Creating load file for Flash:
MSG_EEPROM = Creating load file for EEPROM:
MSG_EXTENDED_LISTING = Creating Extended Listing:
MSG_SYMBOL_TABLE = Creating Symbol Table:
MSG_LINKING = Linking:
MSG_COMPILING = Compiling:
MSG_ASSEMBLING = Assembling:
MSG_CLEANING = Cleaning project:
 
 
# Define all object files.
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)
 
# Define all listing files.
LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)
 
# Combine all necessary flags and optional flags.
# Add target processor to flags.
#ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS)
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS)
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
 
 
# Default target.
all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \
$(TARGET).lss $(TARGET).sym sizeafter finished end
 
 
# Eye candy.
# AVR Studio 3.x does not check make's exit code but relies on
# the following magic strings to be generated by the compile job.
begin:
@echo
@echo $(MSG_BEGIN)
 
finished:
@echo $(MSG_ERRORS_NONE)
 
end:
@echo $(MSG_END)
@echo
 
 
# Display size of file.
sizebefore:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
 
sizeafter:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
 
 
 
# Display compiler version information.
gccversion :
@$(CC) --version
 
 
# Convert ELF to COFF for use in debugging / simulating in
# AVR Studio or VMLAB.
COFFCONVERT=$(OBJCOPY) --debugging \
--change-section-address .data-0x800000 \
--change-section-address .bss-0x800000 \
--change-section-address .noinit-0x800000 \
--change-section-address .eeprom-0x810000
 
 
coff: $(TARGET).elf
@echo
@echo $(MSG_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
 
 
extcoff: $(TARGET).elf
@echo
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
 
 
 
 
# Program the device.
program: $(TARGET).hex $(TARGET).eep
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
 
 
 
 
# Create final output files (.hex, .eep) from ELF output file.
%.hex: %.elf
@echo
@echo $(MSG_FLASH) $@
$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
 
%.eep: %.elf
@echo
@echo $(MSG_EEPROM) $@
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
 
# Create extended listing file from ELF output file.
%.lss: %.elf
@echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S $< > $@
 
# Create a symbol table from ELF output file.
%.sym: %.elf
@echo
@echo $(MSG_SYMBOL_TABLE) $@
avr-nm -n $< > $@
 
 
 
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(OBJ)
%.elf: $(OBJ)
@echo
@echo $(MSG_LINKING) $@
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
 
 
# Compile: create object files from C source files.
%.o : %.c
@echo
@echo $(MSG_COMPILING) $<
$(CC) -c $(ALL_CFLAGS) $< -o $@
 
 
# Compile: create assembler files from C source files.
%.s : %.c
$(CC) -S $(ALL_CFLAGS) $< -o $@
 
 
# Assemble: create object files from assembler source files.
%.o : %.S
@echo
@echo $(MSG_ASSEMBLING) $<
$(CC) -c $(ALL_ASFLAGS) $< -o $@
 
 
 
 
 
 
# Target: clean project.
clean: begin clean_list finished end
 
clean_list :
@echo
@echo $(MSG_CLEANING)
# $(REMOVE) $(TARGET).hex
$(REMOVE) $(TARGET).eep
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).cof
$(REMOVE) $(TARGET).elf
$(REMOVE) $(TARGET).map
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).a90
$(REMOVE) $(TARGET).sym
$(REMOVE) $(TARGET).lnk
$(REMOVE) $(TARGET).lss
$(REMOVE) $(OBJ)
$(REMOVE) $(LST)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRC:.c=.d)
 
 
# Automatically generate C source code dependencies.
# (Code originally taken from the GNU make user manual and modified
# (See README.txt Credits).)
#
# Note that this will work with sh (bash) and sed that is shipped with WinAVR
# (see the SHELL variable defined above).
# This may not work with other shells or other seds.
#
%.d: %.c
set -e; $(CC) -MM $(ALL_CFLAGS) $< \
| sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \
[ -s $@ ] || rm -f $@
 
 
# Remove the '-' if you want to see the dependency files generated.
-include $(SRC:.c=.d)
 
 
 
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \
clean clean_list program
 
/branches/KalmanFilter MikeW/mat.h
0,0 → 1,42
/*****************************************************************************
INCLUDES
**************************************************************************** */
#include "math.h"
#include "float.h"
#include "stdlib.h"
 
 
/*****************************************************************************
TYPEDEFS
*****************************************************************************/
#define f32_t float
#define ui32_t unsigned int
#define i32_t int
 
#define PI 3.1415926535897932384626433832795
#define matSetDiagonal(matM,Row,Column,X) (matM)->pdData[(Row)] = (X)
#define matSetFull(matM,Row,Column,X) (matM)->pdData[(Row) * (matM)->uiColumns + (Column)] = (X)
#define matGetFull(matM,Row,Column,X) *(X) = (matM)->pdData[(Row) * (matM)->uiColumns + (Column)]
#define matGetDiagonal(matM,Row,Column,X) *(X) = (matM)->pdData[(Row)]
 
 
typedef enum
{
matType_Empty = 0, /* empty matrix */
matType_Full, /* full matrix */
matType_Symmetric, /* symmetric matrix */
matType_UpperTriangular, /* upper triangular matrix */
matType_Diagonal, /* diagonal matrix */
matTypeMax /* number of different types of matrices */
} mat_MatrixType_e;
 
typedef struct
{
mat_MatrixType_e matType; /* type of matrix */
ui32_t uiSizeofData; /* needed for MEM_free */
ui32_t uiRows; /* number of rows of matrix */
ui32_t uiColumns; /* number of columns of matrix */
ui32_t uiElements; /* number of elements in matrix */
f32_t* pdData; /* pointer to the entries of the matrix */
} mat_Matrix_t;
 
/branches/KalmanFilter MikeW/matmatrix.c
0,0 → 1,13
/*****************************************************************************
INCLUDES
**************************************************************************** */
 
#include "mat.h"
#include <string.h>
 
 
/*****************************************************************************
FUNCTIONS
*****************************************************************************/
 
 
/branches/KalmanFilter MikeW/menu.h
0,0 → 1,15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Michael Walter
// + only for non-profit use
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
extern void LcdClear(void);
extern void OSDClear(void);
 
extern char DisplayBuff[80];
extern unsigned char DispPtr;
extern char OSDBuff[80];
extern unsigned char OSDPtr;
 
extern unsigned char RemoteTasten;
 
/branches/KalmanFilter MikeW/mm3.c
0,0 → 1,434
/*
 
Copyright 2008, by Killagreg
 
This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
either version 3 of the License, or (at your option) any later version.
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 original implementation was done by Niklas Nold.
All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
*/
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
 
#include "mm3.h"
#include "main.h"
#include "mymath.h"
#include "fc.h"
#include "timer0.h"
#include "rc.h"
//#include "eeprom.h"
#include "printf_P.h"
#include "kafi.h"
 
 
#define MAX_AXIS_VALUE 500
 
#ifdef USE_Extended_MM3_Measurement_Model
f32_t MM3_Hx, MM3_Hy, MM3_Hz;
#endif
 
extern int Theta45;
extern int Phi45;
 
typedef struct
{
uint8_t STATE;
uint16_t DRDY;
uint8_t AXIS;
int16_t x_axis;
int16_t y_axis;
int16_t z_axis;
} MM3_working_t;
 
 
// MM3 State Machine
#define MM3_STATE_RESET 0
#define MM3_STATE_START_TRANSFER 1
#define MM3_STATE_WAIT_DRDY 2
#define MM3_STATE_DRDY 3
#define MM3_STATE_BYTE2 4
 
#define MM3_X_AXIS 0x01
#define MM3_Y_AXIS 0x02
#define MM3_Z_AXIS 0x03
 
 
#define MM3_PERIOD_32 0x00
#define MM3_PERIOD_64 0x10
#define MM3_PERIOD_128 0x20
#define MM3_PERIOD_256 0x30
#define MM3_PERIOD_512 0x40
#define MM3_PERIOD_1024 0x50
#define MM3_PERIOD_2048 0x60
#define MM3_PERIOD_4096 0x70
 
MM3_calib_t MM3_calib;
volatile MM3_working_t MM3;
volatile uint8_t MM3_Timeout = 0;
 
/***************************************************/
/* Read Parameter from EEPROM as byte */
/***************************************************/
uint8_t GetParamByte(uint8_t param_id)
{
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
 
/***************************************************/
/* Write Parameter to EEPROM as byte */
/***************************************************/
void SetParamByte(uint8_t param_id, uint8_t value)
{
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/***************************************************/
/* Read Parameter from EEPROM as word */
/***************************************************/
uint16_t GetParamWord(uint8_t param_id)
{
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
 
/***************************************************/
/* Write Parameter to EEPROM as word */
/***************************************************/
void SetParamWord(uint8_t param_id, uint16_t value)
{
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/*********************************************/
/* Initialize Interface to MM3 Compass */
/*********************************************/
void MM3_Init(void)
{
uint8_t sreg = SREG;
 
cli();
 
// Configure Pins for SPI
// set SCK (PB7), MOSI (PB5) as output
DDRB |= (1<<DDB7)|(1<<DDB5);
// set MISO (PB6) as input
DDRB &= ~(1<<DDB6);
 
// Output Pins PC4->MM3_SS ,PC5->MM3_RESET
DDRC |= (1<<DDC4)|(1<<DDC5);
// set pins permanent to low
PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
 
// Initialize SPI-Interface
// Enable interrupt (SPIE=1)
// Enable SPI bus (SPE=1)
// MSB transmitted first (DORD = 0)
// Master SPI Mode (MSTR=1)
// Clock polarity low when idle (CPOL=0)
// Clock phase sample at leading edge (CPHA=0)
// Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
SPSR &= ~(1<<SPI2X);
 
// Init Statemachine
MM3.AXIS = MM3_X_AXIS;
MM3.STATE = MM3_STATE_RESET;
 
// Read calibration from EEprom
MM3_calib.X_off = (int16_t)GetParamWord(PID_MM3_X_OFF);
MM3_calib.Y_off = (int16_t)GetParamWord(PID_MM3_Y_OFF);
MM3_calib.Z_off = (int16_t)GetParamWord(PID_MM3_Z_OFF);
MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
 
MM3_Timeout = 0;
 
SREG = sreg;
}
 
/*********************************************/
/* Get Data from MM3 */
/*********************************************/
void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
{
switch (MM3.STATE)
{
case MM3_STATE_RESET:
PORTC &= ~(1<<PORTC4); // select slave
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset
MM3.STATE = MM3_STATE_START_TRANSFER;
return;
 
case MM3_STATE_START_TRANSFER:
PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level)
// write to SPDR triggers automatically the transfer MOSI MISO
// MM3 Period, + AXIS code
switch(MM3.AXIS)
{
case MM3_X_AXIS:
SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
break;
case MM3_Y_AXIS:
SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
break;
case MM3_Z_AXIS:
SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
break;
default:
MM3.AXIS = MM3_X_AXIS;
MM3.STATE = MM3_STATE_RESET;
return;
}
 
// DRDY line is not connected, therefore
// wait before reading data back
MM3.DRDY = SetDelay(8); // wait 8ms for data ready
MM3.STATE = MM3_STATE_WAIT_DRDY;
return;
 
case MM3_STATE_WAIT_DRDY:
if (CheckDelay(MM3.DRDY))
{
// write something into SPDR to trigger data reading
SPDR = 0x00;
MM3.STATE = MM3_STATE_DRDY;
}
return;
}
}
 
/*********************************************/
/* Interrupt SPI transfer complete */
/*********************************************/
ISR(SPI_STC_vect)
{
static int8_t tmp;
int16_t value;
 
switch (MM3.STATE)
{
// 1st byte received
case MM3_STATE_DRDY:
tmp = SPDR; // store 1st byte
SPDR = 0x00; // trigger transfer of 2nd byte
MM3.STATE = MM3_STATE_BYTE2;
return;
 
case MM3_STATE_BYTE2: // 2nd byte received
value = (int16_t)tmp; // combine the 1st and 2nd byte to a word
value <<= 8; // shift 1st byte to MSB-Position
value |= (int16_t)SPDR; // add 2nd byte
 
if(abs(value) < MAX_AXIS_VALUE) // ignore spikes
{
switch (MM3.AXIS)
{
case MM3_X_AXIS:
MM3.x_axis = value;
MM3.AXIS = MM3_Y_AXIS;
break;
case MM3_Y_AXIS:
MM3.y_axis = value;
MM3.AXIS = MM3_Z_AXIS;
break;
case MM3_Z_AXIS:
MM3.z_axis = value;
MM3.AXIS = MM3_X_AXIS;
break;
default:
MM3.AXIS = MM3_X_AXIS;
break;
}
}
PORTC |= (1<<PORTC4); // deselect slave
MM3.STATE = MM3_STATE_RESET;
// Update timeout is called every 102.4 µs.
// It takes 2 cycles to write a measurement data request for one axis and
// at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
// I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
// The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
// decremtent the MM3_Timeout every 100 ms.
// incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
{ // if all axis measurements give diffrent readings the data should be valid
if(MM3_Timeout < 20) MM3_Timeout++;
}
else // something is very strange here
{
if(MM3_Timeout ) MM3_Timeout--;
}
return;
 
default:
return;
}
}
 
 
 
/*********************************************/
/* Calibrate Compass */
/*********************************************/
void MM3_Calibrate(void)
{
static uint8_t debugcounter = 0;
int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
uint8_t measurement = 50, beeper = 0;
uint16_t timer;
 
GRN_ON;
ROT_OFF;
x_max = -16000;
x_min = 16000;
y_max = -16000;
y_min = 16000;
z_max = -16000;
z_min = 16000;
 
// get maximum and minimum reading of all axis
while (measurement)
{
if (MM3.x_axis > x_max) x_max = MM3.x_axis;
else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
 
if (MM3.y_axis > y_max) y_max = MM3.y_axis;
else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
 
if (MM3.z_axis > z_max) z_max = MM3.z_axis;
else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
 
if (!beeper)
{
ROT_FLASH;
GRN_FLASH;
beeper = 50;
}
beeper--;
 
// loop with period of 10 ms / 100 Hz
timer = SetDelay(10);
while(!CheckDelay(timer));
 
if(debugcounter++ > 30)
{
printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max);
debugcounter = 0;
}
 
// If thrust is less than 100, stop calibration with a delay of 0.5 seconds
if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--;
}
 
// Rage of all axis
MM3_calib.X_range = (x_max - x_min);
MM3_calib.Y_range = (y_max - y_min);
MM3_calib.Z_range = (z_max - z_min);
 
// Offset of all axis
MM3_calib.X_off = (x_max + x_min) / 2;
MM3_calib.Y_off = (y_max + y_min) / 2;
MM3_calib.Z_off = (z_max + z_min) / 2;
 
// save to EEProm
SetParamWord(PID_MM3_X_OFF, (uint16_t)MM3_calib.X_off);
SetParamWord(PID_MM3_Y_OFF, (uint16_t)MM3_calib.Y_off);
SetParamWord(PID_MM3_Z_OFF, (uint16_t)MM3_calib.Z_off);
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
}
 
 
/*********************************************/
/* Calculate north direction (heading) */
/*********************************************/
int16_t MM3_Heading(void)
{
int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;
int16_t angle;
int16_t heading;
 
if (MM3_Timeout)
{
// Offset correction and normalization (values of H are +/- 512)
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
 
 
// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
// assuming the MM3 board is mounted parallel to the frame.
// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
// in a top view counter clockwise direction.
// North is in opposite direction of the small arrow on the MM3 board.
// Therefore 180 deg must be added to that angle.
angle = ((int16_t)180);
// wrap angle to interval of 0°- 359°
angle += 360;
angle %= 360;
sin_yaw = (int32_t)(c_sin_8192(angle));
cos_yaw = (int32_t)(c_cos_8192(angle));
 
Hx_corr = Hx;
Hy_corr = Hy;
 
// rotate
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192;
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192;
 
#ifdef USE_Extended_MM3_Measurement_Model
/* Normalize the values to be in the same range as the accelerations */
MM3_Hx = Hx / 51.2F;
MM3_Hy = Hy / 51.2F;
MM3_Hz = Hz / 51.2F;
#endif
 
// tilt compensation
 
// calibration factor for transforming Gyro Integrals to angular degrees
 
// calculate sinus cosinus of pitch and tilt angle
//angle = (status. IntegralPitch/div_factor);
angle = (status.iTheta10 / 10);
angle = 0;
sin_pitch = (int32_t)(c_sin_8192(angle));
cos_pitch = (int32_t)(c_cos_8192(angle));
 
//angle = (IntegralRoll/div_factor);
angle = (status.iPhi10/10);
angle = 0;
sin_roll = (int32_t)(c_sin_8192(angle));
cos_roll = (int32_t)(c_cos_8192(angle));
 
Hx_corr = (Hx * cos_pitch - Hz * sin_pitch) / 8192;
Hy_corr = (Hy * cos_roll + Hz * sin_roll) / 8192;
 
// calculate Heading
heading = c_atan2(Hy_corr, Hx_corr);
 
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
}
else // MM3_Timeout = 0 i.e now new data from external board
{
// if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
heading = -1;
}
return heading;
}
 
 
 
 
/branches/KalmanFilter MikeW/mm3.h
0,0 → 1,31
#ifndef _MM3_H
#define _MM3_H
 
#include <inttypes.h>
 
typedef struct
{
int16_t X_off;
int16_t Y_off;
int16_t Z_off;
int16_t X_range;
int16_t Y_range;
int16_t Z_range;
} MM3_calib_t;
 
extern MM3_calib_t MM3_calib;
 
// Initialization of the MM3 communication
void MM3_Init(void);
 
// should be called cyclic to get actual compass reading
void MM3_Update(void);
// this function calibrates the MM3
// and returns immediately if the communication to the MM3-Board is broken.
void MM3_Calibrate(void);
 
// calculates the current compass heading in a range from 0 to 360 deg.
// returns -1 if no compass data are available
int16_t MM3_Heading(void);
 
#endif //_MM3_H
/branches/KalmanFilter MikeW/mymath.c
0,0 → 1,119
#include <stdlib.h>
#include <avr/pgmspace.h>
#include "mymath.h"
#include "main.h"
// discrete mathematics
 
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192};
 
int16_t c_sin_8192(int16_t angle)
{
int8_t m,n;
int16_t sinus;
 
// avoid negative angles
if (angle < 0)
{
m = -1;
angle = abs(angle);
}
else m = +1;
 
// fold angle to intervall 0 to 359
angle %= 360;
 
// check quadrant
if (angle <= 90) n=1; // first quadrant
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
else {angle = 360 - angle; n = -1;} //fourth quadrant
// get lookup value
sinus = pgm_read_word(&pgm_sinlookup[angle]);
// calculate sinus value
return (sinus * m * n);
}
 
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
int16_t c_cos_8192(int16_t angle)
{
return (c_sin_8192(90 - angle));
}
 
// Arcustangens returns degree in a range of +/. 180 deg
const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79};
 
int16_t c_atan2(int16_t y, int16_t x)
{
int16_t index, angle;
int8_t m;
 
if (!x && !y) return 0; //atan2(0, 0) is undefined
 
if (y < 0) m = -1;
else m = 1;
 
if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg
 
index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table
if (index < 0) index = -index;
 
if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg
else if (index > 7334) angle = 90; // limit is 90 deg
else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls
else if (index > 1465) angle = 88;
else if (index > 1046) angle = 87;
else if (index > 813) angle = 86;
else if (index > 664) angle = 85;
else if (index > 561) angle = 84;
else if (index > 486) angle = 83;
else if (index > 428) angle = 82;
else if (index > 382) angle = 81;
else angle = 80; // (index>345)
 
if (x > 0) return (angle * m); // 1st and 4th quadrant
else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant
else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant
}
 
// Arcustangens returns degree in a range of +/. 180 deg
const uint8_t pgm_asinlookup[128] PROGMEM = {0,0,1,1,2,2,3,3,4,4,4,5,5,6,6,7,7,8,8,9,9,9,10,10,11,11,12,12,13,13,14,14,14,15,15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,31,31,32,32,33,33,34,34,35,35,36,36,37,38,38,39,39,40,40,41,42,42,43,43,44,45,45,46,47,47,48,49,49,50,51,51,52,53,54,54,55,56,57,58,58,59,60,61,62,63,64,65,66,67,68,70,71,72,74,76,78,80,83};
 
int c_asin_8192(int y)
{
int index, m;
if (y < 0)
{
m = -1;
index = -y / 64;
}
else
{
m = 1;
index = y / 64;
}
 
if (index > 127)
{
index = 127;
}
return(m * pgm_read_byte(&pgm_asinlookup[index]));
}
// integer square root
uint32_t c_sqrt(uint32_t number)
{
uint32_t s1, s2;
uint8_t iter = 0;
// initialization of iteration
s2 = number;
do // iterative formula to solve x^2 - n = 0
{
s1 = s2;
s2 = number / s1;
s2 += s1;
s2 /= 2;
iter++;
//if(iter > 40) break;
}while( ( (s1-s2) > 1) && (iter < 40));
return s2;
}
/branches/KalmanFilter MikeW/mymath.h
0,0 → 1,11
#ifndef _MYMATH_H
#define _MYMATH_H
 
#include <inttypes.h>
 
extern int16_t c_sin_8192(int16_t angle);
extern int16_t c_cos_8192(int16_t angle);
extern int16_t c_asin_8192(int16_t y);
extern int16_t c_atan2(int16_t y, int16_t x);
 
#endif // _MYMATH_H
/branches/KalmanFilter MikeW/old_macros.h
0,0 → 1,47
/*
For backwards compatibility only.
Ingo Busker ingo@mikrocontroller.com
*/
 
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
 
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
 
#ifndef inb
#define inb(sfr) _SFR_BYTE(sfr)
#endif
 
#ifndef outb
#define outb(sfr, val) (_SFR_BYTE(sfr) = (val))
#endif
 
#ifndef inw
#define inw(sfr) _SFR_WORD(sfr)
#endif
 
#ifndef outw
#define outw(sfr, val) (_SFR_WORD(sfr) = (val))
#endif
 
#ifndef outp
#define outp(val, sfr) outb(sfr, val)
#endif
 
#ifndef inp
#define inp(sfr) inb(sfr)
#endif
 
#ifndef BV
#define BV(bit) _BV(bit)
#endif
 
 
#ifndef PRG_RDB
#define PRG_RDB pgm_read_byte
#endif
 
/branches/KalmanFilter MikeW/printf_P.c
0,0 → 1,490
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt
 
/*
Copyright (C) 1993 Free Software Foundation
 
This file is part of the GNU IO Library. This library is free
software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the
Free Software Foundation; either version 2, or (at your option)
any later version.
 
This library 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 General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this library; see the file COPYING. If not, write to the Free
Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 
As a special exception, if you link this library with files
compiled with a GNU compiler to produce an executable, this does not cause
the resulting executable to be covered by the GNU General Public License.
This exception does not however invalidate any other reasons why
the executable file might be covered by the GNU General Public License. */
 
/*
* Copyright (c) 1990 Regents of the University of California.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. [rescinded 22 July 1999]
* 4. Neither the name of the University nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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.
*/
 
/******************************************************************************
This file is a patched version of printf called _printf_P
It is made to work with avr-gcc for Atmel AVR MCUs.
There are some differences from standard printf:
1. There is no floating point support (with fp the code is about 8K!)
2. Return type is void
3. Format string must be in program memory (by using macro printf this is
done automaticaly)
4. %n is not implemented (just remove the comment around it if you need it)
5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the
folowing specifiers are disabled :
space # * . - + p s o O
6. A function void uart_sendchar(char c) is used for output. The UART must
be initialized before using printf.
 
Alexander Popov
sasho@vip.orbitel.bg
******************************************************************************/
 
/*
* Actual printf innards.
*
* This code is large and complicated...
*/
 
#include <string.h>
#ifdef __STDC__
#include <stdarg.h>
#else
#include <varargs.h>
#endif
 
#include "main.h"
 
 
//#define LIGHTPRINTF
char PrintZiel;
 
 
char Putchar(char zeichen)
{
if(PrintZiel == OUT_LCD)
{
DisplayBuff[DispPtr++] = zeichen; return(1);
}
else if (PrintZiel == OUT_OSD)
{
OSDBuff[OSDPtr++] = zeichen; return(1);
}
else
{
return(uart_putchar(zeichen));
}
}
 
 
void PRINT(const char * ptr, unsigned int len)
{
for(;len;len--) Putchar(*ptr++);
}
void PRINTP(const char * ptr, unsigned int len)
{
for(;len;len--) Putchar(pgm_read_byte(ptr++));
}
 
void PAD_SP(signed char howmany)
{
for(;howmany>0;howmany--) Putchar(' ');
}
 
void PAD_0(signed char howmany)
{
for(;howmany>0;howmany--) Putchar('0');
}
 
#define BUF 40
 
/*
* Macros for converting digits to letters and vice versa
*/
#define to_digit(c) ((c) - '0')
#define is_digit(c) ((c)<='9' && (c)>='0')
#define to_char(n) ((n) + '0')
 
/*
* Flags used during conversion.
*/
#define LONGINT 0x01 /* long integer */
#define LONGDBL 0x02 /* long double; unimplemented */
#define SHORTINT 0x04 /* short integer */
#define ALT 0x08 /* alternate form */
#define LADJUST 0x10 /* left adjustment */
#define ZEROPAD 0x20 /* zero (as opposed to blank) pad */
#define HEXPREFIX 0x40 /* add 0x or 0X prefix */
 
void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */
{
va_list ap;
register const char *fmt; /* format string */
register char ch; /* character from fmt */
register int n; /* handy integer (short term usage) */
register char *cp; /* handy char pointer (short term usage) */
const char *fmark; /* for remembering a place in fmt */
register unsigned char flags; /* flags as above */
signed char width; /* width from format (%8d), or 0 */
signed char prec; /* precision from format (%.3d), or -1 */
char sign; /* sign prefix (' ', '+', '-', or \0) */
unsigned long _ulong=0; /* integer arguments %[diouxX] */
#define OCT 8
#define DEC 10
#define HEX 16
unsigned char base; /* base for [diouxX] conversion */
signed char dprec; /* a copy of prec if [diouxX], 0 otherwise */
signed char dpad; /* extra 0 padding needed for integers */
signed char fieldsz; /* field size expanded by sign, dpad etc */
/* The initialization of 'size' is to suppress a warning that
'size' might be used unitialized. It seems gcc can't
quite grok this spaghetti code ... */
signed char size = 0; /* size of converted field or string */
char buf[BUF]; /* space for %c, %[diouxX], %[eEfgG] */
char ox[2]; /* space for 0x hex-prefix */
 
PrintZiel = ziel; // bestimmt, LCD oder UART
va_start(ap, fmt0);
fmt = fmt0;
 
/*
* Scan the format for conversions (`%' character).
*/
for (;;) {
for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++)
/* void */;
if ((n = fmt - fmark) != 0) {
PRINTP(fmark, n);
}
if (ch == '\0')
goto done;
fmt++; /* skip over '%' */
 
flags = 0;
dprec = 0;
width = 0;
prec = -1;
sign = '\0';
 
rflag: ch = PRG_RDB(fmt++);
reswitch:
#ifdef LIGHTPRINTF
if (ch=='o' || ch=='u' || (ch|0x20)=='x') {
#else
if (ch=='u' || (ch|0x20)=='x') {
#endif
if (flags&LONGINT) {
_ulong=va_arg(ap, unsigned long);
} else {
register unsigned int _d;
_d=va_arg(ap, unsigned int);
_ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d;
}
}
#ifndef LIGHTPRINTF
if(ch==' ') {
/*
* ``If the space and + flags both appear, the space
* flag will be ignored.''
* -- ANSI X3J11
*/
if (!sign)
sign = ' ';
goto rflag;
} else if (ch=='#') {
flags |= ALT;
goto rflag;
} else if (ch=='*'||ch=='-') {
if (ch=='*') {
/*
* ``A negative field width argument is taken as a
* - flag followed by a positive field width.''
* -- ANSI X3J11
* They don't exclude field widths read from args.
*/
if ((width = va_arg(ap, int)) >= 0)
goto rflag;
width = -width;
}
flags |= LADJUST;
flags &= ~ZEROPAD; /* '-' disables '0' */
goto rflag;
} else if (ch=='+') {
sign = '+';
goto rflag;
} else if (ch=='.') {
if ((ch = PRG_RDB(fmt++)) == '*') {
n = va_arg(ap, int);
prec = n < 0 ? -1 : n;
goto rflag;
}
n = 0;
while (is_digit(ch)) {
n = n*10 + to_digit(ch);
ch = PRG_RDB(fmt++);
}
prec = n < 0 ? -1 : n;
goto reswitch;
} else
#endif /* LIGHTPRINTF */
if (ch=='0') {
/*
* ``Note that 0 is taken as a flag, not as the
* beginning of a field width.''
* -- ANSI X3J11
*/
if (!(flags & LADJUST))
flags |= ZEROPAD; /* '-' disables '0' */
goto rflag;
} else if (ch>='1' && ch<='9') {
n = 0;
do {
n = 10 * n + to_digit(ch);
ch = PRG_RDB(fmt++);
} while (is_digit(ch));
width = n;
goto reswitch;
} else if (ch=='h') {
flags |= SHORTINT;
goto rflag;
} else if (ch=='l') {
flags |= LONGINT;
goto rflag;
} else if (ch=='c') {
*(cp = buf) = va_arg(ap, int);
size = 1;
sign = '\0';
} else if (ch=='D'||ch=='d'||ch=='i') {
if(ch=='D')
flags |= LONGINT;
if (flags&LONGINT) {
_ulong=va_arg(ap, long);
} else {
register int _d;
_d=va_arg(ap, int);
_ulong = flags&SHORTINT ? (long)(short)_d : (long)_d;
}
if ((long)_ulong < 0) {
_ulong = -_ulong;
sign = '-';
}
base = DEC;
goto number;
} else
/*
if (ch=='n') {
if (flags & LONGINT)
*va_arg(ap, long *) = ret;
else if (flags & SHORTINT)
*va_arg(ap, short *) = ret;
else
*va_arg(ap, int *) = ret;
continue; // no output
} else
*/
#ifndef LIGHTPRINTF
if (ch=='O'||ch=='o') {
if (ch=='O')
flags |= LONGINT;
base = OCT;
goto nosign;
} else if (ch=='p') {
/*
* ``The argument shall be a pointer to void. The
* value of the pointer is converted to a sequence
* of printable characters, in an implementation-
* defined manner.''
* -- ANSI X3J11
*/
/* NOSTRICT */
_ulong = (unsigned int)va_arg(ap, void *);
base = HEX;
flags |= HEXPREFIX;
ch = 'x';
goto nosign;
} else if (ch=='s') { // print a string from RAM
if ((cp = va_arg(ap, char *)) == NULL) {
cp=buf;
cp[0] = '(';
cp[1] = 'n';
cp[2] = 'u';
cp[4] = cp[3] = 'l';
cp[5] = ')';
cp[6] = '\0';
}
if (prec >= 0) {
/*
* can't use strlen; can only look for the
* NUL in the first `prec' characters, and
* strlen() will go further.
*/
char *p = (char*)memchr(cp, 0, prec);
 
if (p != NULL) {
size = p - cp;
if (size > prec)
size = prec;
} else
size = prec;
} else
size = strlen(cp);
sign = '\0';
} else
#endif /* LIGHTPRINTF */
if(ch=='U'||ch=='u') {
if (ch=='U')
flags |= LONGINT;
base = DEC;
goto nosign;
} else if (ch=='X'||ch=='x') {
base = HEX;
/* leading 0x/X only if non-zero */
if (flags & ALT && _ulong != 0)
flags |= HEXPREFIX;
 
/* unsigned conversions */
nosign: sign = '\0';
/*
* ``... diouXx conversions ... if a precision is
* specified, the 0 flag will be ignored.''
* -- ANSI X3J11
*/
number: if ((dprec = prec) >= 0)
flags &= ~ZEROPAD;
 
/*
* ``The result of converting a zero value with an
* explicit precision of zero is no characters.''
* -- ANSI X3J11
*/
cp = buf + BUF;
if (_ulong != 0 || prec != 0) {
register unsigned char _d,notlastdigit;
do {
notlastdigit=(_ulong>=base);
_d = _ulong % base;
 
if (_d<10) {
_d+='0';
} else {
_d+='a'-10;
if (ch=='X') _d&=~0x20;
}
*--cp=_d;
_ulong /= base;
} while (notlastdigit);
#ifndef LIGHTPRINTF
// handle octal leading 0
if (base==OCT && flags & ALT && *cp != '0')
*--cp = '0';
#endif
}
 
size = buf + BUF - cp;
} else { //default
/* "%?" prints ?, unless ? is NUL */
if (ch == '\0')
goto done;
/* pretend it was %c with argument ch */
cp = buf;
*cp = ch;
size = 1;
sign = '\0';
}
 
/*
* All reasonable formats wind up here. At this point,
* `cp' points to a string which (if not flags&LADJUST)
* should be padded out to `width' places. If
* flags&ZEROPAD, it should first be prefixed by any
* sign or other prefix; otherwise, it should be blank
* padded before the prefix is emitted. After any
* left-hand padding and prefixing, emit zeroes
* required by a decimal [diouxX] precision, then print
* the string proper, then emit zeroes required by any
* leftover floating precision; finally, if LADJUST,
* pad with blanks.
*/
 
/*
* compute actual size, so we know how much to pad.
*/
fieldsz = size;
 
dpad = dprec - size;
if (dpad < 0)
dpad = 0;
 
if (sign)
fieldsz++;
else if (flags & HEXPREFIX)
fieldsz += 2;
fieldsz += dpad;
 
/* right-adjusting blank padding */
if ((flags & (LADJUST|ZEROPAD)) == 0)
PAD_SP(width - fieldsz);
 
/* prefix */
if (sign) {
PRINT(&sign, 1);
} else if (flags & HEXPREFIX) {
ox[0] = '0';
ox[1] = ch;
PRINT(ox, 2);
}
 
/* right-adjusting zero padding */
if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD)
PAD_0(width - fieldsz);
 
/* leading zeroes from decimal precision */
PAD_0(dpad);
 
/* the string or number proper */
PRINT(cp, size);
 
/* left-adjusting padding (always blank) */
if (flags & LADJUST)
PAD_SP(width - fieldsz);
}
done:
va_end(ap);
}
/branches/KalmanFilter MikeW/printf_P.h
0,0 → 1,19
#ifndef _PRINTF_P_H_
#define _PRINTF_P_H_
 
#include <avr/pgmspace.h>
 
#define OUT_V24 0
#define OUT_LCD 1
#define OUT_OSD 2
 
void _printf_P (char, char const *fmt0, ...);
extern char PrintZiel;
 
 
#define printf_P(format, args...) _printf_P(OUT_V24,format , ## args)
#define printf(format, args...) _printf_P(OUT_V24,PSTR(format) , ## args)
#define LCD_printfxy(x,y,format, args...) { DispPtr = y * 20 + x; _printf_P(OUT_LCD,PSTR(format) , ## args);}
#define LCD_printf(format, args...) { _printf_P(OUT_LCD,PSTR(format) , ## args);}
 
#endif
/branches/KalmanFilter MikeW/rc.c
0,0 → 1,94
/*#######################################################################################
Decodieren eines RC Summen Signals
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "rc.h"
#include "main.h"
 
volatile int PPM_in[11];
int RCQuality = 0;
volatile unsigned char NewPpmData = 1;
 
//############################################################################
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
//Capture Funktion benutzt:
void rc_sum_init (void)
//############################################################################
{
TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
TIMSK1 |= _BV(ICIE1);
return;
}
 
//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
{
static unsigned int AltICR=0;
signed int signal = 0,tmp;
static int index;
static float RC_Quality = 0.0F;
static int PPM_org[11];
 
signal = (unsigned int) ICR1 - AltICR;
AltICR = ICR1;
//Syncronisationspause?
if ((signal > 1500) && (signal < 8000))
{
index = 1;
NewPpmData = 0; // Null bedeutet: Neue Daten
}
else
{
if(index < 10)
{
if((signal > 250) && (signal < 687))
{
signal -= 466;
// Stabiles Signal
if(abs(signal - PPM_in[index]) < 6)
{
if(SenderOkay < 200)
{
SenderOkay += 10;
}
}
/* Give an estimate for the Signal Level based on the RC-Jitter */
if (abs(2 * (signal - PPM_org[index]) > (int) RC_Quality))
{
RC_Quality = 0.99F * RC_Quality + 0.01F * (float) abs(2 * (signal - PPM_org[index])) ;
}
else
{
RC_Quality = 0.998F * RC_Quality + 0.002F * (float) abs(2 * (signal - PPM_org[index])) ;
}
tmp = (3 * (PPM_in[index]) + signal) / 4;
PPM_in[index] = tmp;
PPM_org[index] = signal;
}
else
{
RC_Quality = 0.95F * RC_Quality + 0.05F * 100;
}
RC_Quality = MIN(100.F, RC_Quality);
RCQuality = 100 - (int) RC_Quality;
index++;
if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen
if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10; // Servosignal an J4 anlegen
if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen
}
}
}
 
 
 
 
 
/branches/KalmanFilter MikeW/rc.h
0,0 → 1,25
/*#######################################################################################
Derkodieren eines RC Summen Signals
#######################################################################################*/
 
#ifndef _RC_H
#define _RC_H
 
#if defined (__AVR_ATmega32__)
#define TIMER_TEILER CK64
#define TIMER_RELOAD_VALUE 250
#endif
 
#if defined (__AVR_ATmega644__)
//#define TIMER_TEILER CK64
#define TIMER_RELOAD_VALUE 250
//#define TIMER_TEILER CK256 // bei 20MHz
//#define TIMER_RELOAD_VALUE -78 // bei 20MHz
#endif
 
extern void rc_sum_init (void);
volatile extern int PPM_in[11];
extern int PPM_diff[11]; // das diffenzierte Stick-Signal
volatile extern unsigned char NewPpmData;
 
#endif //_RC_H
/branches/KalmanFilter MikeW/timer0.c
0,0 → 1,169
#include "main.h"
volatile unsigned long cnt_ms = 0;
volatile unsigned int CountMilliseconds = 0;
volatile unsigned int Count8Khz = 0;
 
volatile static unsigned int tim_main;
volatile unsigned char UpdateMotor = 0;
volatile unsigned int cntKompass = 0;
volatile unsigned int beeptime = 0;
unsigned int BeepMuster = 0xffff;
int ServoValue = 0;
extern void MM3_Update(void);
enum {
STOP = 0,
CK = 1,
CK8 = 2,
CK64 = 3,
CK256 = 4,
CK1024 = 5,
T0_FALLING_EDGE = 6,
T0_RISING_EDGE = 7
};
 
 
SIGNAL (SIG_OVERFLOW0) // 8kHz
{
static unsigned char cnt_1ms = 1,cnt = 0;
unsigned char pieper_ein = 0;
 
Count8Khz++;
if(!cnt--)
{
cnt = 9;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms)
{
UpdateMotor = 1;
}
CountMilliseconds++;
cnt_ms++;
// update compass value if this option is enabled in the settings
MM3_Update(); // read out mm3 board
}
 
if(beeptime > 1)
{
beeptime--;
if(beeptime & BeepMuster)
{
pieper_ein = 1;
}
else pieper_ein = 0;
}
else
{
pieper_ein = 0;
BeepMuster = 0xffff;
}
 
if(pieper_ein)
{
PORTC |= (1<<7); // Speaker an PORTC.7
}
else
{
PORTC &= ~(1<<7);
}
#if 0
if(PINC & 0x10)
{
cntKompass++;
}
else
{
if((cntKompass) && (cntKompass < 4000))
{
if(cntKompass < 10)
{
cntKompass = 10;
}
KompassValue = (((int) cntKompass-10) * 36) / 35;
}
cntKompass = 0;
}
#endif
}
 
 
void Timer_Init(void)
{
tim_main = SetDelay(10);
TCCR0B = CK8;
TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
OCR0A = 0;
OCR0B = 120;
TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
TIMSK2 |= _BV(OCIE2A);
 
TIMSK0 |= _BV(TOIE0);
OCR2A = 10;
TCNT2 = 0;
}
 
// -----------------------------------------------------------------------
 
unsigned int SetDelay (unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(CountMilliseconds + t + 1);
// TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
char CheckDelay(unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
void Delay_ms(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
}
 
void Delay_ms_Mess(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt)) ANALOG_ON;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Servo ansteuern
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
static unsigned char timer = 10;
unsigned char Parameter_ServoNickControl = 100;
 
if(!timer--)
{
TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
ServoValue = Parameter_ServoNickControl;
if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (0)) / 512;
else ServoValue -= ((long) EE_Parameter.ServoNickComp * (0)) / 512;
if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
 
OCR2A = ServoValue;// + 75;
timer = EE_Parameter.ServoNickRefresh;
}
else
{
TCCR2A =3;
PORTD&=~0x80;
}
}
/branches/KalmanFilter MikeW/timer0.h
0,0 → 1,18
 
#define TIMER_TEILER CK8
#define TIMER_RELOAD_VALUE 250
 
void Timer_Init(void);
void Delay_ms(unsigned int);
void Delay_ms_Mess(unsigned int);
unsigned int SetDelay (unsigned int t);
char CheckDelay (unsigned int t);
 
extern volatile unsigned long cnt_ms;
extern volatile unsigned int CountMilliseconds;
extern volatile unsigned int Count8Khz;
extern volatile unsigned char UpdateMotor;
extern volatile unsigned int beeptime;
extern volatile unsigned int cntKompass;
extern int ServoValue;
extern unsigned int BeepMuster;
/branches/KalmanFilter MikeW/twimaster.c
0,0 → 1,427
/*############################################################################
############################################################################*/
 
#include "main.h"
#include "kafi.h"
 
unsigned char twi_state = 0;
unsigned char motor = 0;
unsigned char motorread = 0;
unsigned char motor_rx[8];
 
unsigned int TriggerMagneticHeading = 0;
unsigned int TriggerSonicReading = 0;
 
 
//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
void i2c_init(void)
//############################################################################
{
TWSR = 0;
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
}
 
//############################################################################
//Start I2C
char i2c_start(void)
//############################################################################
{
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
return(0);
}
 
//############################################################################
//Start I2C
void i2c_stop(void)
//############################################################################
{
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
}
 
void i2c_reset(void)
//############################################################################
{
i2c_stop();
twi_state = 0;
motor = TWDR;
motor = 0;
TWCR = 0x80;
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
i2c_init();
i2c_start();
i2c_write_byte(0);
}
 
//############################################################################
//Start I2C
char i2c_write_byte(char byte)
//############################################################################
{
TWSR = 0x00;
TWDR = byte;
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
return(0);
}
 
//############################################################################
//Start I2C
SIGNAL (TWI_vect)
//############################################################################
{
#if 0
int motorwert = 0;
int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F) / 6;
int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F) / 6;
#endif
 
switch (twi_state++)
{
case 0:
i2c_write_byte(0x52+(motor*2));
break;
case 1:
switch(motor++)
{
case 0:
#if 0
pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r V o r n */
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Vorne = motorwert;
if(!MotorenEin)
{
Motor_Vorne = 0;
if(MotorTest[0]) Motor_Vorne = MotorTest[0];
}
#endif
i2c_write_byte(Motor_Vorne);
break;
case 1:
#if 0
pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r H e c k */
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Hinten = motorwert;
if(!MotorenEin)
{
Motor_Hinten = 0;
if(MotorTest[1]) Motor_Hinten = MotorTest[1];
}
#endif
i2c_write_byte(Motor_Hinten);
break;
case 2:
#if 0
pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r R e c h t s */
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if (motorwert < 0)
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Rechts = motorwert;
if(!MotorenEin)
{
Motor_Rechts = 0;
if(MotorTest[3]) Motor_Rechts = MotorTest[3];
}
#endif
i2c_write_byte(Motor_Rechts);
break;
case 3:
#if 0
pd_ergebnis = (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r L i n k s */
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
if (motorwert < 0)
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Links = motorwert;
if(!MotorenEin)
{
Motor_Links = 0;
if(MotorTest[2]) Motor_Links = MotorTest[2];
}
#endif
i2c_write_byte(Motor_Links);
break;
}
break;
case 2:
i2c_stop();
if (motor<4) twi_state = 0;
else motor = 0;
i2c_start();
break;
//Liest Daten von Motor
case 3:
i2c_write_byte(0x53+(motorread*2));
break;
case 4:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 5: //1 Byte vom Motor lesen
motor_rx[motorread] = TWDR;
 
case 6:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 7: //2 Byte vom Motor lesen
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread>3) motorread=0;
i2c_stop();
I2CTimeout = 10;
twi_state = 0;
}
#if 0
break;
TriggerMagneticHeading++;
TriggerMagneticHeading %= 50;
TriggerSonicReading++;
TriggerSonicReading %= 200;
if (TriggerMagneticHeading == 1)
{
// Get Magnetic Heading
i2c_start();
twi_state = 8;
break;
}
if (TriggerSonicReading == 1)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 20;
break;
}
else if (TriggerSonicReading == 190)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 30;
break;
}
else
{
I2CTimeout = 10;
twi_state = 0;
break;
}
 
case 8:
i2c_write_byte(0xC0);
break;
case 9:
i2c_write_byte(0x02);
break;
case 10:
i2c_start();
break;
case 11:
i2c_write_byte(0xC1);
break;
case 12:
i2c_write_byte(MagneticHeading);
break;
case 13:
MagneticHeading = (0x0F & TWDR) << 8;
i2c_stop();
i2c_start();
break;
case 14:
i2c_write_byte(0xC0);
break;
case 15:
i2c_write_byte(0x03);
break;
case 16:
i2c_start();
break;
case 17:
i2c_write_byte(0xC1);
break;
case 18:
i2c_write_byte(MagneticHeading);
break;
case 19:
MagneticHeading += TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
break;
/* Trigger Ultrasonic Ping */
case 20: /* Ping */
i2c_write_byte(0xE0); /* Send I2C Adress */
break;
case 21:
i2c_write_byte(0x00); /* Send I2C Register */
break;
case 22:
i2c_write_byte(82); /* Send I2C Value */
break;
case 24:
VersionID = TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
UltrasonicPingCnt++;
break;
case 30:
i2c_write_byte(0xE0);/* Send I2C Adress */
break;
case 31:
i2c_write_byte(0x02); /* Send I2C Register */
break;
case 32:
i2c_start();
break;
case 33:
i2c_write_byte(0xE1);
break;
case 34:
i2c_write_byte(255);
break;
case 35:
UltrasonicRange = TWDR << 8;/* Read I2C Value */
UltrasonicRangeHigh = TWDR;
i2c_stop();
i2c_start();
break;
case 36:
i2c_write_byte(0xE0);
break;
case 37:
i2c_write_byte(0x03);
break;
case 38:
i2c_start();
break;
case 39:
i2c_write_byte(0xE1);
break;
case 40:
i2c_write_byte(UltrasonicRangeLow);
break;
case 41:
UltrasonicRange += TWDR;
UltrasonicRangeLow = TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
break;
}
#endif
TWCR |= 0x80;
}
/branches/KalmanFilter MikeW/twimaster.h
0,0 → 1,33
/*############################################################################
############################################################################*/
 
#ifndef _I2C_MASTER_H
#define _I2C_MASTER_H
 
//############################################################################
 
// I2C Konstanten
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
#define I2C_START 0x08
#define I2C_REPEATED_START 0x10
#define I2C_TX_SLA_ACK 0x18
#define I2C_TX_DATA_ACK 0x28
#define I2C_RX_SLA_ACK 0x40
#define I2C_RX_DATA_ACK 0x50
 
//############################################################################
 
extern unsigned char twi_state;
extern unsigned char motor;
extern unsigned char motorread;
extern unsigned char motor_rx[8];
 
void i2c_reset(void);
extern void i2c_init (void); // I2C initialisieren
extern char i2c_start (void); // Start I2C
extern void i2c_stop (void); // Stop I2C
extern char i2c_write_byte (char byte); // 1 Byte schreiben
extern void i2c_reset(void);
 
#endif
/branches/KalmanFilter MikeW/uart.c
0,0 → 1,303
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "main.h"
#include "uart.h"
 
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
unsigned volatile char SioTmp = 0;
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NeuerDatensatzEmpfangen = 0;
unsigned volatile char NeueKoordinateEmpfangen = 0;
unsigned volatile char UebertragungAbgeschlossen = 1;
 
unsigned volatile char CntCrcError = 0;
unsigned volatile char AnzahlEmpfangsBytes = 0;
unsigned volatile char PC_DebugTimeout = 0;
unsigned char NurKanalAnforderung = 0;
unsigned char PcZugriff = 100;
unsigned char MotorTest[4] = {0,0,0,0};
unsigned char MeineSlaveAdresse;
struct str_DebugOut DebugOut;
struct str_Debug DebugIn;
struct str_VersionInfo VersionInfo;
int Debug_Timer;
 
 
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Sende-Part der Datenübertragung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_TX)
{
static unsigned int ptr = 0;
unsigned char tmp_tx;
if(!UebertragungAbgeschlossen)
{
ptr++; // die [0] wurde schon gesendet
tmp_tx = SendeBuffer[ptr];
if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
{
ptr = 0;
UebertragungAbgeschlossen = 1;
}
UDR = tmp_tx;
}
else ptr = 0;
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_RX)
{
static unsigned int crc;
static unsigned char crc1,crc2,buf_ptr;
static unsigned char UartState = 0;
unsigned char CrcOkay = 0;
 
SioTmp = UDR;
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;
if(SioTmp == '\r' && UartState == 2)
{
UartState = 0;
crc -= RxdBuffer[buf_ptr-2];
crc -= RxdBuffer[buf_ptr-1];
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
CrcOkay = 0;
if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
{
NeuerDatensatzEmpfangen = 1;
AnzahlEmpfangsBytes = buf_ptr;
RxdBuffer[buf_ptr] = '\r';
if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando
}
}
else
switch(UartState)
{
case 0:
if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet
buf_ptr = 0;
RxdBuffer[buf_ptr++] = SioTmp;
crc = SioTmp;
break;
case 1: // Adresse auswerten
UartState++;
RxdBuffer[buf_ptr++] = SioTmp;
crc += SioTmp;
break;
case 2: // Eingangsdaten sammeln
RxdBuffer[buf_ptr] = SioTmp;
if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
else UartState = 0;
crc += SioTmp;
break;
default:
UartState = 0;
break;
}
}
 
 
// --------------------------------------------------------------------------
void AddCRC(unsigned int wieviele)
{
unsigned int tmpCRC = 0,i;
for(i = 0; i < wieviele;i++)
{
tmpCRC += SendeBuffer[i];
}
tmpCRC %= 4096;
SendeBuffer[i++] = '=' + tmpCRC / 64;
SendeBuffer[i++] = '=' + tmpCRC % 64;
SendeBuffer[i++] = '\r';
UebertragungAbgeschlossen = 0;
UDR = SendeBuffer[0];
}
 
 
 
// --------------------------------------------------------------------------
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
{
unsigned int pt = 0;
unsigned char a,b,c;
unsigned char ptr = 0;
 
SendeBuffer[pt++] = '#'; // Startzeichen
SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...)
SendeBuffer[pt++] = cmd; // Commando
 
while(len)
{
if(len) { a = snd[ptr++]; len--;} else a = 0;
if(len) { b = snd[ptr++]; len--;} else b = 0;
if(len) { c = snd[ptr++]; len--;} else c = 0;
SendeBuffer[pt++] = '=' + (a >> 2);
SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
SendeBuffer[pt++] = '=' + ( c & 0x3f);
}
AddCRC(pt);
}
 
 
// --------------------------------------------------------------------------
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
{
unsigned char a,b,c,d;
unsigned char ptr = 0;
unsigned char x,y,z;
while(len)
{
a = RxdBuffer[ptrIn++] - '=';
b = RxdBuffer[ptrIn++] - '=';
c = RxdBuffer[ptrIn++] - '=';
d = RxdBuffer[ptrIn++] - '=';
if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
 
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
 
if(len--) ptrOut[ptr++] = x; else break;
if(len--) ptrOut[ptr++] = y; else break;
if(len--) ptrOut[ptr++] = z; else break;
}
}
 
// --------------------------------------------------------------------------
void BearbeiteRxDaten(void)
{
if(!NeuerDatensatzEmpfangen) return;
unsigned char tmp_char_arr2[2] ={0,0};
PcZugriff = 255;
switch(RxdBuffer[2])
{
case 'c':// Debugdaten incl. Externe IOs usw
Decode64((unsigned char *) &DebugIn,sizeof(DebugIn),3,AnzahlEmpfangsBytes);
DebugDataAnforderung = 1;
break;
case 'h':// x-1 Displayzeilen
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten
DebugDisplayAnforderung = 1;
break;
case 't':// Motortest
Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
break;
case 'v': // Version-Anforderung und Ausbaustufe
GetVersionAnforderung = 1;
break;
case 'g':// "Get"-Anforderung für Debug-Daten
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
DebugGetAnforderung = 1;
break;
case 'q':// "Get"-Anforderung für Settings
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
if(tmp_char_arr2[0] != 0xff)
{
if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;
ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
}
else
SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
break;
case 'l':
case 'm':
case 'n':
case 'o':
case 'p': // Parametersatz speichern
Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes);
WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1); // aktiven Datensatz merken
break;
}
NeuerDatensatzEmpfangen = 0;
}
 
 
//############################################################################
//Routine für die Serielle Ausgabe
int uart_putchar (char c)
//############################################################################
{
if (c == '\n')
uart_putchar('\r');
//Warten solange bis Zeichen gesendet wurde
loop_until_bit_is_set(USR, UDRE);
//Ausgabe des Zeichens
UDR = c;
return (0);
}
 
// --------------------------------------------------------------------------
void WriteProgramData(unsigned int pos, unsigned char wert)
{
//if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
// else eeprom_write_byte(&EE_Buffer[pos], wert);
// Buffer[pos] = wert;
}
 
//############################################################################
//INstallation der Seriellen Schnittstelle
void UART_Init (void)
//############################################################################
{
//Enable TXEN im Register UCR TX-Data Enable & RX Enable
 
UCR=(1 << TXEN) | (1 << RXEN);
// UART Double Speed (U2X)
USR |= (1<<U2X);
// RX-Interrupt Freigabe
UCSRB |= (1<<RXCIE);
// TX-Interrupt Freigabe
UCSRB |= (1<<TXCIE);
 
//Teiler wird gesetzt
UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);
//UBRR = 33;
//öffnet einen Kanal für printf (STDOUT)
//fdevopen (uart_putchar, 0);
//sbi(PORTD,4);
Debug_Timer = SetDelay(400);
}
 
//---------------------------------------------------------------------------------------------
void DatenUebertragung(void)
{
if(!UebertragungAbgeschlossen) return;
 
if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
{
SendOutData('G',MeineSlaveAdresse,(unsigned char *) &DebugIn,sizeof(DebugIn));
DebugGetAnforderung = 0;
}
 
if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)
{
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
DebugDataAnforderung = 0;
Debug_Timer = SetDelay(2 * MIN_DEBUG_INTERVALL);
}
if(GetVersionAnforderung && UebertragungAbgeschlossen)
{
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo));
GetVersionAnforderung = 0;
}
}
 
/branches/KalmanFilter MikeW/uart.h
0,0 → 1,96
#ifndef _UART_H
#define _UART_H
 
#define MAX_SENDE_BUFF 150
#define MAX_EMPFANGS_BUFF 150
 
 
void BearbeiteRxDaten(void);
 
extern unsigned char DebugGetAnforderung;
extern unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
extern unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
extern unsigned volatile char UebertragungAbgeschlossen;
extern unsigned volatile char PC_DebugTimeout;
extern unsigned volatile char NeueKoordinateEmpfangen;
extern unsigned char MeineSlaveAdresse;
extern unsigned char PcZugriff;
extern int Debug_Timer;
extern void UART_Init (void);
extern int uart_putchar (char c);
extern void boot_program_page (uint32_t page, uint8_t *buf);
extern void DatenUebertragung(void);
extern void DecodeNMEA(void);
extern void BearbeiteRxDaten(void);
extern unsigned char MotorTest[4];
struct str_DebugOut
{
unsigned char Digital[2];
unsigned int Analog[16]; // Debugwerte
};
 
extern struct str_DebugOut DebugOut;
 
struct str_Debug
{
unsigned char Digital[2];
unsigned char RemoteTasten;
unsigned int Analog[4];
};
extern struct str_Debug DebugIn;
 
struct str_VersionInfo
{
unsigned char Hauptversion;
unsigned char Nebenversion;
unsigned char PCKompatibel;
unsigned char Rserved[7];
};
extern struct str_VersionInfo VersionInfo;
 
//Die Baud_Rate der Seriellen Schnittstelle ist 9600 Baud
//#define BAUD_RATE 9600 //Baud Rate für die Serielle Schnittstelle
//#define BAUD_RATE 14400 //Baud Rate für die Serielle Schnittstelle
//#define BAUD_RATE 28800 //Baud Rate für die Serielle Schnittstelle
//#define BAUD_RATE 38400 //Baud Rate für die Serielle Schnittstelle
#define BAUD_RATE 57600 //Baud Rate für die Serielle Schnittstelle
 
//Anpassen der seriellen Schnittstellen Register wenn ein ATMega128 benutzt wird
#if defined (__AVR_ATmega128__)
# define USR UCSR0A
# define UCR UCSR0B
# define UDR UDR0
# define UBRR UBRR0L
# define EICR EICRB
#endif
 
#if defined (__AVR_ATmega32__)
# define USR UCSRA
# define UCR UCSRB
# define UBRR UBRRL
# define EICR EICRB
# define INT_VEC_RX SIG_UART_RECV
# define INT_VEC_TX SIG_UART_TRANS
#endif
 
//#if defined (__AVR_ATmega644__)
#define SYSCLK 20000000
#if 1
# define USR UCSR0A
# define UCR UCSR0B
# define UDR UDR0
# define UBRR UBRR0L
# define EICR EICR0B
# define TXEN TXEN0
# define RXEN RXEN0
# define RXCIE RXCIE0
# define TXCIE TXCIE0
# define U2X U2X0
# define UCSRB UCSR0B
# define UDRE UDRE0
# define INT_VEC_RX SIG_USART_RECV
# define INT_VEC_TX SIG_USART_TRANS
#endif
 
 
#endif //_UART_H
/branches/KalmanFilter MikeW/version.txt
0,0 → 1,3
-------
V0.67 26.04.2008 M.Walter
- erste öffentliche Version