/branches/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/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/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/Settings.h |
---|
--- _Settings.h (nonexistent) |
+++ _Settings.h (revision 834) |
@@ -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/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/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/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/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/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/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/flight.pnps |
---|
0,0 → 1,0 |
<pd><ViewState><e p="Flight-Ctrl" x="true"></e></ViewState></pd> |
/branches/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/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/kafi.c |
---|
0,0 → 1,15 |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "kafi.h" |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
/branches/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/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/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/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/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/matmatrix.c |
---|
0,0 → 1,13 |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "mat.h" |
#include <string.h> |
/***************************************************************************** |
FUNCTIONS |
*****************************************************************************/ |
/branches/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/version.txt |
---|
0,0 → 1,3 |
------- |
V0.67 26.04.2008 M.Walter |
- erste öffentliche Version |