/branches/KalmanFilter MikeW/Bob4_OSD.c |
---|
14,23 → 14,30 |
#include "main.h" |
#include "kafi.h" |
#include "mymath.h" |
#define sin45 -0.707106 |
#define cos45 0.707106 |
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 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 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 f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P; |
extern unsigned long maxDistance; |
int Theta45; |
int Phi45; |
unsigned char UART2PrintAbgeschlossen = 1; |
void UART2Print(void); |
48,19 → 55,19 |
/* 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)); |
} |
if(PrintZiel == OUT_LCD) |
{ |
DisplayBuff[DispPtr++] = zeichen; return(1); |
} |
else if (PrintZiel == OUT_OSD) |
{ |
OSDBuff[OSDPtr++] = zeichen; return(1); |
} |
else |
{ |
return(uart_putchar(zeichen)); |
} |
} |
*/ |
77,8 → 84,8 |
**************************************************************************** */ |
void OsdClear(void) |
{ |
unsigned char i; |
for(i=0;i<80;i++) OSDBuff[i] = ' '; |
unsigned char i; |
for(i=0;i<80;i++) OSDBuff[i] = ' '; |
} |
/* **************************************************************************** |
94,8 → 101,8 |
**************************************************************************** */ |
void LcdClear(void) |
{ |
unsigned char i; |
for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
unsigned char i; |
for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
} |
112,22 → 119,22 |
**************************************************************************** */ |
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"); |
/* 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"); |
} |
143,12 → 150,12 |
**************************************************************************** */ |
void UART2Print() |
{ |
OSDBuff[OSDPtr] = '\r'; |
if (UART2PrintAbgeschlossen == 1) |
{ |
UART2PrintAbgeschlossen = 0; |
UDR1 = OSDBuff[0]; |
} |
OSDBuff[OSDPtr] = '\r'; |
if (UART2PrintAbgeschlossen == 1) |
{ |
UART2PrintAbgeschlossen = 0; |
UDR1 = OSDBuff[0]; |
} |
} |
/* **************************************************************************** |
162,21 → 169,21 |
**************************************************************************** */ |
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; |
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; |
} |
193,162 → 200,165 |
**************************************************************************** */ |
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; |
/*--- (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 1 |
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 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 "); |
iState++; |
break; |
case 4: /* Draw an artificial horizon. Part 2 */ |
#if 1 |
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 = 8; |
break; |
case 7: |
OSD_printf ("\33[4;0HN:%03d\33[5;0HR:%03d", status.iTheta10 / 10, status.iPhi10 / 10); |
iState++; |
break; |
case 8: /* Draw an artificial horizon. Part 1 */ |
#if 1 |
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 9: /* Draw an artificial horizon. Part 2 */ |
#if 1 |
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 10: /* 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 11: |
OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude); |
iState++; |
break; |
case 12:/* 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 = 14; |
break; |
} |
case 13: /* 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 14: |
#if 1/* Display the Horizon */ |
OSD_printf ("\33[156;120.r\33[204;120+r\33[/r "); |
#endif |
iState=0; |
break; |
default: |
iState = 0; |
} |
iState=0; |
break; |
default: |
iState = 0; |
} |
} |
/branches/KalmanFilter MikeW/Flight-Ctrl_MEGA644p_V0_66c.hex |
---|
0,0 → 1,2645 |
:100000000C94EE020C940B030C940B030C940B0356 |
:100010000C940B030C940B030C940B030C940B0328 |
:100020000C940B030C9404140C940B030C940B030E |
:100030000C94D6270C940B030C940B030C940B0319 |
:100040000C940B030C940B030C9441140C940A3B7A |
:100050000C94F3060C940B030C94BB060C940B034A |
:100060000C942B150C940B030C94DD260C940B03B1 |
:100070000C94BD2F0C940B030C9478170829573F50 |
:100080009F2D49CBA5310F76C73493F27E37D00D23 |
:10009000013AB60B613D2AAAAB3F0000003F800049 |
:1000A000000003398527603CC8DE923EEDB0CA3FB0 |
:1000B00080000003373700F33B4B9FB83E060C3EF1 |
:1000C0003F8000000A0D496E69742E20454550524C |
:1000D0004F4D3A2047656E657269657265204465CB |
:1000E0006661756C742D506172616D657465722EF8 |
:1000F0002E2E004F4B0A0D000A0D4162676C656998 |
:100100006368204C756674647275636B73656E7397 |
:100110006F722E2E000A0D42656E75747A6520503E |
:100120006172616D657465727361747A2025640013 |
:100130002E001B5B3135363B3132302E721B5B3269 |
:1001400030343B3132302B721B5B2F7220001B5B33 |
:1001500031383B3048534E3A253033642053523ABD |
:100160002530336420001B5B31383B3048474E3A22 |
:10017000253033642047523A2530336420001B5B1E |
:10018000323B256448201B5B323B2564485E001BE4 |
:100190005B31363B323048436C623A253033640081 |
:1001A0001B5B31363B30484E473A2530336420459F |
:1001B000473A25303364201B5B31373B30484E567D |
:1001C0003A253033642045563A2530336420001BED |
:1001D0005B25643B25642E721B5B25643B25642BE9 |
:1001E000721B5B2F72001B5B25643B25642E721B08 |
:1001F0005B25643B25642B721B5B302F72001B5BFD |
:10020000343B30484E3A253033641B5B353B304835 |
:10021000523A25303364001B5B313B323048447323 |
:10022000743A2D2E2D001B5B313B323048447374E1 |
:100230003A25303364001B5B303B32304856656CE6 |
:100240003A2D2E2D001B5B303B32304856656C3A00 |
:1002500025303364001B5B25643B25642E721B5BD9 |
:1002600025643B25642B721B5B2F72001B5B25648E |
:100270003B25642E721B5B25643B25642B721B5B44 |
:10028000302F72001B5B31383B32304841474C3ACB |
:100290002D2E2D1B5B31373B3230484261723A259F |
:1002A000303364001B5B31383B32304841474C3AB5 |
:1002B000253033641B5B31373B3230484261723A40 |
:1002C00025303364001B5B303B3048553A253033D2 |
:1002D000641B5B313B3048523A25303364001B5B72 |
:1002E000303B3131484844473A25303364001B5B8A |
:1002F000307A001B5B32353B34343876001B5B327E |
:10030000333B3676001B5B324A001B5B324A0000EF |
:10031000008F001E01AD013B02CA025803E60374C0 |
:100320000402058F051B06A7063307BE074808D23F |
:10033000085B09E3096B0AF20A780BFD0B810C04D8 |
:100340000D860D070E870E060F840F00107B10F52B |
:10035000106E11E5115B12CF124213B313231492E6 |
:1003600014FE146A15D3153B16A11605176717C896 |
:100370001727188318DE1837198E19E319361A87CC |
:100380001AD61A231B6E1BB61BFD1B411C831CC3F4 |
:100390001C001D3C1D751DAB1DE01D121E421E6F75 |
:1003A0001E9A1EC31EE91E0D1F2E1F4D1F691F849E |
:1003B0001F9B1FB01FC31FD31FE11FEC1FF51FFBA7 |
:1003C0001FFF1F002000010203040405060708099F |
:1003D0000A0B0B0C0D0E0F1011111213141515161C |
:1003E000171818191A1B1B1C1D1D1E1F1F20212149 |
:1003F00022232324242525262727282829292A2A99 |
:100400002B2B2C2C2D2D2D2E2E2F2F30303031310B |
:100410003232323333333434343535353636363799 |
:10042000373737383838393939393A3A3A3A3B3B3D |
:100430003B3B3C3C3C3C3C3D3D3D3D3E3E3E3E3EF0 |
:100440003F3F3F3F3F3F40404040404041414141AE |
:100450004141424242424242424343434343434377 |
:100460004444444444444444454545454545454544 |
:100470004546464646464646464647474747474717 |
:1004800047474747474848484848484848484848F1 |
:1004900049494949494949494949494949494A4ACA |
:1004A0004A4A4A4A4A4A4A4A4A4A4A4A4B4B4B4BA8 |
:1004B0004B4B4B4B4B4B4B4B4B4B4B4B4B4C4C4C89 |
:1004C0004C4C4C4C4C4C4C4C4C4C4C4C4C4C4C4C6C |
:1004D0004D4D4D4D4D4D4D4D4D4D4D4D4D4D4D4D4C |
:1004E0004D4D4D4D4D4D4E4E4E4E4E4E4E4E4E4E32 |
:1004F0004E4E4E4E4E4E4E4E4E4E4E4E4E4E4E4E1C |
:100500004F4F4F4F4F4F4F4F4F4F4F4F4F4F4F4FFB |
:100510004F4F4F4F4F4F4F4F4F4F4F4F4F4F4F003A |
:10052000000101020203030404040505060607078F |
:1005300008080909090A0A0B0B0C0C0D0D0E0E0E0A |
:100540000F0F101011111212131314141515161683 |
:10055000171717181819191A1A1B1B1C1C1D1D1EFA |
:100560001F1F202021212222232324242526262761 |
:10057000272828292A2A2B2B2C2D2D2E2F2F3031BE |
:1005800031323333343536363738393A3A3B3C3DFD |
:100590003E3F40414243444647484A4C4E50530A2E |
:1005A0000D584D696E3A2534642C20584D61783AC7 |
:1005B0002534642C20594D696E3A2534642C205919 |
:1005C0004D61783A2534642C205A4D696E3A2534B1 |
:1005D000642C205A4D61783A2534640011241FBEE2 |
:1005E000CFEFD0E1DEBFCDBF11E0A0E0B1E0ECE5A0 |
:1005F000F4EA02C005900D92A43DB107D9F717E0C7 |
:10060000A4EDB1E001C01D92A434B107E1F70E944E |
:1006100080060C942D520C940000089522E030E0E6 |
:10062000D9010E940552802D863040F082E0D90128 |
:10063000082E0E94175282E090E0089599270895AD |
:1006400014B815B881E887B98FEF88B98BE184B900 |
:1006500081E085B98EE38AB9579A87EF8BB984B761 |
:10066000877F84BFE0E6F0E080818861808310822C |
:1006700008958091A002813069F0109220031092B9 |
:100680001F031092D7011092D6011092D50110923B |
:10069000D401089580911F0390912003892BD9F0F4 |
:1006A0008091C4029091C50281359105CCF1809171 |
:1006B000D4019091D50127E280319207B0F18536BF |
:1006C000910538F31092200310921F031092D70166 |
:1006D0001092D60108958091C4029091C502CC97E2 |
:1006E000B4F48091D6019091D70127E2803192072E |
:1006F00010F1855F914078F081E090E090932003C5 |
:1007000080931F031092D5011092D4010895109286 |
:10071000D7011092D60180919F02843668F3B9CF39 |
:100720001092D5011092D401089501969093D501AD |
:100730008093D401C4CF01969093D7018093D601C2 |
:10074000D8CF80E090E00E94F813882319F18091BF |
:100750007005992720910601309107012817390764 |
:100760002CF084E690E00E94EF1314C08091020107 |
:10077000909103018F5F9F4FA1F780E797E19093DE |
:10078000FD018093FC0180E093E09093030180934E |
:100790000201E7CF809106019091070190935205E5 |
:1007A0008093510508950E948C090E94DB0B0895E7 |
:1007B00080910101882319F081508093010180917B |
:1007C0009F02882319F0815080939F028091C40179 |
:1007D0009091C501009731F001979093C5018093E6 |
:1007E000C401089585E090E09093C5018093C40111 |
:1007F0000E94A72780910201909103018F5F9F4F74 |
:1008000081F78091A002882361F380E197E29093C1 |
:10081000FD018093FC0180E890E0909303018093B8 |
:1008200002010895AF92BF92CF92DF92EF92FF92B2 |
:100830000F931F93CF93DF93CDB7DEB721970FB6FA |
:10084000F894DEBF0FBECDBF1092910432E4C32EE8 |
:10085000C092920486E08093930421E0A22E20E0CF |
:10086000B22ED5010E940552802D823409F46EC04B |
:1008700084EC90E09F938F931F920E94DD0F0E9463 |
:100880003E26A4E6B0E081E408E515E0F801182E64 |
:100890000E94285290E075010894E11CF11CD701D8 |
:1008A000092E0E941752A5EAB0E0F801182E0E9406 |
:1008B000285291E0D701092E0E94175289830E9485 |
:1008C000B925A6EEB0E08981F801182E0E942852C1 |
:1008D00092E0D701092E0E94175289830E94342585 |
:1008E000A7E2B1E08981F801182E0E94285293E016 |
:1008F000D92ED7010D2C0E94175289830E94B92549 |
:10090000A8E6B1E08981F801182E0E94285294E0EF |
:10091000D701092E0E94175289830E94B925A9EA9E |
:10092000B1E08981F801182E0E94285285E0D70194 |
:10093000082E0E941752D7010D2C0E941752D50184 |
:100940000C2C0E9417520F900F900F9021960FB60B |
:10095000F894DEBF0FBECDBFDF91CF911F910F91F5 |
:10096000FF90EF90DF90CF90BF90AF900895CF931E |
:10097000DF9322E030E0D9010E940552802D982FAC |
:10098000863040F581E4989FD0011124AC59BF4FC7 |
:1009900081E4E8E5F5E0182E0E940D5242E050E0B7 |
:1009A000D9010E940552802D8630E0F499279F934B |
:1009B0008F9385E191E09F938F931F920E94DD0FAB |
:1009C000809160050F900F900F900F900F9080FF17 |
:1009D00031C010C082E0D901082E0E941752A6EE45 |
:1009E000B0E0D6CF82E0DA01082E0E94175282E0F2 |
:1009F00090E0DDCF88EF90E09F938F931F920E944D |
:100A0000DD0F88EE93E00E94EF13EC010E94F716D1 |
:100A10000F900F900F90CE010E94F8138823D9F306 |
:100A200083EF90E09F938F931F920E94DD0F0F90B2 |
:100A30000F900F90DF91CF910895FB01282F863002 |
:100A400008F025E0842F9927429FD0011124AC594A |
:100A5000BF4F019724F001900E941752FACFA2E0F5 |
:100A6000B0E0022E0E9417520895CF93DF93FB014E |
:100A7000C42FDD27863008F085E0489FD00111247F |
:100A8000AC59BF4F219724F00E9405520192FACF32 |
:100A9000DF91CF910895289888E893E19093C5015C |
:100AA0008093C40165C08091F7019091F801909303 |
:100AB000D9018093D8010E940F1D0E94BD4A0E9457 |
:100AC00061240E947E1F0E9443238091A0028130F6 |
:100AD00009F4AEC01092200310921F031092D701A8 |
:100AE0001092D6011092D5011092D40120919F024C |
:100AF00080910101882319F0815080930101222304 |
:100B000019F0215020939F028091C4019091C5015A |
:100B1000009709F46BC001979093C5018093C401BD |
:100B20000E948C090E94DB0B80E090E00E94F81389 |
:100B30008823B1F0809170059927209106013091AA |
:100B400007012817390744F48091020190910301AD |
:100B50008F5F9F4F09F4BAC084E690E00E94EF13C4 |
:100B60008091060190910701909352058093510561 |
:100B70008091F9018823A1F21092F9014091D801E6 |
:100B80005091D9014115510509F48DCF8091F7019C |
:100B90009091F801841B950B9C01220F331F220FAB |
:100BA000331F220F331F880F991F820F931F805C02 |
:100BB000934060F38091F7019091F801841B950BAD |
:100BC0009C01220F331F220F331F220F331F880F68 |
:100BD000991F820F931F9695879596958795969561 |
:100BE00087959093500580934F055DCF85E090E009 |
:100BF0009093C5018093C4010E94A72780910201B0 |
:100C0000909103018F5F9F4F09F08ACF8091A002DE |
:100C1000882309F485CF80E197E29093FD018093CA |
:100C2000FC0180E890E0909303018093020178CF6B |
:100C300080911F0390912003892B29F58091C40294 |
:100C40009091C502CC970CF04EC08091D601909146 |
:100C5000D70127E28031920728F401969093D701BB |
:100C60008093D601855F914008F441C020919F0296 |
:100C700081E090E09093200380931F031092D501B0 |
:100C80001092D40135CF8091C4029091C502813574 |
:100C900091050CF427CF8091D4019091D50127E2E2 |
:100CA0008031920728F401969093D5018093D40166 |
:100CB0008536910510F11092200310921F031092B7 |
:100CC000D7011092D60120919F0212CF80E797E1C1 |
:100CD0009093FD018093FC0180E093E090930301E9 |
:100CE0008093020139CF1092D7011092D601209142 |
:100CF0009F02243608F0FCCEBBCF20919F02F8CE95 |
:100D000014B815B881E887B98FEF88B98BE184B939 |
:100D100081E085B98EE38AB9579A87EF8BB984B79A |
:100D2000877F84BFE0E6F0E0808188618083108265 |
:100D30000E94E7470E94CE140E94A8080E949429AE |
:100D40000E94CD270E9425150E94C32678940E94F8 |
:100D500012040E94B7040E947E2388EE93E09093D1 |
:100D60008C0480938B0485E5809386040E944B0558 |
:100D700081E090E008951F920F920FB60F92112418 |
:100D80008F939F93EF93FF93809100018823A9F4A1 |
:100D90008091EF019091F00101969093F001809382 |
:100DA000EF01FC01E85AFC4FE081ED3059F0863943 |
:100DB000910541F0E093C6000DC01092F001109231 |
:100DC000EF0108C01092F0011092EF0181E08093D2 |
:100DD0000001F0CFFF91EF919F918F910F900FBE87 |
:100DE0000F901F9018951F920F920FB60F9211241B |
:100DF0002F933F934F938F939F93AF93BF93CF9333 |
:100E0000DF93EF93FF938091C6008093DE014091C2 |
:100E1000EA01463910F01092E9018091DE018D302F |
:100E200009F18091E901813009F477C0813030F017 |
:100E3000823009F495C01092E901B4C08091DE01BE |
:100E4000833209F484C08091DE0180939B0481E0A9 |
:100E50008093EA018091DE0199279093EE018093BF |
:100E6000ED01A0C08091E9018230E9F61092E9011C |
:100E7000A42FBB27FD01E756FB4F3081ED01C6567D |
:100E8000DB4F28818091ED019091EE01831B910948 |
:100E9000821B91099F709093EE018093ED019C015C |
:100EA0000024220F331F001C220F331F001C232F8E |
:100EB000302D235C2093EC01982F9F73935C9093CB |
:100EC000EB018081281709F467C08091E1018F5FF1 |
:100ED0008093E10190E08091DF01882309F062C0F6 |
:100EE000992309F45FC081E08093DF014093E20120 |
:100EF000A556BB4F8DE08C9380919D04823509F0FF |
:100F000051C088E190E02CE00FB6F894A89580934A |
:100F100060000FBE2093600045C082E08093E9012D |
:100F2000E42FFF278091DE01E556FB4F80834F5F62 |
:100F30004093EA012091DE018091ED019091EE0154 |
:100F4000820F911D9093EE018093ED012BC0809153 |
:100F5000DF01882309F077CF81E08093E90173CF27 |
:100F6000E42FFF278091DE01E556FB4F8083463951 |
:100F700080F44F5F4093EA012091DE018091ED0102 |
:100F80009091EE01820F911D9093EE018093ED01FF |
:100F900009C01092E901F0CF8881981709F095CF28 |
:100FA00091E099CFFF91EF91DF91CF91BF91AF91F8 |
:100FB0009F918F914F913F912F910F900FBE0F9066 |
:100FC0001F901895CF93DF93BC01892B11F120E07E |
:100FD00030E040E050E0E8E5F3E08191280F311D7A |
:100FE0004F5F5F4F64177507C1F73F70C901002459 |
:100FF000880F991F001C880F991F001C892F902DA6 |
:10100000835CDB0111962F73235CED012196FB01BC |
:1010100008C0E0E0F0E08DE3A1E0B0E02DE3C2E045 |
:10102000D0E0E85AFC4F8083A85ABC4F2C93C85A92 |
:10103000DC4F8DE0888310920001809158038093EB |
:10104000C600DF91CF9108950F931F93CF93DF9345 |
:10105000EC01662309F476C0E42FFF27E556FB4F29 |
:101060005081842F8F5FE82FFF27E556FB4F30819B |
:101070008F5FE82FFF27E556FB4F90818F5FE82FAA |
:10108000FF27E556FB4FE0814C5F022F11270250EE |
:101090001040A42FBB270A171B070CF453C05D5345 |
:1010A0003D53792F7D532E2F2D5390E03EC09F5FEF |
:1010B000FE01E90FF11D91503295307F872F869503 |
:1010C0008695382B308363506F3FE1F19E5FFE01C0 |
:1010D000E90FF11D92507295770F770F707C722B8C |
:1010E0007083662379F1A556BB4F5C914F5FE42F67 |
:1010F000FF27E556FB4F30814F5FE42FFF27E55672 |
:10110000FB4F80814F5FE42FFF27E556FB4FE081C7 |
:101110004F5FA42FBB270A171B07A4F05D533D5355 |
:10112000782F7D532E2F2D539D5FFE01E90FF11D6A |
:10113000550F550F832F82958F70582B5083613038 |
:1011400009F0B5CFDF91CF911F910F9108950895C8 |
:10115000E1ECF0E088E18083A0ECB0E08C9182606B |
:101160008C938081806880838081806480838AE220 |
:101170008093C40080E991E00E94EF13909385046E |
:10118000809384040895982F8A3029F08091C000BC |
:1011900085FFFCCF08C08091C00085FFFCCF8DE0AB |
:1011A0008093C600F3CF9093C60080E090E008954E |
:1011B000EF92FF921F93CF93DF93322F93E290939E |
:1011C00058036093590380935A03222309F482C081 |
:1011D000A3E0EA2EF12C70E0ABE5B3E01FC0E0E045 |
:1011E0006DE32DE3812F86958695835C8C93812F0B |
:1011F00099278370907082959295907F9827807F31 |
:101200009827E82BE35CED01E9836A832B83E4E014 |
:10121000F0E0EE0EFF1E14963323C1F1FA01E70F42 |
:10122000F11D10817F5F3150D1F2FA01E70FF11DFE |
:1012300060817F5F315079F4862F9927E62FE29500 |
:10124000EF708F709070880F991F880F991F682F0B |
:10125000635C2DE3C7CFFA01E70FF11D20817F5FAB |
:101260003150862F9927E62FE295EF708F7090709E |
:10127000880F991F880F991F622F62956695669552 |
:101280006370682B635C2F73235CACCFE114F104B3 |
:1012900029F120E030E0A0E0B0E0FD01E85AFC4F89 |
:1012A0008081280F311D1196AE15BF05B0F33F7038 |
:1012B000C9010024880F991F001C880F991F001C6A |
:1012C000892F902D835CFD0131962F73235CEF01F4 |
:1012D00021960CC043E0E42EF12CDBCFA0E0B0E07F |
:1012E0008DE3E1E0F0E02DE3C2E0D0E0A85ABC4F8E |
:1012F0008C93E85AFC4F2083C85ADC4F8DE08883DA |
:1013000010920001809158038093C600DF91CF9125 |
:101310001F91FF90EF900895CF93DF93809100018C |
:10132000882309F445C28091DA01882309F040C17D |
:1013300080918404909185040E94F813882309F415 |
:101340002CC180910001882309F492C08091570339 |
:1013500093E2909358038093590384E480935A0353 |
:101360006091310571E051E2A3E0B0E0E72FFF2783 |
:10137000EF5CFA4F30817F5F515009F08FC1832FAE |
:10138000992732953F708F709070880F991F880F42 |
:10139000991F482F435C2DE3862F86958695835CA5 |
:1013A000FD01E85AFC4F8083862F99278370907047 |
:1013B00082959295907F9827807F9827382B335C71 |
:1013C000FD01E75AFC4F3083FD01E65AFC4F408394 |
:1013D000FD01E55AFC4F20831496552361F0E72F59 |
:1013E000FF27EF5CFA4F60817F5F51502DE34DE3A3 |
:1013F00030E091F2BBCF109709F4CCC120E030E08F |
:1014000040E050E0E8E5F3E08191280F311D4F5FA7 |
:101410005F4F4A175B07C1F73F70C9010024880F6F |
:10142000991F001C880F991F001C892F902D835C29 |
:10143000FD0131962F73235CEF012196A85ABC4F12 |
:101440008C93E85AFC4F2083C85ADC4F8DE0888388 |
:1014500010920001809158038093C6001092DC0125 |
:1014600088EE93E00E94EF139093850480938404A8 |
:101470008091DD01882309F49BC1809100018823BC |
:1014800009F496C18091570393E290935803809397 |
:10149000590386E580935A036091910471E059E005 |
:1014A000A3E0B0E0E72FFF27EF56FB4F30817F5FCF |
:1014B000515009F0D8C0832F992732953F708F7013 |
:1014C0009070880F991F880F991F482F435C2DE358 |
:1014D000862F86958695835CFD01E85AFC4F8083B4 |
:1014E000862F99278370907082959295907F982788 |
:1014F000807F9827382B335CFD01E75AFC4F3083FF |
:10150000FD01E65AFC4F4083FD01E55AFC4F208364 |
:101510001496552361F0E72FFF27EF56FB4F6081AC |
:101520007F5F51502DE34DE330E091F2BBCF109738 |
:1015300009F429C120E030E040E050E0E8E5F3E0C4 |
:101540008191280F311D4F5F5F4F4A175B07C1F72D |
:101550003F70C9010024880F991F001C880F991F34 |
:10156000001C892F902D835CFD0131962F73235C25 |
:10157000EF012196A85ABC4F8C93E85AFC4F208368 |
:10158000C85ADC4F8DE08883109200018091580387 |
:101590008093C6001092DD010BC18091DC0188238D |
:1015A00009F0CFCE8091DD01882309F066CF00C11C |
:1015B00080910001882309F4BBCE8091570393E208 |
:1015C000909358038093590387E480935A03609162 |
:1015D000860471E05AE0A3E0B0E0E72FFF27EA5766 |
:1015E000FB4F30817F5F515009F073C0832F9927E3 |
:1015F00032953F708F709070880F991F880F991FD8 |
:10160000482F435C2DE3862F86958695835CFD01EC |
:10161000E85AFC4F8083862F9927837090708295BB |
:101620009295907F9827807F9827382B335CFD0117 |
:10163000E75AFC4F3083FD01E65AFC4F4083FD0121 |
:10164000E55AFC4F20831496552309F45DC0E72F1B |
:10165000FF27EA57FB4F60817F5F51502DE34DE339 |
:1016600030E089F2BACFE72FFF27EF56FB4F2081FA |
:101670007F5F5150832F992732953F70422F4295BB |
:101680004695469543708F709070880F991F880F0C |
:10169000991F482B435C2F73235C1ACFE72FFF273A |
:1016A000EF5CFA4F20817F5F5150832F992732954D |
:1016B0003F708F709070880F991F880F991F422F6D |
:1016C0004295469546954370482B435C2F73235CA7 |
:1016D00063CEE72FFF27EA57FB4F20817F5F5150F2 |
:1016E000832F992732953F708F709070880F991FC4 |
:1016F000880F991F422F4295469546954370482B77 |
:10170000435C2F73235C7FCF109709F44AC020E01D |
:1017100030E040E050E0E8E5F3E08191280F311D32 |
:101720004F5F5F4F4A175B07C1F73F70C901002445 |
:10173000880F991F001C880F991F001C892F902D5E |
:10174000835CFD0131962F73235CEF012196A85A2B |
:10175000BC4F8C93E85AFC4F2083C85ADC4F8DE075 |
:10176000888310920001809158038093C6001092E4 |
:10177000DA0180918404909185040E94F8138823F3 |
:1017800009F0DFCD0ACF8DE3E1E0F0E02DE3C2E028 |
:10179000D0E0F0CE8DE3E1E0F0E02DE3C2E0D0E078 |
:1017A0004DCE8DE3E1E0F0E02DE3C2E0D0E0CFCF1D |
:1017B000DF91CF910895EF92FF920F931F93CF93F4 |
:1017C000DF938091DF01882309F405C38FEF8093B5 |
:1017D000010180919D04813760F0843709F419C1BB |
:1017E000863709F412C1813709F456C11092DF011E |
:1017F000F2C28C3608F465C08091E201082F1127EF |
:101800000250104040E050E0EEE9F4E0A8E5B5E019 |
:101810000FC09295990F990F907C6D53962B9A83D8 |
:1018200013968CEAE81681E0F80671F1A701349668 |
:10183000208184E0E82EF12CE40EF51E31817281C6 |
:101840006381CA01079608171907F4F081E04C3A42 |
:101850005807D1F03D532D53220F220F832F82952D |
:101860008F70282B2C9383E04030580769F0972F16 |
:101870009D533295307F892F86958695382BED01C3 |
:1018800039834435510529F680919D0441E468E58A |
:1018900075E08B560E941D0580919D048B56A2E039 |
:1018A000B0E0082E0E94175280E197E29093FD016C |
:1018B0008093FC0180E890E0909303018093020103 |
:1018C00095CF873609F46CC1883609F4A1C18336F7 |
:1018D00009F08CCF8091E201682F77276250704029 |
:1018E00030919E0420919F049091A0044091A10406 |
:1018F000673071050CF485C02D533D53330F330F02 |
:10190000822F82958F70382B309386049D532295B9 |
:10191000207F892F86958695282B20938704929582 |
:10192000990F990F907C4D53942B9093880430918C |
:10193000A2042091A3049091A4044091A5046B30CB |
:1019400071050CF45EC02D533D53330F330F822FBE |
:1019500082958F70382B309389049D532295207F78 |
:10196000892F86958695282B20938A049295990F26 |
:10197000990F907C4D53942B90938B043091A60437 |
:101980002091A7049091A8044091A9046F3071059B |
:10199000C4F12D533D53330F330F822F82958F7037 |
:1019A000382B30938C049D532295207F892F869568 |
:1019B0008695282B20938D049295990F990F907CF2 |
:1019C0004D53942B90938E042091AA049091AB04D4 |
:1019D0003091AC048091AD046331710594F09D5356 |
:1019E0002D53220F220F892F82958F70282B209341 |
:1019F0008F049295907F3D5336953695932B909317 |
:101A0000900481E08093DC01F1CE81E08093DD01E0 |
:101A1000EDCE8091E201482F552742505040309141 |
:101A20009E0420919F049091A0046091A1044730EE |
:101A300051050CF4DBCE2D533D53330F330F822F62 |
:101A400082958F70382B3093E5019D532295207F2E |
:101A5000892F86958695282B2093E6019295990FDC |
:101A6000990F907C6D53962B9093E7019091A2046F |
:101A70002091A3048091A4048091A5044B305105CA |
:101A80000CF4B4CE9D53990F990F2D5322952F70BE |
:101A9000922B9093E801AACE8091E20140919E049E |
:101AA00030919F042091A0042091A10499270297CE |
:101AB00007970CF0CBC080E01BE441E468E575E0DB |
:101AC0000E9435059091570383E2809358039093C9 |
:101AD000590310935A0360915805A3E0B0E071E0F8 |
:101AE00050E4E72FFF27E85AFA4F30817F5F5150CB |
:101AF000E1F5832F992732953F708F709070880F92 |
:101B0000991F880F991F482F435C2DE3862F8695D8 |
:101B10008695835CFD01E85AFC4F8083862F9927C8 |
:101B20008370907082959295907F9827807F9827F8 |
:101B3000382B335CFD01E75AFC4F3083FD01E65A38 |
:101B4000FC4F4083FD01E55AFC4F2083149655233A |
:101B500059F1E72FFF27E85AFA4F60817F5F515014 |
:101B60002DE34DE330E091F2BCCFE72FFF27E85A99 |
:101B7000FA4F20817F5F5150832F992732953F7014 |
:101B80008F709070880F991F880F991F422F429570 |
:101B9000469546954370482B435C2F73235CB6CF24 |
:101BA00081E08093DA0122CE109709F406C120E08B |
:101BB00030E040E050E0E8E5F3E08191280F311D8E |
:101BC0004F5F5F4F4A175B07C1F73F70C9010024A1 |
:101BD000880F991F001C880F991F001C892F902DBA |
:101BE000835CFD0131962F73235CEF012196A85A87 |
:101BF000BC4F8C93E85AFC4F2083C85ADC4F8DE0D1 |
:101C0000888310920001809158038093C600EECD26 |
:101C10008091E20120919E0430919F044091A004A4 |
:101C20002091A10499270297079704F13D5332951B |
:101C3000307F4D5346954695342B3F3FB9F481E0B4 |
:101C40008093E40181E08093DB01D0CD842F8D531C |
:101C5000880F880F3D5332953F70832B8F3F49F09B |
:101C6000863008F085E0182F155B27CF1092E4012D |
:101C7000E9CF109157030E940E03855B93E2909386 |
:101C800058031093590380935A036091580571E0EB |
:101C900050E4A3E0B0E0E72FFF27E85AFA4F308185 |
:101CA0007F5F5150E1F5832F992732953F708F70F8 |
:101CB0009070880F991F880F991F482F435C2DE360 |
:101CC000862F86958695835CFD01E85AFC4F8083BC |
:101CD000862F99278370907082959295907F982790 |
:101CE000807F9827382B335CFD01E75AFC4F308307 |
:101CF000FD01E65AFC4F4083FD01E55AFC4F20836D |
:101D00001496552339F1E72FFF27E85AFA4F6081DF |
:101D10007F5F51502DE34DE330E091F2BCCFE72FD0 |
:101D2000FF27E85AFA4F20817F5F5150832F992770 |
:101D300032953F708F709070880F991F880F991F90 |
:101D4000422F4295469546954370482B435C2F732E |
:101D5000235CB6CF1097C1F120E030E040E050E0C6 |
:101D6000E8E5F3E08191280F311D4F5F5F4F4A177F |
:101D70005B07C1F73F70C9010024880F991F001C41 |
:101D8000880F991F001C892F902D835CED012196EF |
:101D90002F73235CFE013196A85ABC4F8C93C85A0E |
:101DA000DC4F2883E85AFC4F8DE0808310920001BD |
:101DB000809158038093C60019CD8DE3E1E0F0E0F7 |
:101DC0002DE3C2E0D0E013CF8DE3C1E0D0E02DE3FE |
:101DD000E2E0F0E0E1CFDF91CF911F910F91FF9012 |
:101DE000EF900895982F809153058130A9F08230AB |
:101DF00031F0892F0E94C308282F332719C0809102 |
:101E00003A02E82FFF27E55AFE4F90838F5F8093B9 |
:101E10003A0221E030E00CC080913902E82FFF2720 |
:101E2000E55FFE4F90838F5F8093390221E030E0C1 |
:101E3000C90108950F931F93182F18162CF500E26F |
:101E40000CC080913A02E82FFF27E55AFE4F00832D |
:101E50008F5F80933A021150B9F080915305813021 |
:101E600041F0823071F380E20E94C3081150A9F75B |
:101E70000BC080913902E82FFF27E55FFE4F0083FA |
:101E80008F5F80933902E7CF1F910F9108950F93D1 |
:101E90001F93CF93DF938C01EB01672B69F426C06E |
:101EA00080913A02E82FFF27E55AFE4F90838F5F1B |
:101EB00080933A022197D1F0F80191918F0180919E |
:101EC0005305813041F0823059F3892F0E94C308B5 |
:101ED000219791F70BC080913902E82FFF27E55F2A |
:101EE000FE4F90838F5F80933902E4CFDF91CF91D3 |
:101EF0001F910F9108950F931F93182F18162CF50B |
:101F000000E30CC080913A02E82FFF27E55AFE4F0C |
:101F100000838F5F80933A021150B9F0809153058E |
:101F2000813041F0823071F380E30E94C308115088 |
:101F3000A9F70BC080913902E82FFF27E55FFE4F1C |
:101F400000838F5F80933902E7CF1F910F9108952F |
:101F50000F931F93CF93DF938C01EB01672B69F4F1 |
:101F600027C080913A02E82FFF27E55AFE4F908361 |
:101F70008F5F80933A022197D9F0F8010F5F1F4FCE |
:101F8000949180915305813041F0823051F3892F33 |
:101F90000E94C308219789F70BC080913902E82F6E |
:101FA000FF27E55FFE4F90838F5F80933902E3CF79 |
:101FB000DF91CF911F910F9108952F923F924F92F1 |
:101FC0005F926F927F928F929F92AF92BF92CF92C9 |
:101FD000DF92EF92FF920F931F93CF93DF93CDB7D2 |
:101FE000DEB7E5970FB6F894DEBF0FBECDBF2B96D8 |
:101FF0008FAD2B9780935305CE01835B9F4F9D83BD |
:102000008C832D96EEACFFAC2D9788249924540137 |
:10201000F7018491882309F4C9C3853209F4CCC23D |
:10202000870103C0F5E27F1631F00F5F1F4FF80103 |
:1020300074907720B9F7C8018E199F0961F16C017E |
:1020400010C080913A02E82FFF27E55AFE4F908397 |
:102050008F5F80933A020894C108D108C114D1045B |
:10206000D1F0F7010894E11CF11C94918091530583 |
:10207000813031F0823029F3892F0E94C308EBCFE1 |
:1020800080913902E82FFF27E55FFE4F90838F5F35 |
:1020900080933902E0CF772009F488C3780108944F |
:1020A000E11CF11C1982AC81BD81222433242FEF65 |
:1020B0002BAB66247724F7010894E11CF11CE49112 |
:1020C00081E0C82ED12CC620D7201E2F1537C9F18C |
:1020D000812F80628837A9F1103209F43FC0133292 |
:1020E00009F48AC01A3209F47AC01D3209F47DC09D |
:1020F0001B32C1F11E32C1F1103309F48BC0812FA4 |
:102100008153893008F07DC020E030E0C901880F9C |
:10211000991F880F991F880F991F220F331F280FAF |
:10212000391F210F311D20533040F7010894E11C65 |
:10213000F11C1491812F80538A3040F3322E1537D1 |
:1021400039F6C114D10409F43CC08D909D90AD9036 |
:10215000BC9013971496103209F0C1CF898188235F |
:1021600009F0A9CF1983A7CFF7010894E11CF11C4E |
:10217000E491EA3209F455C08E2F80538A3060F51D |
:1021800020E030E0C901880F991F880F991F880F40 |
:10219000991F220F331F280F391F2E0F311D205377 |
:1021A0003040F7010894E11CF11C6491E62F862F62 |
:1021B00080538A3038F3A90137FD0BC04BAB162F83 |
:1021C00085CF8D919C9111974C01AA24BB24129626 |
:1021D00083CF4FEF5FEFF2CF1E2F1BAA77CFFD010A |
:1021E0001296308037FE67CF319430E1232A8FED8D |
:1021F0002822622C77245FCFF8E02F2A622C7724E4 |
:102200005ACF1836B9F01C36D1F421E0222A622CBC |
:10221000772451CF64FC4FCF90E2292A622C772497 |
:102220004ACFCD0102964D915C9157FD13C14BAB46 |
:10223000DC0141CFE4E02E2A622C77243CCFBD8321 |
:10224000AC83133609F4D1C1143409F4BCC114367B |
:1022500009F4BDC1193609F4BAC11F3409F4AEC17D |
:102260001F3609F491C2103709F480C2133709F4FC |
:102270003FC2153509F437C2153709F47CC118354A |
:1022800009F4EBC0183709F4E8C0112309F48EC231 |
:102290001E8319828EA69FA6A8AAB9AA992496E0A1 |
:1022A000C92ED12CCC0EDD1E81E0582E21E030E06D |
:1022B0009981992309F479C1852D8F5F882E830E2A |
:1022C000F301E073F070FDABECABEF2B49F5032DA0 |
:1022D000081910162CF530E2B32E0CC080913A028A |
:1022E000E82FFF27E55AFE4FB0828F5F80933A02B6 |
:1022F0000150A9F080915305813031F0823071F3A3 |
:1023000080E20E94C308F4CF80913902E82FFF27B2 |
:10231000E55FFE4FB0828F5F80933902E9CF9981EC |
:10232000992309F4BAC180915305813009F4AAC1F7 |
:10233000823009F0A3C180913A02E82FFF27E55AC5 |
:10234000FE4F90838F5F80933A022CA93DA92032E3 |
:10235000310509F46AC1992019F110E30EC08230E9 |
:10236000D9F480913A02E82FFF27E55AFE4F1083F7 |
:102370008F5F80933A029A9499F08091530581304F |
:1023800071F780913902E82FFF27E55FFE4F108338 |
:102390008F5F80933902EFCF80E30E94C308EBCFB9 |
:1023A000552051F1052D112707FD10950FC08230E2 |
:1023B000F9F480913A02E82FFF27E55AFE4F908307 |
:1023C0008F5F80933A0201501040B1F0F601919175 |
:1023D0006F0180915305813051F780913902E82FC8 |
:1023E000FF27E55FFE4F90838F5F80933902EBCF2D |
:1023F000892F0E94C308E7CF64FE27C0132D181948 |
:1024000011161CF500E20CC080913A02E82FFF275C |
:10241000E55AFE4F00838F5F80933A021150A9F076 |
:1024200080915305813031F0823071F380E20E9457 |
:10243000C308F4CF80913902E82FFF27E55FFE4FF4 |
:1024400000838F5F80933902E9CF8EA49FA4A8A850 |
:10245000B9A8DECD4FEF5FEFEACE63FC7EC030E17E |
:102460003AAB1982FBA9F7FD02C02FED222281149D |
:102470009104A104B10409F454C03AA9432E55248F |
:1024800066247724183541F16EE2C62ED12CCC0E8D |
:10249000DD1E17C0062F005DF60102936F01C50116 |
:1024A000B401A30192010E94C8512EA73FA748ABD7 |
:1024B00059AB84149504A604B70408F456C0490126 |
:1024C0005A01C501B401A30192010E94C8516A30AA |
:1024D00008F3062F095AE0CF4EE2C42ED12CCC0EC1 |
:1024E000DD1EC501B401A30192010E94C8516A30EA |
:1024F00008F044C0062F005DF60102936F01C5018C |
:10250000B401A30192010E94C8512EA73FA748AB76 |
:1025100059AB84149504A604B70438F149015A0153 |
:10252000E0CFFBA9FF2309F0A8CF622C77241EA6D9 |
:102530001FA618AA19AA5EE2C52ED12CCC0EDD1E4C |
:102540009E012C19522E3EE2530E4BA9252D942E9E |
:10255000951897FC1CC0392DABCE81149104A104B1 |
:10256000B10481F480E18AAB7CCFFAA9F83091F014 |
:10257000622C7724E5CFEAE0EAAB73CF062F095A45 |
:102580000F7DBACF90E4292AE0E1EAAB6ACF992423 |
:1025900030E08ECE622C772463FED2CF003381F2FE |
:1025A00080E3F60182936F01CBCF66FC02C0852DDC |
:1025B00085CE822F8E5F82CE870170CDF1E02F2AEB |
:1025C00028E02AAB4ECF31E0232A622C772460FE2C |
:1025D00021C01496BD83AC83BE90AE909E908E9029 |
:1025E000B7FC0BC08AE08AAB3DCF1296BD83AC83AB |
:1025F00012978C918E8319824DCEB094A0949094B2 |
:102600008094811C911CA11CB11C9DE299839AE0CD |
:102610009AAB28CF1296BD83AC839E918E914C01CC |
:10262000AA2497FCA094BA2CDBCF132D18191116ED |
:102630000CF091CE00E30FC08230E1F480913A02B9 |
:10264000E82FFF27E55AFE4F00838F5F80933A0201 |
:10265000115009F480CE80915305813069F7809143 |
:102660003902E82FFF27E55FFE4F00838F5F8093DD |
:102670003902EECF80E30E94C308EACF892F0E947F |
:10268000C30863CE80913902E82FFF27E55FFE4F34 |
:1026900090838F5F8093390258CE66FE56CE80E3DA |
:1026A0008A831B838E010E5F1F4F22E0A22EB12C66 |
:1026B000A00EB11EF80191918F01809153058130D8 |
:1026C00009F469C0823009F062C080913A02E82FB3 |
:1026D000FF27E55AFE4F90838F5F80933A020A15D9 |
:1026E0001B0541F732CEF1E02F2A2AE02AABB9CE02 |
:1026F0001296BD83AC83DE90CE90C114D10489F4D0 |
:1027000088E28E838EE68F8385E788878CE68987CB |
:102710008A8789E28B871C8676E0C72ED12CCC0E67 |
:10272000DD1E9BA997FD1AC0492F552747FD5095DF |
:1027300060E070E0C6010E94AA4E009769F08C1913 |
:102740005BA885150CF4582E198213E78EA69FA658 |
:10275000A8AAB9AA40E0FACE5BA8F6CFF60101908C |
:102760000020E9F731975E2E5C18EECF1296BD83FC |
:10277000AC839E918E914C01AA24BB2430E4232A81 |
:1027800018E780E18AAB6DCEE8E0EAAB6ACE892F2C |
:102790000E94C308A4CF80913902E82FFF27E55F8C |
:1027A000FE4F90838F5F8093390299CFE5960FB6E5 |
:1027B000F894DEBF0FBECDBFDF91CF911F910F9177 |
:1027C000FF90EF90DF90CF90BF90AF909F908F9051 |
:1027D0007F906F905F904F903F902F900895209141 |
:1027E000F5013091F6012F5F3F4F820F931F08953F |
:1027F0002091F5013091F601821B930B892F9927C7 |
:1028000086958074992708951F920F920FB60F92A4 |
:1028100011248F939F938091040181508093040130 |
:102820008F3F29F083E08093B0005F9823C083E856 |
:102830008093B00084E690E09093FF018093FE01C6 |
:1028400080917B059927853691056CF09093FF0167 |
:102850008093FE018091FE018093B30080917D05FD |
:102860008093040107C080917C05992784369105E7 |
:102870008CF7ECCF9F918F910F900FBE0F901F9010 |
:1028800018951F920F920FB60F9211242F933F931A |
:102890004F935F936F937F938F939F93AF93BF9368 |
:1028A000EF93FF938091F7019091F8010196909337 |
:1028B000F8018093F701809100028150809300021B |
:1028C0008F3F01F18091FC019091FD01029708F486 |
:1028D00049C08091FC019091FD0101979093FD0109 |
:1028E0008093FC018091FC019091FD0120910201F7 |
:1028F0003091030182239323892B11F447983DC023 |
:10290000479A3BC089E080930002809105018F5F68 |
:10291000817080930501882361F18091F501909188 |
:10292000F60101969093F6018093F5018091F101F3 |
:102930009091F201A091F301B091F4010196A11DD3 |
:10294000B11D8093F1019093F201A093F301B09334 |
:10295000F4010E940E3D8091FC019091FD010297CF |
:1029600008F0B7CF8FEF9FEF9093030180930201A0 |
:10297000C5CF81E08093F901D0CFFF91EF91BF9156 |
:10298000AF919F918F917F916F915F914F913F9107 |
:102990002F910F900FBE0F901F9018958091F50109 |
:1029A0009091F6010B96909302028093010282E0CF |
:1029B00085BD83EA84BD17BC88E788BD96E096BDD7 |
:1029C00083EC8093B0009093B100E0E7F0E0808169 |
:1029D00082608083EEE6F0E08081816080838AE01F |
:1029E0008093B3001092B20008952091F5013091C8 |
:1029F000F6012F5F3F4F280F391F8091F50190910D |
:102A0000F601A901481B590B57FD0CC04FEE40932E |
:102A10007A008091F5019091F601B901681B790B5C |
:102A200077FFF5CF08952091F5013091F6012F5FE2 |
:102A30003F4F280F391F8091F5019091F601A901B0 |
:102A4000481B590B57FFF7CF089510927C008FEE6B |
:102A500080937A0008951F920F920FB60F9211245F |
:102A60002F933F934F935F936F937F938F939F9396 |
:102A7000AF93BF9310927A00609137026F5F6093BB |
:102A800037026150633009F44CC0643098F065300F |
:102A900009F4CBC0653008F463C0663009F429C17D |
:102AA000673009F467C110923802109237028091A2 |
:102AB000380289C1613009F4EBC0623008F484C087 |
:102AC00080917800909179009093040280930302A2 |
:102AD00080917800909179009093100280930F027A |
:102AE00020917800309179008091550290915602A2 |
:102AF000281B390B80912D0290912E02820F931F7B |
:102B000090932E0280932D02809133029091340293 |
:102B10000196909334028093330284E080933802CC |
:102B200052C18091780090917900409106015091B6 |
:102B300007019A01220F331F240F351F63E070E055 |
:102B40000E94A151260F371F369527953695279558 |
:102B5000309307012093060186E08093380233C149 |
:102B600020915D0230915E02809178009091790011 |
:102B7000A901481B590B50930A0240930902809106 |
:102B800078009091790090931602809315028091BD |
:102B9000780090917900281B390B80911D0290914B |
:102BA0001E02820F931F90931E0280931D0280913C |
:102BB0002502909126020196909326028093250289 |
:102BC00087E080933802FFC080917800909179006F |
:102BD0009093080280930702809178009091790089 |
:102BE0009093140280931302209178003091790021 |
:102BF0008091590290915A02281B390B80912F0223 |
:102C000090913002820F931F9093300280932F0295 |
:102C1000809135029091360201969093360280930E |
:102C2000350281E080933802CEC080917800909187 |
:102C3000790040915B0250915C02841B950B90934C |
:102C40000C0280930B028091780090917900909310 |
:102C50001802809317022091780030917900241B8C |
:102C6000350B80911B0290911C02820F931F909351 |
:102C70001C0280931B0280912302909124020196F2 |
:102C8000909324028093230260933802862F9BC086 |
:102C900080917800909179009093060280930502CC |
:102CA00080917800909179009093120280931102A4 |
:102CB00020917800309179008091570290915802CC |
:102CC000281B390B80912B0290912C02820F931FAD |
:102CD00090932C0280932B028091310290913202CA |
:102CE0000196909332028093310282E08093380201 |
:102CF0006AC0409178005091790020915B02309138 |
:102D00005C0280915D0290915E02280F391F37FDB1 |
:102D100057C035952795421B530B50930E02409395 |
:102D20000D02809178009091790090931A0280931F |
:102D30001902209178003091790080915F02909182 |
:102D40006002281B390B80911F0290912002820F94 |
:102D5000931F9093200280931F02809127029091ED |
:102D600028020196909328028093270283E08093A3 |
:102D7000380229C0809178009091790090930901E0 |
:102D80008093080120917800309179008091210290 |
:102D900090912202820F931F90932202809321022E |
:102DA0008091290290912A02019690932A028093A1 |
:102DB000290210923802109237028091380203C023 |
:102DC0002F5F3F4FA6CF80937C008FEE80937A00D9 |
:102DD000BF91AF919F918F917F916F915F914F9133 |
:102DE0003F912F910F900FBE0F901F901895CF928B |
:102DF000DF92EF92FF921F93CF93DF93F3E0EF2EDA |
:102E0000F0E0FF2ED7010E940552802DC82FDD274C |
:102E1000C531D10508F02A971C2FC7BD84E690E084 |
:102E20000E94F51480910801909109018255934008 |
:102E300008F5C0E0D0E0E0E3CE2EE1E0DE2E04C0F5 |
:102E40002196CA3FD105F0F41C2FC7BD82E390E064 |
:102E50000E94F514DF92CF921F920E94DD0F0F9017 |
:102E60000F900F9080910801909109018458934030 |
:102E700038F709C0CA3FD10530F470E3C72E71E0BE |
:102E8000D72EE2CF1C2FD701012E0E9417528CE2C1 |
:102E900091E00E94F514DF91CF911F91FF90EF9088 |
:102EA000DF90CF900895EBE5F1E080E2819391E02F |
:102EB000EB3AF907D9F70895EBE0F1E080E281936E |
:102EC00091E0EB35F907D9F70895E0913A02FF2731 |
:102ED000E55AFE4F8DE0808380910A01813009F030 |
:102EE000089510920A0180915B018093CE000895AD |
:102EF0001F920F920FB60F9211248F93EF93FF93AF |
:102F000080910A01882389F4E0915302F0915402E0 |
:102F10003196F0935402E0935302E55AFE4FE0815C |
:102F2000ED3041F0E093CE000DC0109254021092AB |
:102F3000530208C0109254021092530281E0809311 |
:102F40000A01F0CFFF91EF918F910F900FBE0F907C |
:102F50001F901895EF92FF920F931F93CF93DF93DB |
:102F600080913D0290913E028730910509F4A5C001 |
:102F7000883091050CF463C08B30910509F4DCC1F5 |
:102F80008C3091050CF4D4C08D30910509F462C3E6 |
:102F90008D3091050CF01FC380910A0390910B03B3 |
:102FA0002091310330913203821B930B6AE070E071 |
:102FB0000E94B551653B710514F0685671409FEF52 |
:102FC0006C34790714F468597E4FCB016EE070E0E1 |
:102FD0000E94B551EB0110923A022E96DF93CF93E7 |
:102FE0002E9780913B0290913C020E969F938F9377 |
:102FF0008EE791E09F938F9382E08F930E94DD0F85 |
:10300000E0913A02FF27E55AFE4F8DE080838DB7AD |
:103010009EB707960FB6F8949EBF0FBE8DBF8091E6 |
:103020000A01813009F4CBC3D0933C02C0933B0228 |
:103030008EE090E090933E0280933D023EC4833048 |
:10304000910509F484C1843091050CF047C18130A9 |
:10305000910509F441C3823091050CF48DC3809130 |
:10306000E6029091E702019709F46FC310923A02C9 |
:1030700080918D0290918E029F938F9384E892E0CD |
:103080009F938F9382E08F930E94DD0FE0913A022D |
:10309000FF27E55AFE4F8DE080830F900F900F9031 |
:1030A0000F900F9080910A018130C1F510920A01B2 |
:1030B00080915B018093CE0031C010923A028091E2 |
:1030C0002D0390912E036AE070E00E94B5517F932A |
:1030D0006F9380912F03909130036AE070E00E941B |
:1030E000B5517F936F938EEF91E09F938F9382E022 |
:1030F0008F930E94DD0FE0913A02FF27E55AFE4FC1 |
:103100008DE080838DB79EB707960FB6F8949EBF6B |
:103110000FBE8DBF80910A01813041F280913D0246 |
:1031200090913E02019690933E0280933D02C5C36A |
:103130008930910509F4E1C20A970CF054C220913C |
:10314000EC063091ED064091EE065091EF0660914D |
:10315000FA067091FB068091FC069091FD060E9494 |
:1031600071507B018C012091E4063091E50640917D |
:10317000E6065091E7060E94B74E26EE34E045E39E |
:103180005FEB0E94715020E030E040E056E40E9486 |
:1031900071500E94464FDC01CB010E94303A90935F |
:1031A0005505809354056091E4067091E506809181 |
:1031B000E6069091E7069058A80197010E94B74E45 |
:1031C00024ED3BE64DE852EC0E9471507B018C01EE |
:1031D0002091EC063091ED064091EE065091EF06FD |
:1031E0006091F4067091F5068091F6069091F706CD |
:1031F0000E9471509B01AC01C801B7010E94004FB1 |
:103200000E94464FDC01CB0164E670E00E947B39EE |
:1032100090935705809356050E94983A97FD30C3C6 |
:10322000880F892F881F990B909352028093510227 |
:1032300080915605909157050E942F3997FD1DC327 |
:10324000AC01440F452F441F550B5093500240933F |
:103250004F02209151023091520284EB90E0821B88 |
:10326000930B90934E0280934D0280915405909160 |
:103270005505880F991F88589F4F840F951F90936D |
:103280004C0280934B022C543F4F30934A022093C0 |
:103290004902440F551F841B950B9093480280935D |
:1032A000470210923A0280913F02909140029F9310 |
:1032B0008F9380914102909142029F938F938091CE |
:1032C0004302909144029F938F9380914502909185 |
:1032D00046029F938F9386EE91E007C1853091055A |
:1032E00009F4D0C106970CF418C18091E6029091C0 |
:1032F000E702019709F48AC210923A0287E192E04C |
:103300009F938F9382E08F930E94DD0FE0913A02AA |
:10331000FF27E55AFE4F8DE080830F900F900F90AE |
:1033200080910A01813009F4A1C288E090E0909375 |
:103330003E0280933D02C1C210923A0280918B02FC |
:1033400090918C029F938F938FE891E099CE20917A |
:10335000EC063091ED064091EE065091EF0660913B |
:10336000FA067091FB068091FC069091FD060E9482 |
:1033700071507B018C012091E4063091E50640916B |
:10338000E6065091E7060E94B74E26EE34E045E38C |
:103390005FEB0E94715020E030E040E056E40E9474 |
:1033A00071500E94464FDC01CB010E94303A90934D |
:1033B0005505809354056091E4067091E50680916F |
:1033C000E6069091E7069058A80197010E94B74E33 |
:1033D00024ED3BE64DE852EC0E9471507B018C01DC |
:1033E0002091EC063091ED064091EE065091EF06EB |
:1033F0006091F4067091F5068091F6069091F706BB |
:103400000E9471509B01AC01C801B7010E94004F9E |
:103410000E94464FDC01CB0164E670E00E947B39DC |
:1034200090935705809356050E94983A97FD8AC15C |
:10343000880F892F881F990B909352028093510215 |
:1034400080915605909157050E942F3997FD77C1BD |
:10345000AC01440F452F441F550B5093500240932D |
:103460004F02209151023091520284EB90E0821B76 |
:10347000930B90934E0280934D028091540590914E |
:103480005505880F991F88589F4F840F951F90935B |
:103490004C0280934B022C543F4F30934A022093AE |
:1034A0004902440F551F841B950B9093480280934B |
:1034B000470210923A0280913F02909140029F93FE |
:1034C0008F9380914102909142029F938F938091BC |
:1034D0004302909144029F938F9380914502909173 |
:1034E00046029F938F938CE692E09F938F9382E0A6 |
:1034F0008F930E94DD0F80913D0290913E020196D4 |
:1035000090933E0280933D028DB79EB70B960FB607 |
:10351000F8949EBF0FBE8DBFD0C18091470290919D |
:1035200048029F938F938091490290914A029F9302 |
:103530008F9380914B0290914C029F938F93809137 |
:103540004D0290914E029F938F9385E592E09F9359 |
:103550008F9382E08F930E94DD0FE0913A02FF2764 |
:10356000E55AFE4F8DE080838DB79EB70B960FB660 |
:10357000F8949EBF0FBE8DBF80910A01813009F47F |
:10358000D7C080914D0290914E0290934602809355 |
:10359000450280914B0290914C029093440280939B |
:1035A00043028091490290914A0290934202809393 |
:1035B000410280914702909148029093400280938B |
:1035C0003F0280913D0290913E02019690933E020F |
:1035D00080933D0272C10E9709F4F8C010923E022A |
:1035E00010923D026AC110923A0280919606909123 |
:1035F00097069F938F938091C0069091C1069F93E9 |
:103600008F938091990690919A069F938F938091C2 |
:10361000B2059091B3059F938F9380EA91E09F93B9 |
:103620008F9382E08F930E94DD0FE0913A02FF2793 |
:10363000E55AFE4F8DE080838DB79EB70B960FB68F |
:10364000F8949EBF0FBE8DBF80910A01813009F0B2 |
:1036500065CD2CCD8091080390910903892B09F049 |
:103660009DC010923A028091EE029091EF029F93DA |
:103670008F938091EC029091ED029F938F9386E659 |
:1036800091E033CD80919306823008F4FCC0109213 |
:103690003A02609187067091880680918906909120 |
:1036A0008A062AE030E040E050E00E94EA51CA0178 |
:1036B000B90124E230E040E050E00E94765124E677 |
:1036C00030E040E050E00E94EA515F934F933F9317 |
:1036D0002F9385E492E009CD10923A028091C402C2 |
:1036E0009091C5029F938F9380910601909107015D |
:1036F0009F938F9385EC92E0F8CC80914702909154 |
:1037000048029F938F938091490290914A029F9320 |
:103710008F9380914B0290914C029F938F93809155 |
:103720004D0290914E029F938F938FEC91E00FCFBB |
:1037300010920A0180915B018093CE0022CF8158C4 |
:103740009F4F86CE81589F4F73CE20917706309140 |
:10375000780680914F0690915006281B390B1092E5 |
:103760003A0280918D0290918E029F938F933F93A6 |
:103770002F9384EA92E0B9CC892B09F02FCF1092D5 |
:103780003A0280913103909132036AE070E00E9426 |
:10379000B5517F936F938EED92E072CC10923A0206 |
:1037A0008091990290919A029F938F9380919702B2 |
:1037B000909198029F938F938EE491E096CC109213 |
:1037C0000A0180915B018093CE002ECC10923A02C8 |
:1037D00082E391E09F938F9382E08F930E94DD0FAD |
:1037E000E0913A02FF27E55AFE4F8DE080830F906B |
:1037F0000F900F9080910A01813009F0EFCE109266 |
:103800000A0180915B018093CE00E8CE10923A02CB |
:103810006091000370910103809102039091030372 |
:103820002AE030E040E050E00E94C8515F934F939F |
:103830003F932F9386E292E09F938F9382E08F9342 |
:103840000E94DD0FE0913A02FF27E55AFE4F8DE01E |
:1038500080838DB79EB707960FB6F8949EBF0FBEB4 |
:103860008DBF80910A01813009F05FCD10920A016D |
:1038700080915B018093CE0058CD81589F4FE0CC62 |
:1038800081589F4FCDCC10923A0286E392E09F93ED |
:103890008F9382E08F930E94DD0FE0913A02FF2721 |
:1038A000E55AFE4F8DE080830F900F900F9080912E |
:1038B0000A01813009F032CCF9CBDF91CF911F9111 |
:1038C0000F91FF90EF9008950F931F9310923A027B |
:1038D0008AE093E09F938F9302E00F930E94DD0FA5 |
:1038E000E0913A02FF27E55AFE4F1DE010830F904A |
:1038F0000F900F9080910A01813009F486C08CE20C |
:1039000091E00E94F51410923A0285E093E09F93B3 |
:103910008F930F930E94DD0FE0913A02FF27E55A43 |
:10392000FE4F10830F900F900F9080910A0181300D |
:1039300009F464C08CE291E00E94F51410923A02FE |
:103940008DEF92E09F938F930F930E94DD0FE09194 |
:103950003A02FF27E55AFE4F10830F900F900F9009 |
:1039600080910A01813009F442C08CE291E00E940A |
:10397000F51410923A0283EF92E09F938F930F9386 |
:103980000E94DD0FE0913A02FF27E55AFE4F1083B7 |
:103990000F900F900F9080910A01813009F18CE215 |
:1039A00091E00E94F51410923A028EEE92E09F93FD |
:1039B0008F930F930E94DD0FE0913A02FF27E55AA3 |
:1039C000FE4F10830F900F900F9080910A0181306D |
:1039D00019F510920A0180915B018093CE001CC002 |
:1039E00010920A0180915B018093CE00D8CF109293 |
:1039F0000A0180915B018093CE00B7CF10920A013B |
:103A000080915B018093CE0095CF10920A01809146 |
:103A10005B018093CE0073CF1F910F9108952F9279 |
:103A20003F924F925F926F927F928F929F92AF924E |
:103A3000BF92CF92DF92EF92FF920F931F93CF939B |
:103A4000DF93CDB7DEB726970FB6F894DEBF0FBE73 |
:103A5000CDBF10927A006091310270913202809154 |
:103A60002B0290912C020E94B5517E836D83609150 |
:103A700033027091340280912D0290912E020E94A7 |
:103A8000B5517C836B8360913502709136028091D1 |
:103A90002F02909130020E94B5517A836983609120 |
:103AA000290270912A0280912102909122020E94A3 |
:103AB000B5512091910230919202261B370B309321 |
:103AC0008E0220938D0210922A02109229021092E7 |
:103AD0002202109221024091A602842F69E10E94E5 |
:103AE0009551913009F4DEC14F5F4093A602A0903A |
:103AF0007302B0907402C0907502D09076028D81EE |
:103B00009E81EFEF843C9E070CF0A2C10F2EF0E9DE |
:103B1000EF2EF2ECFF2EF5EF0F2FFCEB1F2FF02D09 |
:103B20002BE33FED4FE75FE3C601B5010E94715003 |
:103B30009B01AC01C801B7010E94B84E1B012C01CA |
:103B4000609373027093740280937502909376026F |
:103B5000A0907702B0907802C0907902D0907A025B |
:103B60002B813C816FEF243C36070CF05EC10F2E99 |
:103B7000F0E9EF2EF2ECFF2EF5EF0F2FFCEB1F2FED |
:103B8000F02D2BE33FED4FE75FE3C601B5010E9447 |
:103B900071509B01AC01C801B7010E94B84E3B01B6 |
:103BA0004C0160937702709378028093790290932E |
:103BB0007A0280919B0290919C020E964D9708F498 |
:103BC000E3C08091A002882309F4DEC080910F0237 |
:103BD00090911002909342058093410580911102CB |
:103BE00090911202909344058093430580911302B3 |
:103BF0009091140290934605809345052D813E8156 |
:103C0000B901882777FD8095982F0E94634FA20104 |
:103C100091010E94B74E0E94464F709362026093DA |
:103C20006102EB81FC81BF01882777FD8095982F89 |
:103C30000E94634FA40193010E94B74E0E94464F19 |
:103C4000709364026093630229813A81B9018827E5 |
:103C500077FD8095982F0E94634F20917B023091D1 |
:103C60007C0240917D0250917E020E94B74E0E94DC |
:103C7000464F709366026093650210922C02109278 |
:103C80002B02109232021092310210922E021092E8 |
:103C90002D021092340210923302109230021092D0 |
:103CA0002F02109236021092350260912302709119 |
:103CB000240280911B0290911C020E94B5517093C6 |
:103CC00068026093670260912502709126028091DC |
:103CD0001D0290911E020E94B55170936A0260937A |
:103CE0006902609127027091280280911F029091D1 |
:103CF00020020E94B55170936C0260936B02109287 |
:103D00001C0210921B0210922402109223021092A5 |
:103D10001E0210921D02109226021092250210928D |
:103D2000200210921F0210922802109227028FEE9A |
:103D300080937A0080916D0290916E028D3391058F |
:103D40000CF468C08CE390E090936E0280936D0257 |
:103D500080916F02909170028D3391050CF44FC0E9 |
:103D60008CE390E09093700280936F0280917102D7 |
:103D7000909172028D339105BCF18CE390E09093A9 |
:103D8000720280937102A1C0A0907B02B0907C026D |
:103D9000C0907D02D0907E0289819A81EFEF843CB1 |
:103DA0009E070CF067C00F2EF6EAEF2EFBE9FF2E00 |
:103DB000F4E40F2FFCEB1F2FF02D25EE32EF4FE731 |
:103DC0005FE3C601B5010E9471509B01AC01C801BF |
:103DD000B7010E94B84E60937B0270937C0280937F |
:103DE0007D0290937E02F2CE845C9F4F0CF06DC0FA |
:103DF00084EC9FEF909372028093710266C0845CA2 |
:103E00009F4F0CF0B3CF84EC9FEF909370028093A0 |
:103E10006F02ACCF845C9F4F0CF09ACF84EC9FEF85 |
:103E200090936E0280936D0293CFB9012D333105CB |
:103E3000B4F5882777FD8095982F0E94634F2FE671 |
:103E400032E143E05AE30E9471507B018C0199CE2C |
:103E5000BC01CD970CF5882777FD8095982F0E949F |
:103E6000634F2FE632E143E05AE30E9471507B0139 |
:103E70008C0156CEBC01CD972CF5882777FD809517 |
:103E8000982F0E94634F27E137EB41E559E30E94E9 |
:103E900071507B018C0191CF6CE370E0DCCF6CE35F |
:103EA00070E0C7CF80918F0290919002B901681B9A |
:103EB000790B70938C0260938B0230939002209365 |
:103EC0008F0212CE6CE370E0D8CF26960FB6F8942E |
:103ED000DEBF0FBECDBFDF91CF911F910F91FF903D |
:103EE000EF90DF90CF90BF90AF909F908F907F909A |
:103EF0006F905F904F903F902F9008952F923F9238 |
:103F00004F925F926F927F928F929F92AF92BF92E9 |
:103F1000CF92DF92EF92FF920F931F93CF93DF9395 |
:103F20008091CC0181508093CC018F3F09F478C2FD |
:103F300060909B0570909C057FE1671671042CF0E2 |
:103F40008091A002813009F421C3C0911F03D09158 |
:103F50002003C130D10509F4C4C280E09DE79093ED |
:103F6000C7018093C601CD2B09F0F9C16091990278 |
:103F700070919A022091EE023091EF02621B730B56 |
:103F8000882777FD8095982F0E94634F24EF3DEF9F |
:103F900044E35FE30E9471505B016C016091970202 |
:103FA000709198022091EC023091ED02621B730B2C |
:103FB000882777FD8095982F0E94634F7B018C01A5 |
:103FC00024EF3DEF44E35FEB0E9471509B01AC0195 |
:103FD000C601B5010E94B84E0E94464F2B0124EF46 |
:103FE0003DEF44E35FE3C801B7010E9471509B01BC |
:103FF000AC01C601B5010E94B84E0E94464F4B016C |
:1040000084E1681671040CF09AC12090310330905D |
:10401000320330929E0220929D02109296021092DC |
:104020009502109294021092930280919B029091BB |
:104030009C029C0197FD8EC12530310554F0880FFC |
:10404000991F880F991F820D931D90939E02809354 |
:104050009D0280919D0290919E029101281B390B37 |
:10406000C9012091070230910802409159025091F4 |
:104070005A02241B350B4091710250917202241B8D |
:10408000350B821B930B880F991F880F991F6AE0CD |
:1040900070E00E94B551EB017093A1056093A005FB |
:1040A000B301882777FD8095982F0E94634F5B01AD |
:1040B0006C012DEC3CEC4CE45FE30E9471507B0101 |
:1040C0008C01BE01882777FD8095982F0E94634F51 |
:1040D000A80197010E94834F18164CF4C801B7013C |
:1040E0000E94464F7093A1056093A005EB012DEC53 |
:1040F0003CEC4CE45FEBC601B5010E9471507B01C2 |
:104100008C01BE01882777FD8095982F0E94634F10 |
:10411000A80197010E94804F88234CF4C801B70181 |
:104120000E94464FEB017093A1056093A005C33335 |
:10413000D1050CF4F8C0C2E3D0E0D093A105C09340 |
:10414000A005A0902F03B09030038A0C9B1C66271B |
:10415000772768197909C0902D03D0902E034C0C55 |
:104160005D1CEE24FF24E418F508809193029091E1 |
:104170009402860F971F909394028093930227E2F4 |
:10418000813192070CF4C4C080E197E290939402CD |
:104190008093930280919502909196028E0D9F1DBF |
:1041A000909396028093950227E2813192070CF456 |
:1041B000A4C080E197E29093960280939502CB0190 |
:1041C000880F991F860F971F880F991F4091030230 |
:1041D000509104022091550230915602421B530B1C |
:1041E00020916F0230917002421B530B9A01220FF3 |
:1041F000331F220F331F220F331F241B350B820F57 |
:10420000931F6AE070E00E94B551CE01D7FDB9C19D |
:10421000F301E80FF91FE617F7070CF4BF010027B9 |
:1042200011270E1B1F0B9801061717070CF49B0193 |
:10423000C3018C0F9D1F820F931F97FDDDC08B3F25 |
:1042400091050CF4D5C08AEF80939905C301821BB8 |
:10425000930B8C0F9D1F97FDE1C08B3F91050CF4D4 |
:10426000D9C08AEF90E080939A05C701880F991F03 |
:104270008E0D9F1D880F991F4091050250910602D7 |
:104280002091570230915802421B530B20916D022E |
:1042900030916E02421B530B9A01220F331F220FE3 |
:1042A000331F220F331F241B350B820F931F6AE02D |
:1042B00070E00E94B5519F016E177F070CF49B01BF |
:1042C000B801021713070CF4B9019301260F371F29 |
:1042D0002C1B3D0B2B3F31050CF496C08AEF8093CD |
:1042E0009F05661A770AB3016C1B7D0B6B3F710546 |
:1042F0000CF484C06AEF70E0A0C1805F984D0CF0B0 |
:104300005ECF80EF98ED909396028093950257CF01 |
:10431000805F984D0CF03ECF80EF98ED9093940223 |
:104320008093930237CF3FEFCE3CD3070CF009CFF9 |
:10433000CEECDFEFD093A105C093A00502CF209073 |
:1043400031033090320380919B0290919C029C013A |
:1043500097FF72CE22273327281B390B6DCE609131 |
:10436000EE027091EF02709561957F4F882777FD7F |
:104370008095982F0E94634F24EF3DEF44E35FE365 |
:104380000E9471505B016C016091EC027091ED0232 |
:10439000709561957F4F882777FD8095982F0E94B3 |
:1043A000634F7B018C0124EF3DEF44E35FEB0E9400 |
:1043B00071509B01AC01C601B5010E94B84E0E942C |
:1043C000464F2B0124EF3DEF44E35FE3C801B70103 |
:1043D0000E9471509B01AC01C601B5010E94B84E0C |
:1043E0000E94464F4B0110929C0210929B0208CEF5 |
:1043F0008F3091050CF028CF8FE026CF6F307105FC |
:104400000CF01BC16FE019C12F3031050CF0B7C0A3 |
:104410008FE065CF8F3091050CF025CF8FE023CF53 |
:1044200080916705482F55278091970290919802B7 |
:104430009C01220F331F280F391FE0915805FF27D9 |
:10444000EE0FFF1FE255FD4F80819181BC01469F19 |
:10445000C001479F900D569F900D1124280F391FC2 |
:1044600037FDDAC035952795359527953093980215 |
:10447000209397028091990290919A029C01220FB9 |
:10448000331F280F391FE0915905FF27EE0FFF1F3B |
:10449000E255FD4F80819181BC01469FC001479F3D |
:1044A000900D569F900D1124280F391F37FDB1C074 |
:1044B000359527953595279530939A022093990243 |
:1044C000E0915B05FF27EE0FFF1FE255FD4F808156 |
:1044D0009181909581959F4F90939C0280939B0230 |
:1044E00027CD8091A002813009F037CD809106015F |
:1044F00090910701843691050CF47BC080910003F4 |
:1045000090910103A0910203B091030388379105B4 |
:10451000A105B10508F46DC000E01DE71093C701C7 |
:104520000093C60120E030E040EA50E46091C80109 |
:104530007091C9018091CA019091CB010E94B84E3F |
:104540006B017C01B801882777FD8095982F0E9428 |
:10455000634F17FD6BC020E030E04AEF56E40E9445 |
:10456000004F9B01AC01C701B6010E9471500E942F |
:10457000464F3B0170939C0560939B05F4CC822FC2 |
:10458000AECE882799278C1B9D0B42CEC0911F036E |
:10459000D0912003209709F0DCCC27E73EEB4FE7D2 |
:1045A0005FE36091C8017091C9018091CA01909147 |
:1045B000CB010E9471507B018C01B301882777FDEC |
:1045C0008095982F0E94634F2FE632E143E85AE32B |
:1045D0000E9471509B01AC01C801B7010E94B84E06 |
:1045E0006093C8017093C9018093CA019093CB0175 |
:1045F000B4CC0091C6011091C7010115110579F4E1 |
:104600001092A002C0E0D0E01092200310921F038D |
:1046100089CF2D5F3F4F4CCF2D5F3F4F23CF0150B0 |
:1046200010401093C7010093C6017CCF20E030E01A |
:1046300040E857E40E94B84E8ECF60939D05B0923B |
:104640003405A0923305D0923605C0923505C101DC |
:104650006AE070E00E94B5517093380560933705A9 |
:10466000DF91CF911F910F91FF90EF90DF90CF904E |
:10467000BF90AF909F908F907F906F905F904F9082 |
:104680003F902F9008958091A002882381F5109289 |
:104690009A051092990510929D0510929F058091A0 |
:1046A000E5018823E9F48091E601882391F4809163 |
:1046B000E701882339F48091E8018823C1F08093D1 |
:1046C0009D0515C080939F058091E8018823B9F767 |
:1046D0000EC080939A058091E701882361F3F2CFA1 |
:1046E000809399058091E601882309F3F2CF109217 |
:1046F000A7021092A8020E94C926089588EE93E0AE |
:104700000E9413158091600580FF0BC08091080105 |
:10471000909109018E5E9240893C910510F00E94B3 |
:10472000F71680918D0290918E0290939002809363 |
:104730008F028CE291E00E94F51410927A00809131 |
:104740000F0290911002909356028093550280912F |
:104750001102909112029093580280935702809117 |
:1047600013029091140290935A02809359028091FF |
:1047700015029091160290935E0280935D028091E3 |
:1047800017029091180290935C0280935B028091D3 |
:10479000190290911A029093600280935F0280E068 |
:1047A00090E0A0E0B0E08093730290937402A09335 |
:1047B0007502B09376028093770290937802A0936B |
:1047C0007902B0937A0280937B0290937C02A0934B |
:1047D0007D02B0937E0280937F0290938002A0932B |
:1047E0008102B09382028093830290938402A0930B |
:1047F0008502B09386028093870290938802A093EB |
:104800008902B0938A0210922C0210922B0210920D |
:1048100032021092310210922E0210922D0210924A |
:104820003402109233021092300210922F02109232 |
:1048300036021092350210926802109267021092AE |
:104840006A021092690210926C0210926B0210922E |
:104850001C0210921B02109224021092230210924A |
:104860001E0210921D021092260210922502109232 |
:10487000200210921F0210922802109227028FEE3F |
:1048800080937A00109296021092950210929402F0 |
:1048900010929302809131039091320390939E0283 |
:1048A00080939D028091080190910901909392025A |
:1048B0008093910280ED97E09093FD018093FC013D |
:1048C0000895E0915A05FF27EE0FFF1FE255FD4FB7 |
:1048D0000190F081E02DE858FF4FF0939C05E093A4 |
:1048E0009B05F7FD46C080919F02843668F4809155 |
:1048F0000101882341F480910201909103018F5FAF |
:104900009F4F09F479C008958D38E8F3E0919B0535 |
:10491000F0919C05E932F1056CF08091A102909133 |
:10492000A2022FEF8F3F920729F001969093A202E7 |
:104930008093A102E93CF1050CF5B39724F7E091CF |
:104940005B05FF27EE0FFF1FE255FD4F0190F08141 |
:10495000E02D8FEFE53BF8070CF05BC08091A402DF |
:104960008F5F8093A402853608F06EC01092A30278 |
:10497000089510929C0510929B05B5CF8091A002DE |
:10498000882309F0C0CFE0915B05FF27EE0FFF1FE2 |
:10499000E255FD4F808191818C3491050CF451C01A |
:1049A0008091A5028F5F8093A502833308F4ABCF7B |
:1049B0001092A0021092A5021092A2021092A102DF |
:1049C0008091600580FF0AC08091080190910901E3 |
:1049D0008E5E9240893C910508F043C00E940E0310 |
:1049E00041E468E575E00E9435050E947E23E09170 |
:1049F0009B05F0919C05A1CF88E99AE39093FD0176 |
:104A00008093FC0180E09CE090930301809302017D |
:104A100008951092A402EC34F1050CF4A7CF809114 |
:104A2000A3028F5F8093A302833308F46CCF1092AC |
:104A3000A0021092A2021092A10282E38093A3022C |
:104A400008951092A502089581E08093A00281E06C |
:104A500090E09093A2028093A10284E68093A40246 |
:104A600085CF0E94F716BACF81E08093580552E0B7 |
:104A70005093590543E040935A0584E080935B05C9 |
:104A800065E060935C0586E080935D0587E0809338 |
:104A90005E0538E030935F0581EC809360058EE120 |
:104AA0008093610524E6209363058AE08093640582 |
:104AB00092E3909362059093660550936505409349 |
:104AC000670510926805309369058FE080936A0549 |
:104AD0008AEF80936B058AE180936C0580E8809370 |
:104AE0006D0588EC80936E058FEA80936F058EE5E7 |
:104AF0008093700583E28093710584E18093720551 |
:104B00001092730510927405109275051092760537 |
:104B100010927705109278052093790588E28093AA |
:104B20007A051092880590937B0586E980937C0531 |
:104B300060937D0590937E058AE580937F05909331 |
:104B40008005109287052093810580E5809382057A |
:104B5000209383052093840580E180938505ADE84B |
:104B6000B5E0EBEAF1E08CE001900D928150E1F7C5 |
:104B7000089581E08093580532E03093590583E031 |
:104B800080935A0594E090935B0545E040935C0563 |
:104B900086E080935D0587E080935E0588E08093E2 |
:104BA0005F0580E4809360058EE1809361058BEF63 |
:104BB000809363058AE08093640522E32093620575 |
:104BC00020936605309365059093670510926805FC |
:104BD000809369058FE080936A058AEF80936B0567 |
:104BE0008AE180936C0580E880936D058FEA80935D |
:104BF0006E0580936F058EE58093700583E2809348 |
:104C0000710584E18093720510927305109274050A |
:104C1000109275051092760510927705109278051E |
:104C200094E69093790588E280937A05109288053E |
:104C300020937B0586E980937C0540937D05209336 |
:104C40007E058AE580937F05209380051092870575 |
:104C50009093810580E580938205909383059093DE |
:104C6000840580E180938505ADE8B5E0E5EBF1E0F2 |
:104C70008CE001900D928150E1F7089581E08093DE |
:104C8000580582E08093590583E080935A0594E0AB |
:104C900090935B0555E050935C0586E080935D053D |
:104CA00087E080935E0538E030935F0580E4809371 |
:104CB00060058EE1809361058BEF809363054AE088 |
:104CC0004093640522E32093620520936605909348 |
:104CD000650590936705309368058EE080936905BC |
:104CE0008FE080936A058AEF80936B058AE1809359 |
:104CF0006C0580E880936D0588E780936E0536E942 |
:104D000030936F0580E68093700583E28093710590 |
:104D100084E18093720510927305109274051092CD |
:104D2000750510927605109277051092780594E635 |
:104D30009093790588E280937A05109288052093F4 |
:104D40007B0530937C0550937D0520937E058AE595 |
:104D500080937F05209380051092870590938105AD |
:104D600086E4809382059093830590938405409315 |
:104D70008505ADE8B5E0EDEBF1E08CE001900D923A |
:104D80008150E1F708951092B9008AE28093B8004B |
:104D9000089585EA8093BC0080E090E0089584E95E |
:104DA0008093BC0008951092B9008093BB0085E801 |
:104DB0008093BC0080E090E008951F920F920FB6A0 |
:104DC0000F9211248F939F93EF93FF938091A702EB |
:104DD0008F5F8093A7028150833049F18430A0F027 |
:104DE000853009F45EC0853070F58091A90281306C |
:104DF00009F477C0813008F461C0823009F48CC0B6 |
:104E0000833009F096C07EC0813009F461C08230E1 |
:104E1000E0F184E98093BC008091A802843008F01E |
:104E20006AC01092A70285EA8093BC0082C080917C |
:104E3000A902880F8D5A1092B9008093BB0085E8B3 |
:104E40008093BC0076C0863009F470C0873009F0CA |
:104E500070C09091A9028091BB00E92FFF27E9550E |
:104E6000FA4F84839F5F9093A902943010F01092C0 |
:104E7000A90284E98093BC008AE090E09093C50188 |
:104E80008093C4011092A70254C08091A802880F99 |
:104E90008E5A1092B9008093BB0085E88093BC00C5 |
:104EA00048C08091BB009091A902E92FFF27E955E6 |
:104EB000FA4F80839130A9F0913000F580919905E7 |
:104EC0001092B9008093BB0085E88093BC0031C08C |
:104ED0008091A8028F5F8093A8028150813009F0F1 |
:104EE00089CF80919A051092B9008093BB0085E824 |
:104EF0008093BC001EC01092A80295CF923061F042 |
:104F00009330B9F480919F051092B9008093BB0053 |
:104F100085E88093BC000DC080919D051092B9007A |
:104F20008093BB0085E88093BC0003C09091A902E8 |
:104F3000C1CF8091BC0080688093BC00FF91EF914D |
:104F40009F918F910F900FBE0F901F9018954CEB73 |
:104F500050E084E9DA018C931092A7022BEB30E049 |
:104F6000F90180811092A80280E88C931092BD0014 |
:104F70001092BA001082E9EBF0E01082A8EBB0E0EA |
:104F80001C9210828AE28C9385EADA018C9310825B |
:104F9000F901108285E88C93089583EC8093810059 |
:104FA000EFE6F0E080818062808308951F920F9287 |
:104FB0000FB60F9211246F927F928F929F92AF92B1 |
:104FC000BF92CF92DF92EF92FF920F931F932F9396 |
:104FD0003F934F935F936F937F938F939F93AF9381 |
:104FE000BF93CF93DF93EF93FF932091860030918F |
:104FF00087008091E2029091E302281B390B809197 |
:105000008600909187009093E3028093E202C901A9 |
:105010008D5D95408356994148F481E090E09093EE |
:10502000E1028093E0021092CC0122C1A090E00244 |
:10503000B090E1028AE0A816B1040CF019C1C901D0 |
:105040008B5F9040845B914008F45CC023E333E3C2 |
:1050500043E75FE36091DC027091DD028091DE0244 |
:105060009091DF020E94715020E030E040EA50E46D |
:105070000E94B84E6093DC027093DD028093DE02E2 |
:105080009093DF027B018C0120E030E048EC52E499 |
:10509000C801B7010E94834F18160CF0AFC00F2E45 |
:1050A000F0E0EF2EF0E0FF2EF8EC0F2FF2E41F2FD0 |
:1050B000F02D20E030E080E090E0E092DC02F09221 |
:1050C000DD020093DE021093DF023093C5022093CD |
:1050D000C40290934C0580934B05C50101969093B3 |
:1050E000E1028093E0028530910509F491C05D985A |
:1050F0008630910509F4A8C05C98079709F4A7C009 |
:105100005B98B6C06EE2862E6EEF962E820E931ED0 |
:105110006501CC0CDD1CF601E255FD4F608071800D |
:10512000C4018619970905960B9728F480919F0270 |
:10513000883C08F48EC0F601EA53FD4F80819181CE |
:10514000E401C81BD90BCC0FDD1FE090DC02F0900E |
:10515000DD020091DE021091DF02C801B7010E945A |
:10516000464F6C177D070CF057C024EA30E74DE737 |
:105170005FE3C801B7010E9471507B018C01BE0141 |
:10518000D7FD71C0882777FD8095982F0E94634FC7 |
:105190002AE037ED43E25CE30E9471509B01AC01D1 |
:1051A000C801B7010E94B84E6093DC027093DD0223 |
:1051B0008093DE029093DF02F601E255FD4FC301BA |
:1051C000880F991F860D971D880D991D97FD45C065 |
:1051D00095958795959587959183808386EC92E048 |
:1051E000C80ED91EF60191828082E090DC02F09018 |
:1051F000DD020091DE021091DF0246CFC801B70147 |
:105200000E94464F84E690E0861B970B9C0155CF89 |
:105210005D9A5C985B982CC02EEE3CE74FE75FE30D |
:10522000C801B7010E9471507B018C01BE01D7FDFE |
:1052300016C0882777FD8095982F0E94634F2FE630 |
:1052400032E143E05BE3A8CF5C9A5B9811C05B9AC4 |
:105250000FC0865F80939F026ECF0396B9CF709583 |
:1052600061957F4FE6CF662777276C1B7D0B8ACF32 |
:10527000FF91EF91DF91CF91BF91AF919F918F916E |
:105280007F916F915F914F913F912F911F910F915E |
:10529000FF90EF90DF90CF90BF90AF909F908F9056 |
:1052A0007F906F900F900FBE0F901F9018956A35EA |
:1052B00071058105910548F144E050E080910A03B1 |
:1052C00090910B0397FD29C020919D0230919E0281 |
:1052D0002817390764F4821B930B8850974074F0A9 |
:1052E000241B350B30939E0220939D020895B90133 |
:1052F000681B790BCB018850974094F3420F531FE2 |
:1053000050939E0240939D02089540E050E08091AA |
:105310000A0390910B0397FFD7CF805F914F909333 |
:105320000B0380930A03D0CF9FB7F8948091C900F4 |
:105330008F778093C9008091C9008F7B8093C900CB |
:105340008091C9008F7D8093C9005A9A52985B9AC8 |
:10535000539A1092CD0080E28093CC001092C80046 |
:1053600088E18093C9008091CA008F778093CA003A |
:105370008091CA008F7B8093CA008091CA008F7D84 |
:105380008093CA008091CA008F7E8093CA0080916A |
:10539000CA00877F8093CA008091C9008B7F809369 |
:1053A000C9008091CA0084608093CA008091CA00BD |
:1053B00082608093CA008091C80087FF06C08091F8 |
:1053C000CE008091C80087FDFACF8091C900806827 |
:1053D0008093C9008091C90080648093C9009FBFF9 |
:1053E0001092310608958091C505813009F4A2C05C |
:1053F00080914406813009F448C08091BF06813015 |
:1054000009F0089580919F069091A006A091A106B1 |
:10541000B091A20680937B0690937C06A0937D06B4 |
:10542000B0937E068091A3069091A406A091A50654 |
:10543000B091A60680937F0690938006A093810684 |
:10544000B09382068091A7069091A806A091A90624 |
:10545000B091AA068093830690938406A093850654 |
:10546000B09386068091AF069091B006A091B106E8 |
:10547000B091B2068093870690938806A093890620 |
:10548000B0938A061092BF06089560913A06709113 |
:105490003B0680913C0690913D062AE030E040E0DA |
:1054A00050E00E94EA5120936F06309370064093BB |
:1054B000710650937206609136067091370680919E |
:1054C0003806909139062AE030E040E050E00E9432 |
:1054D000EA512093730630937406409375065093F7 |
:1054E000760660913E0670913F068091400690914D |
:1054F000410624E630E040E050E00E94EA5120936B |
:105500007706309378064093790650937A06809117 |
:10551000320690913306A0913406B09135068093FF |
:105520008F0690939006A0939106B09392061092E6 |
:10553000440663CF8091B9058093930680919306CA |
:10554000823030F08091E4029091E502892B19F0CD |
:105550001092C5054DCF85B192E0892785B910928B |
:10556000C50546CF2F923F924F925F926F927F92E6 |
:105570008F929F92AF92BF92CF92DF92EF92FF9263 |
:105580000F931F93CF93DF93CDB7DEB72E970FB650 |
:10559000F894DEBF0FBECDBFE0915E05FF27EE0F92 |
:1055A000FF1FE255FD4F0190F081E02D0FEFEC3928 |
:1055B000F0070CF0ACC240E050E0E0915905FF2745 |
:1055C000EE0FFF1FE255FD4F0190F081E02DF7FD3A |
:1055D000A9C33B977CF4E0915B05FF27EE0FFF1F0B |
:1055E000E255FD4F0190F081E02DF7FD2FC43B9770 |
:1055F0000CF42DC381E090E090930903809308039D |
:1056000080919306833038F08091E8029091E9020E |
:10561000892B09F4E5C280919306833020F041156F |
:10562000510509F4A6C280919306833020F44115F8 |
:10563000510509F499C280919306833058F0809106 |
:10564000080390910903019709F418C342305105EA |
:1056500009F414C380919306833038F480910803D1 |
:1056600090910903019709F461C3809193068330F7 |
:1056700020F04115510509F462C220911F033091B9 |
:10568000200360910C0370910D0383E0683E78075E |
:1056900034F46F5F7F4F70930D0360930C034B01E5 |
:1056A00080919306833008F439C24230510509F4E1 |
:1056B00042C34130510509F408C42130310509F4D1 |
:1056C000D0C31092F0021092F1021092F2021092E6 |
:1056D000F3021092F4021092F5021092F602109268 |
:1056E000F7021092F8021092F9021092FA02109248 |
:1056F000FB021092FC021092FD021092FE02109228 |
:10570000FF02109200031092010310920203109204 |
:10571000030310920D0310920C03B401882777FD48 |
:105720008095982F2CE830E040E050E00E947651C0 |
:1057300028EE33E040E050E00E94EA5119012A01CE |
:105740006090F0027090F1028090F2029090F3026B |
:10575000EE24FF248701E21AF30A040B150BE986F5 |
:10576000FA860B871C87C401B301660F771F881F59 |
:10577000991F660F771F881F991F660F771F881F55 |
:10578000991F22E330E040E050E00E94EA51D2014C |
:10579000C101221533054405550514F4DA01C90188 |
:1057A00009851A852B853C8580179107A207B307C9 |
:1057B00014F4D901C8019093B3058093B2056091A8 |
:1057C000F8027091F9028091FA029091FB02660F43 |
:1057D000771F881F991F660F771F881F991F22E365 |
:1057E00030E040E050E00E94EA51D201C1012215B0 |
:1057F00033054405550514F4DA01C90129853A85B4 |
:105800004B855C85281739074A075B0714F49C0110 |
:10581000AD013093C1062093C006A090F402B09071 |
:10582000F502C090F602D090F702C601B501660FEE |
:10583000771F881F991F660F771F881F991F660F94 |
:10584000771F881F991F22E330E040E050E00E945C |
:10585000EA51D201C101221533054405550514F45E |
:10586000DA01C90129853A854B855C8528173907F6 |
:105870004A075B0714F49C01AD0130939A0620930C |
:1058800099066091FC027091FD028091FE02909158 |
:10589000FF02660F771F881F991F660F771F881FEB |
:1058A000991F22E330E040E050E00E94EA51D2012B |
:1058B000C101221533054405550514F4DA01C90167 |
:1058C00029853A854B855C85281739074A075B0788 |
:1058D00014F49C01AD013093970620939606662739 |
:1058E0007727CB01661977098809990928E030E004 |
:1058F00040E050E00E94EA5179018A01662777274B |
:10590000CB016A197B098C099D0928E030E040E051 |
:1059100050E00E94EA51DA01C901B7010E947B39C7 |
:105920009C01220F331F220F331F220F331F880FBA |
:10593000991F820F931F9E878D8790930B038093EF |
:105940000A03A090B205B090B3058091C006909173 |
:10595000C106A80EB91EB0920703A0920603E090FC |
:105960009906F0909A068091960690919706E80E17 |
:10597000F91EF094E194F108F394F0920503E0929B |
:1059800004036090290370902A0380902B03909069 |
:105990002C03C401B3010E94B9506D837E838F83B1 |
:1059A0009887C401B3010E94FA4E69837A838B837E |
:1059B0009C83CC24B7FCC094DC2CC601B5010E94AA |
:1059C000634F3B014C010027F7FC0095102FC801E5 |
:1059D000B7010E94634F5B016C01A40193016981CF |
:1059E0007A818B819C810E9471507B018C01A60180 |
:1059F00095016D817E818F8198850E9471509B01F8 |
:105A0000AC01C801B7010E94B74E0E94464F9B01EE |
:105A1000442737FD4095542FD201C101221533058B |
:105A20004405550514F4DA01C90129853A854B85E9 |
:105A30005C85281739074A075B0714F49C01AD0100 |
:105A40003093ED022093EC02A40193016D817E81DD |
:105A50008F8198850E9471507B018C01A601950170 |
:105A600069817A818B819C810E9471509B01AC017C |
:105A7000C801B7010E94B84E0E94464F882777FDA3 |
:105A80008095982FA20191016215730584059505F3 |
:105A900014F49B01AC0189859A85AB85BC8582177E |
:105AA0009307A407B50714F4DA01C9019093EF0234 |
:105AB0008093EE028091000390910103A091020374 |
:105AC000B09103038A359105A105B10508F41CC105 |
:105AD00044E050E02D853E8537FD1CC180910A03CE |
:105AE00090910B0320919D0230919E022817390757 |
:105AF0000CF0FBC0821B930B885097400CF4FDC048 |
:105B0000241B350B30939E0220939D021DC2E53667 |
:105B1000F1050CF0B0C041E050E04FCD1092ED0225 |
:105B20001092EC021092EF021092EE02109200031B |
:105B300010920103109202031092030305C2809198 |
:105B40000201909103018F5F9F4F09F096CD88E885 |
:105B500093E19093FD018093FC0180E091E09093AC |
:105B600003018093020189CD1092E7021092E602B0 |
:105B700062CD80916F0690917006A0917106B091F0 |
:105B800072068093470690934806A0934906B09307 |
:105B90004A068091730690917406A0917506B091A3 |
:105BA000760680934B0690934C06A0934D06B093D7 |
:105BB0004E068091770690917806A0917906B09173 |
:105BC0007A0680934F0690935006A0935106B093A7 |
:105BD000520681E090E09093E7028093E60223CDA5 |
:105BE00080916F0690917006A0917106B091720637 |
:105BF00080930B0690930C06A0930D06B0930E06AF |
:105C00008091730690917406A0917506B091760606 |
:105C100080930F0690931006A0931106B09312067E |
:105C20008091770690917806A0917906B0917A06D6 |
:105C30008093130690931406A0931506B09316064E |
:105C400081E090E09093E9028093E802E4CCE09157 |
:105C50005805FF27EE0FFF1FE255FD4F0190F08121 |
:105C6000E02DF7FDFAC03B970CF0C4CC109209036D |
:105C700010920803C5CC42E050E09FCC80916F06A3 |
:105C800090917006A0917106B09172068093C6053E |
:105C90009093C705A093C805B093C905809173067A |
:105CA00090917406A0917506B09176068093CA050E |
:105CB0009093CB05A093CC05B093CD05809177064A |
:105CC00090917806A0917906B0917A068093CE05DE |
:105CD0009093CF05A093D005B093D10581E090E0DB |
:105CE0009093EB028093EA02B5CCD901A81BB90BC3 |
:105CF000CD01885097400CF403CF420F531F5093AF |
:105D00009E0240939D0220C140E050E02D853E85DB |
:105D100037FFE4CEC901805F914F90930B038093CE |
:105D20000A03DCCEF095E195FF4F53CC1092EB02C5 |
:105D30001092EA029ACC8091E6029091E7020197D4 |
:105D400009F0BBCC2115310509F0B7CC80910803CF |
:105D500090910903892B09F0B4CC20916F06309102 |
:105D6000700640917106509172068091470690919D |
:105D70004806A0914906B0914A06281B390B4A0BE8 |
:105D80005B0B2093F0023093F1024093F2025093A8 |
:105D9000F302A0907306B0907406C0907506D09080 |
:105DA000760680914B0690914C06A0914D06B091DD |
:105DB0004E06A81AB90ACA0ADB0AA092F402B092E7 |
:105DC000F502C092F602D092F70280917B06909184 |
:105DD0007C06A0917D06B0917E068093F802909398 |
:105DE000F902A093FA02B093FB0280917F06909192 |
:105DF0008006A0918106B09182068093FC02909368 |
:105E0000FD02A093FE02B093FF02CA01B9010E94F5 |
:105E100076517B018C01C601B501A60195010E9456 |
:105E20007651E60EF71E081F191FC801B7010E9420 |
:105E3000634F0E94CC500E94464F609300037093C2 |
:105E40000103809302039093030367CCF095E195DF |
:105E5000FF4F3B970CF0CECBFACEF095E195FF4F7C |
:105E600002CF8091E8029091E902019709F029CCD4 |
:105E700020916F0630917006409171065091720624 |
:105E800080910B0690910C06A0910D06B0910E0624 |
:105E9000281B390B4A0B5B0B2093F0023093F10265 |
:105EA0004093F2025093F302A0907306B0907406F0 |
:105EB000C0907506D090760680910F0690911006DE |
:105EC000A0911106B091120674CF8091EA029091D0 |
:105ED000EB02019709F0F1CB2115310509F0EDCB6B |
:105EE0008091080390910903892B09F0EACB209156 |
:105EF0006F06309170064091710650917206809144 |
:105F0000C6059091C705A091C805B091C905281B89 |
:105F1000390B4A0B5B0B2093F0023093F102409354 |
:105F2000F2025093F302A0907306B0907406C090F2 |
:105F30007506D09076068091CA059091CB05A09108 |
:105F4000CC05B091CD0535CF2E960FB6F894DEBFB7 |
:105F50000FBECDBFDF91CF911F910F91FF90EF90BA |
:105F6000DF90CF90BF90AF909F908F907F906F9079 |
:105F70005F904F903F902F9008951F920F920FB611 |
:105F80000F9211242F933F934F935F936F937F93BF |
:105F90008F939F93AF93BF93EF93FF938091C8002C |
:105FA0004091CE00887109F073C080913106843031 |
:105FB00009F49CC0853008F46EC0863009F4D1C065 |
:105FC000863008F4A9C0873009F405C1883009F08B |
:105FD0005FC08091B405481709F05AC0E0914506AA |
:105FE000F0914606309709F453C081E08083809198 |
:105FF000C505813009F49AC180914406813009F4C5 |
:1060000040C18091BF06813009F042C080919F0657 |
:106010009091A006A091A106B091A20680937B0664 |
:1060200090937C06A0937D06B0937E068091A30694 |
:106030009091A406A091A506B091A60680937F0634 |
:1060400090938006A0938106B09382068091A70664 |
:106050009091A806A091A906B091AA068093830604 |
:1060600090938406A0938506B09386068091AF0630 |
:106070009091B006A091B106B091B20680938706C8 |
:1060800090938806A0938906B0938A061092BF0663 |
:106090001092310660C1813009F497C0813008F454 |
:1060A00059C0823009F4A0C0833091F7409395061F |
:1060B0004F5F4093B1058091B1058F5F8093B40528 |
:1060C00084E08093310680919506883009F497C06A |
:1060D000823109F4AAC0833009F4BDC081E0809305 |
:1060E0009806109246061092450635C1842F9927CE |
:1060F00090936E0680936D068091B105480F409392 |
:10610000B1058091B4059091B105890F8093B405D4 |
:1061100085E0809331061FC120916D0630916E0697 |
:10612000842F9927982F8827820F931F90936E06AC |
:1061300080936D068091B105480F4093B105809121 |
:10614000B4059091B105890F8093B40586E08093E2 |
:10615000310601C1453B09F0FEC081E08093310664 |
:10616000FAC08091B105840F8093B1058091B40588 |
:106170009091B105890F8093B4058091980688238A |
:1061800081F4E0910906F0910A068091AF059091A3 |
:10619000B005E817F90728F44193F0930A06E09355 |
:1061A000090680916D0690916E06019790936E0698 |
:1061B00080936D0680916D0690916E06892B09F093 |
:1061C000CAC087E080933106C6C04236A1F0453B85 |
:1061D00009F05ECFC0C08091B105481709F058CFD3 |
:1061E00088E080933106B7C0413009F051CF83E099 |
:1061F00080933106B0C082E080933106ACC082E368 |
:1062000096E090930A06809309062BE937E03093D5 |
:10621000B0052093AF0542969093460680934506BD |
:10622000809144068093980696C08BE996E09093FF |
:106230000A068093090624EF3BE03093B0052093D3 |
:10624000AF05849690934606809345068091BF06DD |
:106250008093980680C085EB95E090930A06809322 |
:10626000090626ED36E03093B0052093AF05409641 |
:1062700090934606809345068091C50580939806C5 |
:106280006AC060913A0670913B0680913C069091FD |
:106290003D062AE030E040E050E00E94EA512093C1 |
:1062A0006F063093700640937106509372066091AA |
:1062B00036067091370680913806909139062AE0AB |
:1062C00030E040E050E00E94EA51209373063093A2 |
:1062D0007406409375065093760660913E06709161 |
:1062E0003F06809140069091410624E630E040E070 |
:1062F00050E00E94EA51209377063093780640934D |
:10630000790650937A068091320690913306A091D7 |
:106310003406B091350680938F0690939006A09333 |
:106320009106B0939206109244066BCE8091B90507 |
:106330008093930680919306823050F08091E4021E |
:106340009091E502892B21F485B192E0892785B9E6 |
:106350001092C50551CEFF91EF91BF91AF919F91E2 |
:106360008F917F916F915F914F913F912F910F90FE |
:106370000FBE0F901F9018952F923F924F925F92F1 |
:106380006F927F928F929F92AF92BF92CF92DF9245 |
:10639000EF92FF920F931F93CF93DF93CDB7DEB7AA |
:1063A00024970FB6F894DEBF0FBECDBF3A83298382 |
:1063B0005801121613060CF049C02C013B014A018A |
:1063C00022243324F20142815381F301228133815B |
:1063D000F401828193811A141B043CF57A0189012E |
:1063E0009C838B83CC24DD24F80124813581468174 |
:1063F0005781F70164817581868197810E94B74E2C |
:10640000EB81FC8164837583868397830894C11C28 |
:10641000D11C24E030E0E20EF31E0C5F1F4F3496D7 |
:10642000FC83EB83CA14DB04F9F60894211C311CAD |
:1064300082E090E0480E591E680E791E880E991E63 |
:10644000E981FA812E163F0609F0BCCF24960FB6DB |
:10645000F894DEBF0FBECDBFDF91CF911F910F919A |
:10646000FF90EF90DF90CF90BF90AF909F908F9074 |
:106470007F906F905F904F903F902F9008952F9254 |
:106480003F924F925F926F927F928F929F92AF92C4 |
:10649000BF92CF92DF92EF92FF920F931F93CF9311 |
:1064A000DF93CDB7DEB724970FB6F894DEBF0FBEEB |
:1064B000CDBF3A8329835801121613060CF049C048 |
:1064C0002C013B014A0122243324F20142815381F1 |
:1064D000F30122813381F401828193811A141B0418 |
:1064E0003CF57A0189019C838B83CC24DD24F8015F |
:1064F0002481358146815781F701648175818681C8 |
:1065000097810E94B84EEB81FC81648375838683FA |
:1065100097830894C11CD11C24E030E0E20EF31EE6 |
:106520000C5F1F4F3496FC83EB83CA14DB04F9F62F |
:106530000894211C311C82E090E0480E591E680E20 |
:10654000791E880E991EE981FA812E163F0609F000 |
:10655000BCCF24960FB6F894DEBF0FBECDBFDF913F |
:10656000CF911F910F91FF90EF90DF90CF90BF9050 |
:10657000AF909F908F907F906F905F904F903F90E3 |
:106580002F9008952F923F924F925F926F927F9239 |
:106590008F929F92AF92BF92CF92DF92EF92FF9233 |
:1065A0000F931F93CF93DF93CDB7DEB762970FB6EC |
:1065B000F894DEBF0FBECDBF7A8769873C872B87F3 |
:1065C0003801FE86ED86121613060CF095C09C83EA |
:1065D0008B835A8349831E821D8297012F5F3F4F11 |
:1065E000388B2F87EB81FC810280F381E02DF887C7 |
:1065F000EF832D853E85121613060CF05BC091E0EB |
:10660000492E512C84E0882E912CE981FA81228038 |
:106610003380161417040CF064C0AF80B884C984AA |
:10662000DA840F2EF0E0EF2EF0E0FF2EF0E00F2FD7 |
:10663000F0E01F2FF02D1A8A198AF601A281B3818A |
:10664000A80DB91D2D913D914D915C91F50164818D |
:106650007581868197810E9471509B01AC01C801B0 |
:10666000B7010E94B84E7B018C0129893A892F5FBE |
:106670003F4F3A8B298B84E090E0A80EB91EE2E0F0 |
:10668000F0E0CE0EDF1E26153705B9F6F401E20D57 |
:10669000F31DE082F182028313830894411C511C94 |
:1066A00024E030E0820E931E8F85988948165906A3 |
:1066B00009F0AFCFED81FE813196FE83ED832B8112 |
:1066C0003C812E5F3F4F3C832B8389819A810296C8 |
:1066D0009A8389832B853C85E217F30709F082CFE3 |
:1066E0000BC00F2EF0E0EF2EF0E0FF2EF0E00F2FAA |
:1066F000F0E01F2FF02DCACF62960FB6F894DEBFE0 |
:106700000FBECDBFDF91CF911F910F91FF90EF9002 |
:10671000DF90CF90BF90AF909F908F907F906F90C1 |
:106720005F904F903F902F9008952F923F924F92FD |
:106730005F926F927F928F929F92AF92BF92CF9211 |
:10674000DF92EF92FF920F931F93CF93DF93CDB71A |
:10675000DEB760970FB6F894DEBF0FBECDBF7A8765 |
:1067600069873C872B874801FE86ED861216130643 |
:106770000CF08EC09C838B835A8349831E821D82BA |
:10678000EB81FC810280F381E02DF887EF832D857A |
:106790003E85121613060CF059C069847A842224AF |
:1067A0003324E981FA814280538024E030E0420EB4 |
:1067B000531EF30182819381181419040CF05DC0FB |
:1067C0000F811885988B8F870F2EF0E0AF2EF0E0A9 |
:1067D000BF2EF0E0CF2EF0E0DF2EF02DEE24FF24D0 |
:1067E000EF85F8892481358146815781F8016481DC |
:1067F0007581868197810E9471509B01AC01C60111 |
:10680000B5010E94B84E5B016C010894E11CF11CBB |
:106810000C5F1F4F2F8538892C5F3F4F388B2F8798 |
:10682000E814F904E9F6F201A192B192C192D19271 |
:106830002F010894211C311C22E030E0620E731EEF |
:106840008D859E852816390609F0B3CFED81FE812E |
:106850003196FE83ED832B813C812E5F3F4F3C833D |
:106860002B8389819A8102969A8389832B853C8523 |
:10687000E217F30709F084CF0BC00F2EF0E0AF2E24 |
:10688000F0E0BF2EF0E0CF2EF0E0DF2EF02DCBCFEA |
:1068900060960FB6F894DEBF0FBECDBFDF91CF91EB |
:1068A0001F910F91FF90EF90DF90CF90BF90AF902E |
:1068B0009F908F907F906F905F904F903F902F9020 |
:1068C00008952F923F924F925F926F927F928F9294 |
:1068D0009F92AF92BF92CF92DF92EF92FF920F936F |
:1068E0001F93CF93DF93CDB7DEB762970FB6F894BF |
:1068F000DEBF0FBECDBF9E838D8378876F833A87BF |
:1069000029872801FC86EB86121613060CF094C02A |
:106910005A8349831C821B8264E0662E712C2B856E |
:106920003C85121613060CF067C051E0252E312C61 |
:1069300044E0842E912CE981FA810280F381E02DDC |
:10694000FE87ED872F5F3F4F388B2F871414150478 |
:106950000CF067C0AD80BE80CF80D8840F2EF0E0F1 |
:10696000EF2EF0E0FF2EF0E00F2FF0E01F2FF02DC4 |
:106970001A8A198AF50182819381860D971DF60185 |
:10698000A281B381A80DB91D2D913D914D915C91CE |
:10699000FC0160817181828193810E9471509B0111 |
:1069A000AC01C801B7010E94B84E7B018C01298956 |
:1069B0003A892F5F3F4F3A8B298B82E090E0A80EF7 |
:1069C000B91EC80ED91E24153505A1F6AD85BE85A4 |
:1069D000A80DB91DED92FD920D931C931397089489 |
:1069E000211C311CE4E0F0E08E0E9F1E2F853889BB |
:1069F0002216330609F0AACF8B819C8101969C83D5 |
:106A00008B83E981FA813296FA83E98324E030E0CE |
:106A1000620E731EE985FA858E179F0709F07FCFF6 |
:106A20000BC00F2EF0E0EF2EF0E0FF2EF0E00F2F66 |
:106A3000F0E01F2FF02DCACF62960FB6F894DEBF9C |
:106A40000FBECDBFDF91CF911F910F91FF90EF90BF |
:106A5000DF90CF90BF90AF909F908F907F906F907E |
:106A60005F904F903F902F900895AF92BF92CF923A |
:106A7000DF92EF92FF920F931F93CF93DF936A0100 |
:106A80001416150664F58B01AC01EE24FF241216D2 |
:106A90001306DCF4FA01C281D381F801A280B3802D |
:106AA00060E070E08C819D81AE81BF81F5018483BF |
:106AB0009583A683B7836F5F7F4F249684E090E031 |
:106AC000A80EB91E6217730769F70894E11CF11C40 |
:106AD0000E5F1F4F4E5F5F4FEC14FD04C1F6DF9158 |
:106AE000CF911F910F91FF90EF90DF90CF90BF90CB |
:106AF000AF9008952F923F924F925F926F927F9244 |
:106B00008F929F92AF92BF92CF92DF92EF92FF92BD |
:106B10000F931F93CF93DF93CDB7DEB7EC970FB6EC |
:106B2000F894DEBF0FBECDBF9CAB8BAB7EAB6DAB25 |
:106B3000161617060CF011C218A21F8EFE013D9604 |
:106B40002F8D38A14DA95EA9119211922F5F3F4F51 |
:106B500038A32F8F24173507B9F71EA61DA61CA62C |
:106B60001BA61EA21DA2CE0103969CA38BA3DE0131 |
:106B70005796BAA3A9A32F5F3F4F38AF2FABFE01A3 |
:106B80003196FCAFEBAF2BA83CA821E030E03AA750 |
:106B900029A70F2EF0E04F2EF0E05F2EF0E06F2ED1 |
:106BA000F0E07F2EF02DAE01435F5F4F58A74FA35B |
:106BB000AFA1B8A58D919C91019709F43EC031E039 |
:106BC000C32ED12C2DE0A22EB12CAC0EBD1E94E014 |
:106BD000892E912CF50180819181892BF1F4F101AD |
:106BE000A281B381A80DB91D8D919D910D90BC918D |
:106BF000A02D7C018D01E89417F9A3019201C80131 |
:106C0000B7010E94834F882344F027013801DEA694 |
:106C1000CDA629A53AA53CA72BA70894C11CD11C39 |
:106C200042E050E0A40EB51E84E090E0880E991E6C |
:106C3000AFA9B8ADAC15BD0569F6E9A5FAA53196C1 |
:106C4000FAA7E9A72FA138A52E5F3F4F38A72FA39A |
:106C500042E050E0240E351E8FA998ADE817F907E1 |
:106C600009F0A6CFADA5BEA5AA0FBB1FBAAFA9AFAD |
:106C7000FD0121E030E02C0F3D1FE20FF31F0284E5 |
:106C8000F385E02DFAABE9AB4BA55CA58DA59EA5E0 |
:106C90004817590709F4F6C0FD01ABA9BCA9EA0FD2 |
:106CA000FB1F0190F081E02DF8ABEFA7FA01EE0F8A |
:106CB000FF1FEA0FFB1F60817181EFA4F8A800E0BD |
:106CC00010E0FB012481358146815781F7018481E1 |
:106CD0009581A681B781FB0184839583A683B783C1 |
:106CE000F70124833583468357830F5F1F4F6C5F03 |
:106CF0007F4F24E030E0E20EF31E4F8D58A10417C1 |
:106D00001507F9F6EBA5FCA5ABA1BCA1ED93FC938F |
:106D1000EDA5FEA5A9A1BAA1ED93FC931F01220C3C |
:106D2000331C220C331C0FA518A9020D131DD8010A |
:106D30002D913D914D915C9160E070E080E89FE382 |
:106D40000E94004F5B016C0180E090E0A0E8BFE38F |
:106D5000F80180839183A283B3830FA518A9EE2441 |
:106D6000FF24A6019501F8016481758186819781D0 |
:106D70000E947150F8016483758386839783089419 |
:106D8000E11CF11C0C5F1F4F2F8D38A1E216F3069A |
:106D900041F76BA87CA8B1E04B2E512C4DA55EA508 |
:106DA00044155505E1F1F30182819381FC01E20D67 |
:106DB000F31D80809180A280B38020E030E040E02D |
:106DC00050E020833183428353838C01EFA4F8A8E1 |
:106DD000CC24DD24F7012481358146815781C5010A |
:106DE000B4010E9471509B01AC01F801648175816E |
:106DF000868197810E94B74EF801648375838683EC |
:106E000097830894C11CD11C0C5F1F4F24E030E015 |
:106E1000E20EF31E4F8D58A1C416D506D9F608947C |
:106E2000411C511C82E090E0680E791EA9A5BAA50C |
:106E30004A165B0609F0B2CFEDA1FEA13196FEA382 |
:106E4000EDA32BA13CA12E5F3F4F3CA32BA349A157 |
:106E50005AA14E5F5F4F5AA349A38F8D98A1E8179F |
:106E6000F90751F1A9ADBAADE1E0F0E0EC0FFD1F7B |
:106E7000AE0FBF1F29A93AA92F5F3F4FFD013387EE |
:106E8000228781CE1C01ABA1BCA18D939C93A9A1AB |
:106E9000BAA18D939C93E9ADFAAD2BA93CA9E20F61 |
:106EA000F31F0190F081E02DF8ABEFA7220C331C0B |
:106EB000220C331C8F0139CFCDA8DEA8CC0CDD1CF1 |
:106EC00081E090E08C0F9D1FC80ED91ED6012D9138 |
:106ED0003C91F6018489958928173907A1F1490168 |
:106EE000880C991C880C991C5C01AA0CBB1CAA0C70 |
:106EF000BB1C0BA91CA9EE24FF24F80182819381FD |
:106F0000BC01680D791DDB012D913D914D915C9186 |
:106F10001397FC01EA0DFB1D408051806280738055 |
:106F20004D925D926D927C921397208331834283C0 |
:106F300053830894E11CF11C0E5F1F4FEF8DF8A1E5 |
:106F4000EE16FF06D1F62EEF3FEFC20ED31E4BAD6D |
:106F50005CADC416D50609F0B9CFEC960FB6F8941F |
:106F6000DEBF0FBECDBFDF91CF911F910F91FF907C |
:106F7000EF90DF90CF90BF90AF909F908F907F90D9 |
:106F80006F905F904F903F902F9008958F929F92B7 |
:106F9000AF92BF92CF92DF92EF92FF92CF93DF93A7 |
:106FA000CDB7DEB74A015B01A7019601880C991C99 |
:106FB000880E991E220F331F220F331FF401808188 |
:106FC0009181280F391FC90104970E94544EC401B2 |
:106FD00002970E94544EDF91CF91FF90EF90DF9087 |
:106FE000CF90BF90AF909F908F9008952F923F9237 |
:106FF0004F925F926F927F928F929F92AF92BF92C9 |
:10700000CF92DF92EF92FF920F931F93CF93DF9374 |
:10701000CDB7DEB728970FB6F894DEBF0FBECDBF51 |
:107020003B014C0129833A834B835C831501260184 |
:10703000DA01C90186199709A809B9090196A11DA4 |
:10704000B11D8D839E83AF83B8872E183F08400AF9 |
:10705000510A0894211C311C411C511C880F991F96 |
:10706000AA1FBB1F02960E94B14D02E0A02EB12CB8 |
:10707000A80EB91E6301CC0CDD1CAC18BD082D8117 |
:107080003E814F815885220F331F441F551F220F09 |
:10709000331F441F551FC201B1010E947651DC010C |
:1070A000CB0104960E94B14DCA0CDB1C0496EE0C79 |
:1070B000FF1CEE0CFF1C8E199F09D6018D939C932B |
:1070C000A40193012F5F3F4F4F4F5F4F89819A81FA |
:1070D000AB81BC8182179307A407B5071CF1B101EE |
:1070E000660F771F660F771F8901000F111F0A0DAA |
:1070F0001B1D02501040D8018D919C91860F971F47 |
:10710000F801938382832F5F3F4F4F4F5F4F0E5F96 |
:107110001F4F89819A81AB81BC8182179307A40795 |
:10712000B5074CF78D819E81AF81B8851816190679 |
:107130001A061B06B4F5B501EE24FF2487011214CC |
:10714000130414041504ECF4FB01C280D38020E086 |
:1071500030E040E050E080E090E0A0E0B0E0F601F8 |
:1071600084839583A683B78384E090E0C80ED91EFC |
:107170002F5F3F4F4F4F5F4F221633064406550691 |
:1071800051F70894E11CF11C011D111D6E5F7F4F2A |
:107190008D819E81AF81B885E816F9060A071B0725 |
:1071A00071F6C50128960FB6F894DEBF0FBECDBFAD |
:1071B000DF91CF911F910F91FF90EF90DF90CF90D3 |
:1071C000BF90AF909F908F907F906F905F904F9007 |
:1071D0003F902F900895CF92DF92EF92FF920F93FE |
:1071E0001F93CF93DF937C01EB018A01AB01B801C0 |
:1071F0000E947A35101611064CF5BE01A701EE2447 |
:10720000FF24FA01C281D381FB01C280D38020E038 |
:1072100030E08C819D81AE81BF81F60184839583AE |
:10722000A683B7832F5F3F4F249684E090E0C80E7B |
:10723000D91E2017310769F70894E11CF11C6E5F15 |
:107240007F4F4E5F5F4FE216F306D9F6DF91CF9185 |
:107250001F910F91FF90EF90DF90CF900895CF9303 |
:10726000DF9397FD25C0C1E0D0E068E671E00E94A1 |
:10727000B551FC018B359105C4F08B5590408A3592 |
:10728000910570F0CF01855B90408A359105B0F093 |
:1072900088E691E08E1B9F0BFC012FEF3FEF12C0A1 |
:1072A00084EB90E08E1B9F0BFC0121E030E00AC0D4 |
:1072B000909581959F4FCFEFDFEFD7CFE45BF04004 |
:1072C0002FEF3FEFEE0FFF1FE15FFC4F8591949191 |
:1072D000AC01429FC001439F900D529F900D11241D |
:1072E0009C01C29FC001C39F900DD29F900D11249D |
:1072F000DF91CF9108951F93CF93DF93EB01672B1D |
:1073000019F4009709F457C097FD5BC011E020976E |
:1073100009F441C09C01B901882777FD8095982F19 |
:10732000660F771F881F991F660F771F881F991F89 |
:10733000660F771F881F991F660F771F881F991F79 |
:10734000660F771F881F991F660F771F881F991F69 |
:107350009E01442737FD4095542F0E94EA51F901C0 |
:1073600037FD38C031E0EA35F307D4F4EB53FC4F76 |
:107370008491282F33271C161D062CF5812F992761 |
:1073800087FD9095AC01249FC001259F900D349FEF |
:10739000900D112461C08AE51802C00111245CC05F |
:1073A0004CE1E73AF40754F459E0ED38F507B4F04E |
:1073B00029E530E0E0CF80E090E04EC02AE530E003 |
:1073C000DACF1FEFA4CF1116BCF484EB90E0821B40 |
:1073D000930B42C0F095E195FF4FC4CF85E0EA3BA7 |
:1073E000F8073CF424E0E731F20754F027E530E0F9 |
:1073F000C2CF28E530E0BFCFC901845B90402CC0EC |
:1074000033E0EE32F3071CF026E530E0B4CF42E083 |
:10741000E939F4071CF025E530E0ADCF52E0E23366 |
:10742000F5071CF024E530E0A6CF81E0E73EF80741 |
:107430001CF023E530E09FCF21E0ED3AF2071CF08D |
:1074400022E530E098CFEF57F1401CF021E530E025 |
:1074500092CF20E530E08FCFDF91CF911F9108953B |
:1074600097FD11C0FC010E2EEF2F000CEE1FFF0B3D |
:10747000000CEE1FFF1F21E030E0E038F10594F032 |
:10748000EFE7F0E00FC0EE27FF27E81BF90B0E2E09 |
:10749000EF2F000CEE1FFF0B000CEE1FFF1F2FEF56 |
:1074A0003FEFEBCFE15EFA4F84919927AC01249F27 |
:1074B000C001259F900D349F900D112408959F9237 |
:1074C000AF92BF92CF92DF92EF92FF920F931F93F2 |
:1074D0007B018C015B016C01992406C0939488E2C6 |
:1074E0009816D1F079018A01C601B501A80197016A |
:1074F0000E94C8512E0D3F1D401F511F56954795A4 |
:1075000037952795E21AF30A040B150B82E0E8166B |
:10751000F1040105110510F7CA01B9011F910F917E |
:10752000FF90EF90DF90CF90BF90AF909F90089525 |
:10753000CF93DF932AE530E0281B390B37FD26C0B7 |
:10754000C1E0D0E0C90168E671E00E94B551FC01DC |
:107550008B359105C4F08B5590408A35910570F0BC |
:10756000CF01855B90408A359105B0F088E691E0C7 |
:107570008E1B9F0BFC012FEF3FEF12C084EB90E0BE |
:107580008E1B9F0BFC0121E030E00AC03095219555 |
:107590003F4FCFEFDFEFD6CFE45BF0402FEF3FEF71 |
:1075A000EE0FFF1FE15FFC4F85919491AC01429F6C |
:1075B000C001439F900D529F900D11249C012C9F60 |
:1075C000C0012D9F900D3C9F900D1124DF91CF9114 |
:1075D0000895A4E6B0E0A80FB11D0E940552802DC9 |
:1075E00099270895A4E6B0E0A80FB11D062E0E94C9 |
:1075F00017520895A4E6B0E0A80FB11D0E941252E0 |
:10760000CF010895A4E6B0E0A80FB11D0B010E94C0 |
:10761000235208951F920F920FB60F9211242F93A9 |
:107620003F934F938F939F938091CE06833021F0A9 |
:10763000843009F077C008C08EB5809310031EBC5B |
:1076400084E08093CE066EC080911003992787FD59 |
:107650009095382F22278EB59927282B392BC901D1 |
:107660008D509E4F875E934068F48091D1068230A2 |
:1076700009F448C0833009F44DC08130D9F181E06C |
:107680008093D106449A1092CE062091D206309172 |
:10769000D3068091D4069091D5062817390751F06A |
:1076A00080910E038431F0F580910E038F5F8093FB |
:1076B0000E0338C02091D2063091D3068091D606B1 |
:1076C0009091D7062817390759F72091D4063091A1 |
:1076D000D5068091D6069091D7062817390701F76D |
:1076E00080910E038823F1F080910E0381508093E6 |
:1076F0000E0318C03093D3062093D20682E0809305 |
:10770000D106C0CF3093D5062093D40683E0809372 |
:10771000D106B8CF3093D7062093D60681E0809368 |
:10772000D106B0CF9F918F914F913F912F910F90A4 |
:107730000FBE0F901F9018952F923F924F925F921D |
:107740006F927F928F929F92AF92BF92CF92DF9271 |
:10775000EF92FF920F931F93CF93DF93CDB7DEB7D6 |
:1077600066970FB6F894DEBF0FBECDBF80910E03B3 |
:10777000882319F42FEF3FEF37C16091D206709143 |
:10778000D3062091C2063091C306621B730B882773 |
:1077900077FD8095982F5AE0660F771F881F991FF5 |
:1077A0005A95D1F72091C8063091C906442737FD74 |
:1077B0004095542F0E94EA5129873A874B875C876E |
:1077C0006091D4067091D5062091C4063091C5060B |
:1077D000621B730B882777FD8095982F4AE0660F10 |
:1077E000771F881F991F4A95D1F72091CA063091BB |
:1077F000CB06442737FD4095542F0E94EA512D8730 |
:107800003E874F87588B6091D6067091D70620919E |
:10781000C6063091C706621B730B882777FD8095DB |
:10782000982F3AE0660F771F881F991F3A95D1F776 |
:107830002091CC063091CD06442737FD4095542F3A |
:107840000E94EA51298B3A8B4B8B5C8B84EB90E046 |
:107850000E942F399C01442737FD4095542F2D83DA |
:107860003E834F83588784EB90E00E94983A9C01B6 |
:10787000442737FD4095542F29833A834B835C83FB |
:1078800080E090E00E942F393C0180E090E00E946F |
:10789000983A5C0180E090E00E942F399E8B8D8B9E |
:1078A00080E090E00E94983A1C0169857A858B857A |
:1078B0009C8529813A814B815C810E9476517B01B4 |
:1078C0008C016D857E858F8598892D813E814F81C4 |
:1078D00058850E947651E61AF70A080B190BC80161 |
:1078E000B70120E030E240E050E00E94EA51CC24B1 |
:1078F000B7FCC094DC2CCA01B901A60195010E9415 |
:1079000076517B018C01882477FC8094982C6989BE |
:107910007A898B899C89A40193010E947651E61A89 |
:10792000F70A080B190BC801B70120E030E240E06C |
:1079300050E00E94EA5159016A0169857A858B8578 |
:107940009C852D813E814F8158850E9476517B0117 |
:107950008C016D857E858F85988929813A814B813F |
:107960005C810E947651E60EF71E081F191FC801A0 |
:10797000B70120E030E240E050E00E94EA514424A8 |
:1079800037FC4094542CCA01B901A20191010E9414 |
:1079900076517B018C018D899E899C01442737FD9E |
:1079A0004095542F69897A898B899C890E947651E8 |
:1079B000E60EF71E081F191FC801B70120E030E2CC |
:1079C00040E050E00E94EA51DA01C901B5010E948D |
:1079D0007B3997FD05C028E631E0281B390B04C030 |
:1079E00022273327281B390BC90166960FB6F89456 |
:1079F000DEBF0FBECDBFDF91CF911F910F91FF90E2 |
:107A0000EF90DF90CF90BF90AF909F908F907F903E |
:107A10006F905F904F903F902F9008958091CE0689 |
:107A2000813059F0813030F44498459A81E0809358 |
:107A3000CE060895823079F0089545989091D10648 |
:107A4000923021F19330A1F0913019F18093D10659 |
:107A50001092CE0608958091CF069091D0060E9494 |
:107A6000F813882349F31EBC83E08093CE06089563 |
:107A700083E38EBD88E090E00E94EF139093D006E0 |
:107A80008093CF0682E08093CE06089582E38EBD78 |
:107A9000F1CF81E38EBDEECF4F925F926F927F92D6 |
:107AA0008F929F92AF92BF92CF92DF92EF92FF920E |
:107AB0000F931F93CF93DF93299A289A50E8852E2E |
:107AC0005EE3952E00E811EC40E8642E4EE3742E40 |
:107AD00030E8E32E31ECF32E20E8422E2EE3522E36 |
:107AE00090E8C92E91ECD92E82E3A82EBB24809178 |
:107AF000D2069091D306081719070CF09AC000918E |
:107B0000D2061091D3068091D4069091D506E8163E |
:107B1000F9060CF081C0E090D406F090D506809173 |
:107B2000D6069091D706C816D9060CF068C0C0904A |
:107B3000D606D090D706BB2009F456C0BA948AE086 |
:107B400090E00E94EF13EC01CE010E94F81388230D |
:107B5000D9F380910F038F5F80930F0381508F3192 |
:107B600008F074C0E0915A05FF27EE0FFF1FE255A1 |
:107B7000FD4F80819181843691050CF4AA94AA204E |
:107B800009F0B5CFC801881999099093C906809367 |
:107B9000C806C701861997099093CB068093CA0639 |
:107BA000C601841995099093CD068093CC06C40133 |
:107BB000800F911F97FD68C09C01359527953093E4 |
:107BC000C3062093C206C3018E0D9F1D97FD60C0A2 |
:107BD000959587959093C5068093C406C2018C0D38 |
:107BE0009D1D97FF57C052C085B191E0892785B987 |
:107BF00085B192E0892785B9B1E3BB2EA0CF8091F2 |
:107C0000D6069091D706841595050CF094CF409038 |
:107C1000D6065090D7068FCF8091D4069091D50686 |
:107C2000861597050CF07BCF6090D4067090D50632 |
:107C300076CF8091D2069091D306881599050CF0E5 |
:107C400062CF8090D2069090D3065DCFDF92CF9224 |
:107C50005F924F92FF92EF927F926F921F930F93DA |
:107C60009F928F928FE995E09F938F931F920E942E |
:107C7000DD0F10920F038DB79EB70F960FB6F894D5 |
:107C80009EBF0FBE8DBF6ECF019696CF019602C0EC |
:107C900001969ECF959587959093C7068093C606CB |
:107CA0004EE650E0DA0109010E9423528091C40699 |
:107CB0009091C5064E5F5F4FDA010C010E9423527E |
:107CC0008091C6069091C7064E5F5F4FDA010C01A6 |
:107CD0000E9423528091C8069091C9064E5F5F4F63 |
:107CE000DA010C010E9423528091CA069091CB06C2 |
:107CF0004E5F5F4FDA010C010E9423528091CC0647 |
:107D00009091CD064E5F5F4FDA010C010E94235225 |
:107D1000DF91CF911F910F91FF90EF90DF90CF9067 |
:107D2000BF90AF909F908F907F906F905F904F909B |
:107D300008959FB7F89484B1806A84B9269887B172 |
:107D4000806387B988B18F7C88B983ED8CBD8DB590 |
:107D50008E7F8DBD81E08093D1061092CE064EE6D7 |
:107D600050E0DA010E941252F093C306E093C2067B |
:107D70004E5F5F4FDA010E941252F093C506E09306 |
:107D8000C4064E5F5F4FDA010E941252F093C7069D |
:107D9000E093C6064E5F5F4FDA010E941252F093E5 |
:107DA000C906E093C8064E5F5F4FDA010E94125287 |
:107DB000F093CB06E093CA064E5F5F4FDA010E9454 |
:107DC0001252F093CD06E093CC0610920E039FBFA3 |
:107DD0000895CF93DF932091220730912307E90183 |
:107DE000EA81FB8180E090E0A0E8BFE38483958393 |
:107DF000A683B783EC81FD8180879187A287B387B3 |
:107E0000EE81FF8184879587A687B787DF91CF9121 |
:107E100008956F927F928F929F92AF92BF92CF926E |
:107E2000DF92EF92FF920F931F93CF93DF9381E046 |
:107E30008093DA068093DB068093DC06609109026A |
:107E400070910A02709561957F4F882777FD809524 |
:107E5000982F0E94634F22E133E840E45DE30E94E3 |
:107E600071503B014C0160910D0270910E02882708 |
:107E700077FD8095982F0E94634F22E133E840E41C |
:107E80005DE30E9471506B017C010091000710912D |
:107E90000107E801AA80BB8060910B0270910C027F |
:107EA000882777FD8095982F0E94634F22E133E861 |
:107EB00040E45DE30E947150F5016483758386831D |
:107EC0009783EC81FD816482758286829782EE8140 |
:107ED000FF81C482D582E682F7821092DD06DF91AF |
:107EE000CF911F910F91FF90EF90DF90CF90BF90B7 |
:107EF000AF909F908F907F906F900895EF92FF9238 |
:107F00000F931F93CF93DF93E0902103F090220310 |
:107F100000912303109124032BED3FE049EC50E442 |
:107F2000C801B7010E94834F18161CF52BED3FE0E6 |
:107F300049EC50E4C801B7010E94B74E6093210399 |
:107F4000709322038093230390932403E0910C0702 |
:107F5000F0910D070280F381E02D648375838683A1 |
:107F60009783E0902103F090220300912303109166 |
:107F700024032BED3FE049EC50ECC801B7010E940F |
:107F8000804F8823DCF42BED3FE049EC50E4C8013E |
:107F9000B7010E94B84E609321037093220380932F |
:107FA000230390932403E0910C07F0910D070280C6 |
:107FB000F381E02D6483758386839783E0902503A6 |
:107FC000F090260300912703109128032BED3FE04A |
:107FD00049EC50E4C801B7010E94834F18161CF504 |
:107FE0002BED3FE049EC50E4C801B7010E94B74EC9 |
:107FF00060932503709326038093270390932803AF |
:10800000E0910C07F0910D070480F581E02D648369 |
:10801000758386839783E0902503F0902603009173 |
:108020002703109128032BED3FE049EC50ECC801E9 |
:10803000B7010E94804F8823DCF42BED3FE049EC30 |
:1080400050E4C801B7010E94B84E609325037093B5 |
:1080500026038093270390932803E0910C07F09167 |
:108060000D070480F581E02D6483758386839783F3 |
:10807000E0902903F0902A0300912B0310912C0328 |
:108080002BED3FE049EC50E4C801B7010E94834F5B |
:1080900018160CF0B0C02BED3FE049EC50E4C801DD |
:1080A000B7010E94B74E6093290370932A0380930F |
:1080B0002B0390932C03C0919D02D0919E02C0513E |
:1080C000DE40D0939E02C0939D02E0910C07F09198 |
:1080D0000D07068117812BED3FE049EC50E4F801D4 |
:1080E00064817581868197810E94B74EF80164830F |
:1080F000758386839783E0911A07F0911B070681A9 |
:1081000017812BED3FE049EC50E4F8016481758163 |
:10811000868197810E94B74EF801648375838683B8 |
:108120009783E0902903F0902A0300912B0310918C |
:108130002C0320E030E040E050E0C801B7010E948D |
:10814000804F88230CF45CC020E030E44FE054E41E |
:108150006091210370912203809123039091240365 |
:108160000E9471500E94464F70932E0360932D031E |
:1081700020E030E44FE054E4609125037091260341 |
:1081800080912703909128030E9471500E94464FCE |
:108190007093300360932F0320E030E44FE054E409 |
:1081A0006091290370912A0380912B0390912C03F5 |
:1081B0000E9471500E94464F9B017093320360935E |
:1081C0003103CE01821B930B81519E4034F0C0518C |
:1081D000DE40D0939E02C0939D02CE01821B930B82 |
:1081E000805F914F0CF04FC0C05FD14FD0939E0283 |
:1081F000C0939D0248C0C0919D02D0919E0299CF2C |
:108200002BED3FE049EC50E4C801B7010E94B84EA5 |
:108210006093290370932A0380932B0390932C037C |
:10822000C05FD14FD0939E02C0939D02E0910C0796 |
:10823000F0910D07068117812BED3FE049EC50E4EA |
:10824000F80164817581868197810E94B84EF8019A |
:108250006483758386839783E0911A07F0911B07E7 |
:10826000068117812BED3FE049EC50E4F801648171 |
:108270007581868197810E94B84EF8016483758369 |
:108280008683978361CFDF91CF911F910F91FF90EC |
:10829000EF90089508952F923F924F925F926F92C0 |
:1082A0007F928F929F92AF92BF92CF92DF92EF9286 |
:1082B000FF920F931F93CF93DF93CDB7DEB728972D |
:1082C0000FB6F894DEBF0FBECDBFA0910C07B091E2 |
:1082D0000D07FD0184819581FC01A480B580C680D5 |
:1082E000D780FD0186819781FC01248135814681FB |
:1082F000578129833A834B835C83FD01A281B3813B |
:10830000FD01E480F58006811781C801B7010E9454 |
:10831000B9501B012C016093FA067093FB06809301 |
:10832000FC069093FD06C801B7010E94FA4E6D83CA |
:108330007E838F8398876093F4067093F50680930D |
:10834000F6069093F706C601B5010E94B9507B016D |
:108350008C016093E4067093E5068093E6069093A3 |
:10836000E706C601B5010E94FA4E5B016C016093FD |
:10837000EC067093ED068093EE069093EF068090E6 |
:10838000280790902907F4016280738017FB1095ED |
:1083900017F9109523EC35EF4CE151E4C801B70112 |
:1083A0000E947150F3016483758386839783F4017F |
:1083B00004811581A6019501C201B1010E9471508D |
:1083C00023EC35EF4CE151E40E947150F8016483D5 |
:1083D000758386839783F40106811781A601950131 |
:1083E0006D817E818F8198850E94715023EC35EFDD |
:1083F0004CE151E40E947150F801648375838683D7 |
:108400009783F4012085318589819A81AB81BC8174 |
:10841000F90184839583A683B78328960FB6F894D1 |
:10842000DEBF0FBECDBFDF91CF911F910F91FF90A7 |
:10843000EF90DF90CF90BF90AF909F908F907F9004 |
:108440006F905F904F903F902F900895AF92BF92A2 |
:10845000CF92DF92EF92FF920F931F93CF93DF9310 |
:10846000A0902503B0902603C0902703D090280346 |
:10847000E0902103F0902203009123031091240344 |
:10848000C801B7010E94B95060930E0770930F079F |
:108490008093100790931107C801B7010E94FA4E0C |
:1084A00060932407709325078093260790932707EE |
:1084B000C601B5010E94B95060933A0770933B071B |
:1084C00080933C0790933D07C601B5010E94FA4E88 |
:1084D0006093E8067093E9068093EA069093EB06B2 |
:1084E000E090F006F090F106E7010A811B819058B8 |
:1084F00023EC35EF4CE151E40E947150F8016087A4 |
:108500007187828793870C811D8120912407309188 |
:10851000250740912607509127076091E806709142 |
:10852000E9068091EA069091EB060E94715023ECD7 |
:1085300035EF4CE151E40E947150E8016C837D837A |
:108540008E839F8360913A0770913B0780913C072F |
:1085500090913D07905820910E0730910F07409160 |
:108560001007509111070E94715023EC35EF4CE138 |
:1085700051E40E947150F801608771878287938768 |
:10858000E7010E811F816091E8067091E9068091F4 |
:10859000EA069091EB06905820910E0730910F0754 |
:1085A00040911007509111070E94715023EC35EF54 |
:1085B0004CE151E40E947150F80164837583868315 |
:1085C000978360913A0770913B0780913C079091A7 |
:1085D0003D079058209124073091250740912607A8 |
:1085E000509127070E94715023EC35EF4CE151E484 |
:1085F0000E947150E801688779878A879B87E70125 |
:10860000E885F98580E090E0A0E8BFE3848795875E |
:10861000A687B787DF91CF911F910F91FF90EF90C1 |
:10862000DF90CF90BF90AF9008952F923F924F92DE |
:108630005F926F927F928F929F92AF92BF92CF92F2 |
:10864000DF92EF92FF920F931F93CF93DF93CDB7FB |
:10865000DEB728970FB6F894DEBF0FBECDBFA0914E |
:108660001A07B0911B07FD0182819381FC01E48010 |
:10867000F58006811781E0923407F0923507009368 |
:10868000360710933707FD01A481B581FD01A48051 |
:10869000B580C680D780A0921607B0921707C09207 |
:1086A0001807D0921907C801B7010E94B95060930A |
:1086B0000E0770930F078093100790931107C8015E |
:1086C000B7010E94FA4E60932407709325078093A8 |
:1086D000260790932707C601B5010E940E516093AB |
:1086E0002A0770932B0780932C0790932D07C601C0 |
:1086F000B5010E94FA4E9B01AC0160E070E080E899 |
:108700009FE30E94004F60930407709305078093D6 |
:10871000060790930707E0901207F0901307F70100 |
:108720000281138180E090E0A0E8BFE3F801848338 |
:108730009583A683B78320912A0730912B07409118 |
:108740002C0750912D0760910E0770910F078091B3 |
:108750001007909111070E947150F801608771878E |
:108760008287938720912A0730912B0740912C070D |
:1087700050912D076091240770912507809126075D |
:10878000909127070E947150F8016487758786874A |
:108790009787F701248135818091240790912507DF |
:1087A000A0912607B0912707F90180879187A287BA |
:1087B000B38780910E0790910F07A0911007B09199 |
:1087C0001107B05884879587A687B787F701068178 |
:1087D00017812091040730910507409106075091B9 |
:1087E000070760910E0770910F0780911007909115 |
:1087F00011070E947150F801608771878287938703 |
:108800002091040730910507409106075091070712 |
:108810006091240770912507809126079091270782 |
:108820000E947150F80164877587868797876091E9 |
:10883000610270916202882777FD8095982F0E94CF |
:10884000634F2CEA35EC47E257EB0E94715020E071 |
:1088500030E048E451E40E94715069837A838B834D |
:108860009C836091630270916402882777FD8095F4 |
:10887000982F0E94634F2CEA35EC47E257EB0E9499 |
:10888000715020E030E048E451E40E9471506D8363 |
:108890007E838F8398876091650270916602882736 |
:1088A00077FD8095982F0E94634F2CEA35EC47E2C4 |
:1088B00057EB0E94715020E030E048E451E40E9400 |
:1088C00071501B012C0120E030E044E353E40E948E |
:1088D000715023EC35EF48E450E40E94004F2091A2 |
:1088E0001B0330911C0340911D0350911E030E94F5 |
:1088F000B84E3B014C0160931B0370931C038093A3 |
:108900001D0390931E0320E030E044E353E46D81A7 |
:108910007E818F8198850E94715023EC35EF48E469 |
:1089200050E40E94004F20911703309118034091AA |
:10893000190350911A030E94B84E5B016C016093B9 |
:108940001703709318038093190390931A0320E080 |
:1089500030E044E353E469817A818B819C810E94F9 |
:10896000715023EC35EF48E450E40E94004F209111 |
:1089700013033091140340911503509116030E9484 |
:10898000B84E7B018C0160931303709314038093A2 |
:10899000150390931603C601B5010E94464F7093CC |
:1089A0003A0560933905C801B7010E94464F70939C |
:1089B0003C0560933B05C401B3010E94464F709390 |
:1089C0003E0560933D05A0912E07B0912F07FD0154 |
:1089D0008281938129813A814B815C81FC012483CE |
:1089E000358346835783FD01848195812D813E81A6 |
:1089F0004F815885FC012483358346835783FD01CD |
:108A0000A681B781FD01248235824682578228964D |
:108A10000FB6F894DEBF0FBECDBFDF91CF911F918F |
:108A20000F91FF90EF90DF90CF90BF90AF909F900D |
:108A30008F907F906F905F904F903F902F90089510 |
:108A4000AF92BF92CF92DF92EF92FF920F931F935C |
:108A5000CF93DF93409132075091330760912E07F7 |
:108A600070912F0781E0E82EF12C03E010E023E065 |
:108A700030E080911207909113070E94C23200915A |
:108A80000C0710910D07E0901A07F0901B07C0909B |
:108A90003207D0903307E801AA80BB80F701A2819A |
:108AA000B381E601EA81FB812481358146815781CA |
:108AB000FD0164817581868197810E94B84EE50130 |
:108AC0006C837D838E839F83F801A480B580E7014A |
:108AD000AC81BD81E601EC81FD8124813581468137 |
:108AE0005781FD0164817581868197810E94B84E0E |
:108AF000E5016C837D838E839F83F8010681178156 |
:108B0000E701AE81BF81E601EE81FF8124813581DD |
:108B100046815781FD0164817581868197810E941C |
:108B2000B84EE8016C837D838E839F8300913E075E |
:108B300010913F07E0903807F0903907C090F80691 |
:108B4000D090F906F801A280B380E701AA81BB8129 |
:108B5000E601EA81FB812481358146815781FD014F |
:108B600064817581868197810E94B84EE5016C838E |
:108B70007D838E839F83F801A480B580E701AC815B |
:108B8000BD81E601EC81FD812085318542855385DB |
:108B9000FD0160857185828593850E94B84EE5014F |
:108BA000688779878A879B87F80106811781E701A3 |
:108BB000AE81BF81E601EE81FF8124853585468542 |
:108BC0005785FD0164857585868597850E94B84E19 |
:108BD000E8016C877D878E879F87DF91CF911F91FA |
:108BE0000F91FF90EF90DF90CF90BF90AF900895DE |
:108BF000AF92BF92CF92DF92EF92FF920F931F93AB |
:108C0000CF93DF9340912007509121076091F006A8 |
:108C10007091F106E4E0EE2EF12C03E010E023E089 |
:108C200030E080913E0790913F070E94953340913C |
:108C3000FE065091FF06609120077091210724E005 |
:108C400030E08091F0069091F1060E94C232C0900F |
:108C5000FE06D090FF06E6010A811B81E0901E0708 |
:108C6000F0901F07E701EA81FB812481358146816D |
:108C70005781F80164817581868197810E94B84E81 |
:108C8000E8016C837D838E839F83F60104811581C7 |
:108C9000E701EC81FD812085318542855385F8010E |
:108CA00060857185828593850E94B84EE80168874A |
:108CB00079878A879B87F60106811781E701EE8114 |
:108CC000FF812485358546855785F801648575853E |
:108CD000868597850E94B84EE8016C877D878E87D0 |
:108CE0009F87F60100851185E701E885F9852089D0 |
:108CF000318942895389F801608971898289938910 |
:108D00000E94B84EE801688B798B8A8B9B8B6091AF |
:108D100030077091310744E050E0C6010E94EB3803 |
:108D200040911C0750911D076091300770913107E9 |
:108D300034E0E32EF12C04E010E023E030E08091F9 |
:108D40002007909121070E94C23240911407509150 |
:108D5000150760913E0770913F0723E0E22EF12C4A |
:108D600003E010E024E030E08091F0069091F106FD |
:108D70000E94C2324091D8065091D90660911407E2 |
:108D80007091150704E010E023E030E080911C07AB |
:108D900090911D070E94C2324091380750913907C7 |
:108DA0006091D8067091D90603E010E023E030E02E |
:108DB00080913E0790913F070E94BC31C090E2062F |
:108DC000D090E306E0900007F0900107C0912807DB |
:108DD000D0912907F60102811381F701A281B381A5 |
:108DE000EA81FB812481358146815781FD016481BF |
:108DF0007581868197810E94B74EF80164837583DF |
:108E000086839783F60104811581F701A481B581DA |
:108E1000EC81FD812481358146815781FD0164818A |
:108E20007581868197810E94B74EF80164837583AE |
:108E300086839783F60106811781F701A681B781A2 |
:108E4000EE81FF812481358146815781FD01648156 |
:108E50007581868197810E94B74EF801648375837E |
:108E6000868397838091DD06813061F0E601E88595 |
:108E7000F98580E090E0A0E0B0E084839583A6834C |
:108E8000B78318C0F60100851185F701A085B1856B |
:108E9000E885F9852481358146815781ED016C8112 |
:108EA0007D818E819F810E94B74EF8016483758316 |
:108EB000868397834091F2065091F30691E0E92E64 |
:108EC000F12C04E010E023E030E0B60180911C07B3 |
:108ED00090911D070E94C23200911A0710911B0742 |
:108EE000E0900C07F0900D07C090F206D090F306CA |
:108EF000F801A280B380E701AA81BB81E601EA8183 |
:108F0000FB812481358146815781FD016481758112 |
:108F1000868197810E94B84EE5016C837D838E83A4 |
:108F20009F83F801A480B580E701AC81BD81E60193 |
:108F3000EC81FD812481358146815781FD01648169 |
:108F40007581868197810E94B84EE5016C837D838F |
:108F50008E839F83F801A680B780E701AE81BF8131 |
:108F6000E601EE81FF812481358146815781FD0133 |
:108F700064817581868197810E94B84EE5016C837A |
:108F80007D838E839F83F80100851185E701A88585 |
:108F9000B985E601E885F9852481358146815781C7 |
:108FA000FD0164817581868197810E94B84EE80138 |
:108FB0006C837D838E839F83DF91CF911F910F916F |
:108FC000FF90EF90DF90CF90BF90AF9008952F92D9 |
:108FD0003F924F925F926F927F928F929F92AF9249 |
:108FE000BF92CF92DF92EF92FF920F931F93CF9396 |
:108FF000DF93CDB7DEB72C970FB6F894DEBF0FBE68 |
:10900000CDBFF3E0AF2EB12CC12CD12CE1E0EE2E80 |
:10901000F12C012D112D23E030E040E050E061E023 |
:1090200070E080E090E00E94F63790933F078093D5 |
:109030003E0723E030E040E050E061E070E080E097 |
:1090400090E00E94F6379093F9068093F80674E05A |
:10905000A72EB12CC12CD12C24E030E040E050E010 |
:1090600061E070E080E090E00E94F63790931F0787 |
:1090700080931E0761E0A62EB12CC12CD12C23E0D9 |
:1090800030E040E050E061E070E080E090E00E947D |
:10909000F63790931B0780931A0723E030E040E0F7 |
:1090A00050E061E070E080E090E00E94F63790933D |
:1090B0000D0780930C0723E030E040E050E061E0D2 |
:1090C00070E080E090E00E94F6379093F306809382 |
:1090D000F20623E030E040E050E061E070E080E044 |
:1090E00090E00E94F637909333078093320753E065 |
:1090F000A52EB12CC12CD12C23E030E040E050E073 |
:1091000061E070E080E090E00E94F63790932307E2 |
:109110008093220723E030E040E050E061E070E01F |
:1091200080E090E00E94F637909313078093120737 |
:1091300041E0A42EB12CC12CD12C23E030E040E042 |
:1091400050E061E070E080E090E00E94F63790939C |
:109150002F0780932E0733E0A32EB12CC12CD12CE6 |
:1091600024E030E040E050E061E070E080E090E03A |
:109170000E94F6379093F1068093F00621E0A22E2C |
:10918000B12CC12CD12C24E030E040E050E061E073 |
:1091900070E080E090E00E94F637909301078093A2 |
:1091A000000724E030E040E050E061E070E080E063 |
:1091B00090E00E94F6379093E3068093E20624E065 |
:1091C00030E040E050E061E070E080E090E00E943C |
:1091D000F637909329078093280793E0A92EB12CA6 |
:1091E000C12CD12C23E030E040E050E061E070E0A1 |
:1091F00080E090E00E94F63790933907809338071B |
:1092000084E0A82EB12CC12CD12C23E030E040E02A |
:1092100050E061E070E080E090E00E94F6379093CB |
:109220001D0780931C07B3E0AB2EB12CC12CD12CB1 |
:1092300023E030E040E050E061E070E080E090E06A |
:109240000E94F6379093D9068093D80624E030E048 |
:1092500040E050E061E070E080E090E00E94F6378E |
:109260009093150780931407A4E0AA2EB12CC12C6B |
:10927000D12C23E030E040E050E061E070E080E09D |
:1092800090E00E94F637909321078093200724E016 |
:1092900030E040E050E061E070E080E090E00E946B |
:1092A000F6379093FF068093FE0624E030E040E01E |
:1092B00050E061E070E080E090E00E94F63790932B |
:1092C000310780933007F3E0AF2EB12CC12CD12CA5 |
:1092D00023E030E040E050E061E070E080E090E0CA |
:1092E0000E94F6379093030780930207409122076C |
:1092F0005091230720E030E00F2EF0E06F2EF0E0D9 |
:109300007F2EF0E88F2EFFE39F2EF02D0F2EF0E042 |
:10931000EF2EF0E0FF2EF0E00F2FF0E01F2FF02DEA |
:1093200060911E0770911F070F2EF0E02F2EF0E0C6 |
:109330003F2EF0E24F2EF1E45F2EF02D8091F806E3 |
:109340009091F90698878F830F2EFCEAAF2EF5ECEB |
:10935000BF2EF7E2CF2EF7E3DF2EF02DE0913E0790 |
:10936000F0913F07FE83ED8380911A0790911B07D0 |
:109370009C838B83E0910C07F0910D07FA83E983BE |
:10938000FA010280F381E02DFC87EB87211531057E |
:1093900001F13496FA87E98780E090E0A0E0B0E040 |
:1093A000EB85FC8584839583A683B78321303105C3 |
:1093B00031F1E985FA8584839583A683B7832230CA |
:1093C000310579F4EB85FC856486758686869786FB |
:1093D00027C06482758286829782E086F186028742 |
:1093E0001387EB85FC85E486F586068717872F5F54 |
:1093F0003F4F4E5F5F4F2330310511F611C0E985B5 |
:10940000FA856482758286829782EB85FC858487E3 |
:109410009587A687B78722E030E04E5F5F4FB0CFD9 |
:10942000FB01A281B381FD012482358246825782ED |
:10943000FB01A481B581FD012086318642865386D9 |
:10944000FB01A681B781FD012486358646865786B5 |
:10945000FB012085318580E090E0A0EAB0E4F901CD |
:10946000808B918BA28BB38BEF81F885A281B38126 |
:10947000FD01A482B582C682D782EF81F885A481DE |
:10948000B581FD01A086B186C286D386EF81F885BD |
:109490002681378187E197EBA1E5B8E3F90184875D |
:1094A0009587A687B787ED81FE81A281B381FD01F3 |
:1094B000A482B582C682D782ED81FE81A481B58166 |
:1094C000FD01A086B186C286D386ED81FE81A6818C |
:1094D000B781FD01A486B586C686D786EB81FC815F |
:1094E000A281B381FD01E482F58206831783EB81BB |
:1094F000FC81A481B581FD01E482F5820683178396 |
:10950000EB81FC81A681B781FD01E482F5820683AF |
:109510001783E981FA81A281B381FD01E482F5829A |
:1095200006831783E981FA81A481B581FD01E48274 |
:10953000F58206831783E981FA81A681B781FD014F |
:10954000E482F582068317832C960FB6F894DEBF6B |
:109550000FBECDBFDF91CF911F910F91FF90EF9084 |
:10956000DF90CF90BF90AF909F908F907F906F9043 |
:109570005F904F903F902F9008954F925F926F921F |
:109580007F928F929F92AF92BF92CF92DF92EF9293 |
:10959000FF920F931F93CF93DF930E944B410E9442 |
:1095A000264281E08093DA068093DB068093DC0616 |
:1095B0006091090270910A02709561957F4F88272A |
:1095C00077FD8095982F0E94634F22E133E840E4B5 |
:1095D0005DE30E9471503B014C0160910D0270915E |
:1095E0000E02882777FD8095982F0E94634F22E115 |
:1095F00033E840E45DE30E9471506B017C0100910F |
:10960000000710910107E801AA80BB8060910B025E |
:1096100070910C02882777FD8095982F0E94634FE8 |
:1096200022E133E840E45DE30E947150F501648378 |
:10963000758386839783EC81FD816482758286823F |
:109640009782EE81FF81C482D582E682F7821092F2 |
:10965000DD060E94F8450E94154340913207509163 |
:10966000330760912E0770912F07A1E0EA2EF12CAD |
:1096700003E010E023E030E080911207909113079F |
:109680000E94C23200910C0710910D07E0901A075A |
:10969000F0901B07C0903207D0903307F801C281C9 |
:1096A000D381F701A281B381F60182819381FC010C |
:1096B0002481358146815781FD01648175818681D0 |
:1096C00097810E94B84E6C837D838E839F83F801BF |
:1096D00084809580F701A481B581F601848195810C |
:1096E000FC012481358146815781FD0164817581AA |
:1096F000868197810E94B84EF401648375838683C6 |
:109700009783F80166807780F70146805780F601DD |
:10971000A681B781FD012481358146815781F201FF |
:1097200064817581868197810E94B84EF3016483BC |
:1097300075838683978300913E0710913F07E090E1 |
:109740003807F0903907C090F806D090F906F80174 |
:10975000A280B380F701A281B381F6018281938157 |
:10976000FC012481358146815781FD016481758129 |
:10977000868197810E94B84EF50164837583868344 |
:109780009783F801A480B580F701A481B581F60123 |
:1097900084819581FC012085318542855385FD01B9 |
:1097A00060857185828593850E94B84EF50160873A |
:1097B000718782879387F80106811781F701A68157 |
:1097C000B781F60186819781FC0124853585468520 |
:1097D0005785FD0164857585868597850E94B84EFD |
:1097E000F8016487758786879787AC80BD80CE80B7 |
:1097F000DF80A0922103B0922203C0922303D09273 |
:109800002403F401E480F58006811781E0922503AA |
:10981000F09226030093270310932803F301E480BA |
:10982000F58006811781E0922903F0922A030093C4 |
:109830002B0310932C032BED3FE049EC50E4C601C1 |
:10984000B5010E94834F1816A4F42BED3FE049ECBC |
:1098500050E4C601B5010E94B74E60932103709396 |
:10986000220380932303909324036C837D838E8350 |
:109870009F83E0902103F090220300912303109135 |
:1098800024032BED3FE049EC50ECC801B7010E94E6 |
:10989000804F8823A4F42BED3FE049EC50E4C8014D |
:1098A000B7010E94B84E6093210370932203809306 |
:1098B0002303909324036C837D838E839F83E090A6 |
:1098C0002503F090260300912703109128032BED28 |
:1098D0003FE049EC50E4C801B7010E94834F1816DD |
:1098E000ECF42BED3FE049EC50E4C801B7010E94D5 |
:1098F000B74E6093250370932603809327039093BC |
:109900002803E4016C837D838E839F83E09025038D |
:10991000F090260300912703109128032BED3FE0E0 |
:1099200049EC50ECC801B7010E94804F8823ACF489 |
:109930002BED3FE049EC50E4C801B7010E94B84E5E |
:109940006093250370932603809327039093280345 |
:10995000F4016483758386839783E0902903F090F4 |
:109960002A0300912B0310912C032BED3FE049ECCF |
:1099700050E4C801B7010E94834F18160CF0A4C030 |
:109980002BED3FE049EC50E4C801B7010E94B74E0F |
:109990006093290370932A0380932B0390932C03E5 |
:1099A000C0919D02D0919E02C051DE40D0939E0294 |
:1099B000C0939D022BED3FE049EC50E4F30164813C |
:1099C0007581868197810E94B74EF3016483758308 |
:1099D000868397832BED3FE049EC50E4F2016481EC |
:1099E0007581868197810E94B74EF20164837583E9 |
:1099F00086839783E0902903F0902A0300912B033C |
:109A000010912C0320E030E040E050E0C801B701A5 |
:109A10000E94804F88230CF45CC020E030E44FE0CB |
:109A200054E460912103709122038091230390916B |
:109A300024030E9471500E94464F70932E0360933E |
:109A40002D0320E030E44FE054E460912503709151 |
:109A5000260380912703909128030E9471500E9451 |
:109A6000464F7093300360932F0320E030E44FE0C3 |
:109A700054E46091290370912A0380912B03909103 |
:109A80002C030E9471500E94464F9B017093320339 |
:109A900060933103CE01821B930B81519E4034F0C1 |
:109AA000C051DE40D0939E02C0939D02CE01821B26 |
:109AB000930B805F914F0CF043C0C05FD14FD093A8 |
:109AC0009E02C0939D023CC0C0919D02D0919E0217 |
:109AD00099CF2BED3FE049EC50E4C801B7010E945B |
:109AE000B84E6093290370932A0380932B039093BD |
:109AF0002C03C05FD14FD0939E02C0939D022BEDEB |
:109B00003FE049EC50E4F3016481758186819781DF |
:109B10000E94B84EF30164837583868397832BED8F |
:109B20003FE049EC50E4F2016481758186819781C0 |
:109B30000E94B84EF20164837583868397836DCF4C |
:109B4000DF91CF911F910F91FF90EF90DF90CF9019 |
:109B5000BF90AF909F908F907F906F905F904F904D |
:109B60000895CF93DF93AC01029710F442E050E0E8 |
:109B7000A0914207B0914307FD01C0E0D0E020E092 |
:109B800030E020C0808191818417950769F482813B |
:109B90009381209719F09B838A8304C09093430795 |
:109BA00080934207CF0132C04817590738F4211576 |
:109BB000310519F08217930708F49C01EF01028028 |
:109BC000F381E02D3097F1F62115310589F1C901B6 |
:109BD000841B950B049708F4A901E0E0F0E026C08F |
:109BE0008D919C91119782179307E9F448175907B3 |
:109BF00079F4ED018A819B81309719F093838283F8 |
:109C000004C09093430780934207CD01029649C058 |
:109C1000841B950BFD01E80FF91F419351930297A7 |
:109C20008D939C933AC0FD01A281B3811097C1F638 |
:109C30008091400790914107892B41F48091CF0199 |
:109C40009091D00190934107809340072091D101DA |
:109C50003091D2012115310541F42DB73EB78091E5 |
:109C6000CD019091CE01281B390BE0914007F09176 |
:109C700041072E1B3F0B2417350788F0CA010296B7 |
:109C80002817390760F0CF01840F951F0296909333 |
:109C900041078093400741935193CF0102C080E078 |
:109CA00090E0DF91CF910895CF93DF93009709F46F |
:109CB0004EC0EC0122971B821A82A0914207B091FC |
:109CC0004307109711F140E050E001C0DC01AC17F0 |
:109CD000BD0700F1BB83AA83FE0121913191E20F00 |
:109CE000F31FEA17FB0771F42E5F3F4F8D919C9194 |
:109CF0001197820F931F99838883FD01828193813D |
:109D00009B838A834115510559F4D0934307C0932F |
:109D100042071DC0FD0182819381AD010097B1F61C |
:109D2000FA01D383C28321913191E20FF31FEC1723 |
:109D3000FD0769F42E5F3F4F88819981820F931F41 |
:109D4000FA01918380838A819B8193838283DF914F |
:109D5000CF910895FC014150504030F0019006161B |
:109D6000D1F73197CF010895882799270895505842 |
:109D7000192E59D101D009C1BA1762077307840798 |
:109D80009507B1F188F40EF410940B2EBA2FA02D84 |
:109D9000062E622F202D072E732F302D082E842F94 |
:109DA000402D092E952F502DFF275523B9F0591B13 |
:109DB00049F0573E98F0469537952795A795F0407E |
:109DC0005395C9F776F0BA0F621F731F841F30F4E2 |
:109DD000879577956795B795F040939517FA0F2E6D |
:109DE0000895BF1BBB27BA0B620B730B840BF6CF16 |
:109DF000DEF645C150E449EC3FE02BED6ED0A2C049 |
:109E000012D101D0C2C0552359F0992369F09F5750 |
:109E10005F57951B33F442F4903811F4915805C004 |
:109E2000CCC091589F3F09F42AC1BB271124621767 |
:109E30007307840730F4660F771F881FBB1F91508C |
:109E400098F311D00F920FD00F920DD0A0E82617E3 |
:109E5000370748071B0609F0A048BA2F602D7F91ED |
:109E60008F9100240895A0E80024621773078407E7 |
:109E7000B10528F0621B730B840BB1090A2A660F27 |
:109E8000771F881FBB1FA69581F7089597FBD7D032 |
:109E90009F3738F0FEE9F91B982F872F762F6B2F0D |
:109EA00005C0EAC09695879577956795F150D0F7EC |
:109EB0003EF490958095709561957F4F8F4F9F4FA1 |
:109EC0000895E89403C097FB0EF4F3DFB62F672FD5 |
:109ED000782F892F9EE9002458C05F77552319F405 |
:109EE00044230AF06AC02F933F934F935F9388DF18 |
:109EF00055274427C6D05F914F913F912F91F1C0D4 |
:109F00000ED05EF004C00BD026F001C008D019F0CE |
:109F100020F48FEF089580E0089581E0089597FB85 |
:109F2000092E052600F8689481D0E89407FC07C044 |
:109F3000621773078407950721F008F400940794CB |
:109F4000989408951F939F7750EC49E43FE02BEDE0 |
:109F50000FDF10E89F775FE349EC3FE02BED6217DE |
:109F600073078407950720F050EC49E401DF1127BF |
:109F700051D19068ECE7F0E023D091271F9108952C |
:109F80009A95BB0F661F771F881F11249923A1F094 |
:109F90008823B2F79F3F59F0BB0F48F421F400200B |
:109FA00011F460FF04C06F5F7F4F8F4F9F4F881F7A |
:109FB0009795879597F908955FC09FEF80EC089576 |
:109FC000FF92EF92DF92CF92BF926B017C01B5902E |
:109FD00016D0B590BB2069F09F938F937F936F93BA |
:109FE000B601C7010CD02F913F914F915F910E9414 |
:109FF000004FBF90CF90DF90EF90FF9008954DD12C |
:10A0000002C09601A701EF93FF930E947150FF9148 |
:10A01000EF9143D1EF93FF930E94B84EFF91EF91E0 |
:10A02000BA9479F70895052E092607FA440F551FAB |
:10A030005F3F79F0AA27A51708F051E04795880FF0 |
:10A04000991F9F3F31F0BB27B91708F091E0879522 |
:10A0500008959F919F911124B0CF97FB880F991F6E |
:10A060009F3F31F0BB27B91708F091E0879508951D |
:10A070009F919F911124A1CF662777278827992741 |
:10A080000895EBDFCF93DF93D52FC42F55274427B7 |
:10A09000332722279923D9F09F37C8F0F92F75DF8E |
:10A0A000592F482F372F262FF63968F4EFDE0BDFB4 |
:10A0B000C030CD0721F06993799389939993905893 |
:10A0C000DF91CF9155CE9927882777276627C03013 |
:10A0D000CD0721F02993399349935993DF91CF917B |
:10A0E00054CFA1DF01D051CF992339F0552329F066 |
:10A0F0009F575F57950F13F49AF1C1CF91589F3F27 |
:10A10000E1F3629FA12D0F92BB27639FA00DB11DAC |
:10A11000EE27729FA00DB11DEE1FAF93AA27649F7B |
:10A12000B00DE11D739FB00DE11DAA1F6627829F30 |
:10A13000B00DE11DA61F5527749FE00DA11D551FF1 |
:10A14000839FE00DA11D561F849FA00D511D852FDB |
:10A150007A2F6E2F1F900F9088231AF4939539F45D |
:10A160002CCF000C111CBB1F661F771F881F0128F6 |
:10A1700008959F939F77993358F050E449EC3FE05E |
:10A180002BEDABDE5FEB49EC3FE02BEDF1DDDADEF2 |
:10A190005F9150789527089597FD0FCF992309F483 |
:10A1A0000895482F5ADFF92FFF57F5959F1B9F1BE6 |
:10A1B000FF93EBDEFF92EF92DF92CF92BF92AF92CE |
:10A1C0009F928F926B017C0140584795332722273D |
:10A1D00040685FE3B601C70149015A010E94004F80 |
:10A1E0009401A5010E94B84E4FEF5FEF63D09B0131 |
:10A1F000AC0182169306A406B50661F78F909F9076 |
:10A20000AF90BF90CF90DF90EF90FF905F9125DFF0 |
:10A21000950FBBCE9B01AC010C9471501F93192F6D |
:10A2200010789F77993398F150E449E43FE02BEDA3 |
:10A2300054DE5FE349EC3FE02BED26173707480774 |
:10A24000590749F130F450E449E4906891DD50E851 |
:10A2500015275FE349E43FE02BED26173707480752 |
:10A26000590720F41395906849EC82DD9F938F93F2 |
:10A270007F936F93CFDF9068E2EAF0E0A1DE2F9149 |
:10A280003F914F915F912DDF10FF01C00BD017FD63 |
:10A2900090681F9108951F9190CE559145913591E9 |
:10A2A000259108959B01AC019FE380E877276627FD |
:10A2B0000C94004FD2DE992359F0AA27940FA51FC2 |
:10A2C00043F032F04FEF50E09417A50714F45DCE41 |
:10A2D000D3CE0EF006C000C09FEF80E870E060E0D3 |
:10A2E00008959FE780E870E060E00895629FD001E4 |
:10A2F000739FF001829FE00DF11D649FE00DF11D41 |
:10A30000929FF00D839FF00D749FF00D659FF00DEF |
:10A310009927729FB00DE11DF91F639FB00DE11DDC |
:10A32000F91FBD01CF0111240895991B79E004C0E4 |
:10A33000991F961708F0961B881F7A95C9F7809584 |
:10A340000895AA1BBB1B51E107C0AA1FBB1FA6177C |
:10A35000B70710F0A61BB70B881F991F5A95A9F7CE |
:10A3600080959095BC01CD01089597FB092E072695 |
:10A370000AD077FD04D0E5DF06D000201AF47095EE |
:10A3800061957F4F0895F6F7909581959F4F0895B9 |
:10A39000A1E21A2EAA1BBB1BFD010DC0AA1FBB1FE9 |
:10A3A000EE1FFF1FA217B307E407F50720F0A21B5B |
:10A3B000B30BE40BF50B661F771F881F991F1A94C8 |
:10A3C00069F760957095809590959B01AC01BD01F2 |
:10A3D000CF01089597FB092E05260ED057FD04D016 |
:10A3E000D7DF0AD0001C38F4509540953095219560 |
:10A3F0003F4F4F4F5F4F0895F6F7909580957095BA |
:10A4000061957F4F8F4F9F4F0895F999FECFB2BD51 |
:10A41000A1BDF89A119600B40895F7DF01921A943D |
:10A42000E1F70895F2DFE02DF0DFF02D0895F999BE |
:10A43000FECFB2BDA1BD00BC11960FB6F894FA9A3A |
:10A44000F99A0FBE0895F3DF012CF1DF112408956E |
:0CA450000190EDDF1A94E1F70895FFCFB2 |
:10A45C000164FFFF0A016400FF030100000000001B |
:10A46C0000000000000000000000000000000000E0 |
:10A47C0000000000000000000000000000000000D0 |
:10A48C0000000000000000000000000000000000C0 |
:10A49C0000000000000000000000000000000000B0 |
:10A4AC0000000000000000000000000000000000A0 |
:10A4BC000000000000000000000000000000000090 |
:10A4CC000000000000000000000000000000000080 |
:10A4DC000000000000000000000000000000000070 |
:10A4EC000000000000000000000000000000000060 |
:10A4FC000000000000000000000000426567696E6B |
:10A50C006E657200004B616D657261000053706F77 |
:10A51C00727400006400007D000048420120004479 |
:04A52C000700000024 |
:00000001FF |
/branches/KalmanFilter MikeW/FlightControl.c |
---|
0,0 → 1,647 |
/* |
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 "kafi.h" |
#include "FlightControl.h" |
#include "main.h" |
#include "mm3.h" |
#include "main.h" |
#include "eeprom.c" |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
#define MAX_GAS 250 |
#define MIN_GAS 15 |
#define sin45 sin(45.F / 180.F * PI) |
#define cos45 cos(45.F / 180.F * PI) |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
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; |
int DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, InitialAltitude = 0; |
int SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0; |
int GierMischanteil, GasMischanteil; |
unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
unsigned char MotorWert[5]; |
unsigned char SenderOkay = 0; |
unsigned int I2CTimeout = 100; |
char MotorenEin = 0; |
extern unsigned long maxDistance; |
extern signed int GPS_Nick, GPS_Roll; |
extern int RemoteLinkLost; |
struct mk_param_struct EE_Parameter; |
/* **************************************************************************** |
Functionname: SetNeutral */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void SetNeutral(void) |
{ |
Delay_ms(1000); |
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 = 2000; |
} |
/* **************************************************************************** |
Functionname: GetMeasurements */ /*! |
Description: CalculateAverage |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void GetMeasurements(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 1 |
DebugOut.Analog[7] = AdWertNick_Raw; |
DebugOut.Analog[8] = AdWertRoll_Raw; |
DebugOut.Analog[9] = AdWertGier_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: 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: RemoteControl */ /*! |
Description: |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void RemoteControl(void) |
{ |
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; |
} |
} |
} |
else |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang gut |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay > 140) |
{ |
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: PD_Regler */ /*! |
Description: |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void PD_Regler(void) |
{ |
int StickNick45,StickRoll45; |
int DiffNick,DiffRoll, DiffGier; |
int motorwert = 0; |
int pd_ergebnis; |
/***************************************************************************** |
Update noimial attitude |
**************************************************************************** */ |
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 |
#ifdef USE_GPS |
/* G P S U p d a t e */ |
GPSupdate(); |
#endif |
static float AverageGasMischanteil = 50.F; |
if((GasMischanteil > 30) && |
(MotorenEin == 1) && |
(RemoteLinkLost == 0) ) |
{ /* Average the average throttle to find the hover setting */ |
AverageGasMischanteil = 0.999F * AverageGasMischanteil + 0.001F * GasMischanteil; |
} |
/* Overide GasMischanteil */ |
static unsigned int DescentCnt = 32000; |
if ((RemoteLinkLost == 1) && |
(MotorenEin == 1)) |
{ |
if ((UBat < 100) || /* Start to descent in case of low loltage or in case*/ |
(maxDistance / 10 < 12)) /* we reached our target position */ |
{ |
if (DescentCnt > 0) |
{ |
DescentCnt--; |
} |
else |
{ /* We reached our target (hopefully) */ |
MotorenEin = 0; |
RemoteLinkLost = 0; |
} |
} |
else |
{ |
DescentCnt = 32000; |
} |
/* Bias the throttle for a slow descent */ |
GasMischanteil = (int) ((AverageGasMischanteil + 5.0F) * (DescentCnt / 32000.F)); |
} |
else |
{ |
DescentCnt = 32000; |
} |
//DebugOut.Analog[13] = (int) GasMischanteil; |
/* Overide in case the remote link got lost */ |
if (RemoteLinkLost == 0) |
{ /* We are flying in X-Formation */ |
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)); |
} |
else |
{ /* GPS overide is aktive */ |
StickRoll45 = (int) (0.707F * (float)(-GPS_Roll) - 0.707F * (float)(-GPS_Nick)); |
StickNick45 = (int) (0.707F * (float)(-GPS_Roll) + 0.707F * (float)(-GPS_Nick)); |
StickGier = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yaw |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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; |
} |
/***************************************************************************** |
PD-Control |
**************************************************************************** */ |
int scale_p; |
int scale_d; |
scale_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
scale_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 20); |
scale_p = 6; |
scale_d = 7; |
//DebugOut.Analog[14] = scale_p; |
//DebugOut.Analog[15] = scale_d; |
/* Pitch */ |
DiffNick = -(status.iTheta10 + StickNick45); |
/* R o l l */ |
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; |
#endif |
} |
/branches/KalmanFilter MikeW/FlightControl.h |
---|
0,0 → 1,44 |
/* |
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. |
*/ |
/***************************************************************************** |
Additional Defines |
**************************************************************************** */ |
//#define USE_COMPASS |
//#define INTERNAL_REFERENCE |
//#define MELEXIS_GYRO |
//#define USE_OSD |
//#define USE_GPS |
extern int MesswertNick,MesswertRoll,MesswertGier; |
extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier; |
extern int NeutralAccX, NeutralAccY, NeutralAccZ; |
extern int GierMischanteil,GasMischanteil; |
extern int AverageRoll_X, AverageRoll_Y, AverageRoll_Z; |
extern int AveragerACC_X, AveragerACC_Y, AveragerACC_Z; |
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 GetMeasurements(void); |
void GetRadioValues(void); |
void SendMotorData(void); |
void PID_Regler(void); |
void SetNeutral(void); |
/branches/KalmanFilter MikeW/Kopter_Tool_V1_48/Legend.txt |
---|
0,0 → 1,32 |
Nick * 10 |
Roll * 10 |
Gier |
Accumulated Nick |
Accumulated Roll |
Accumulated Gier |
na |
AdWertNick |
AdWertRoll |
AdWertGier |
na |
na |
RCQuality |
na |
CycleTime |
VCC |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
na |
/branches/KalmanFilter MikeW/Kopter_Tool_V1_48/MikroKopter-Tool.exe |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/branches/KalmanFilter MikeW/License_buss.txt |
---|
0,0 → 1,47 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 (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 veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlikt 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 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 |
// + 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. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/branches/KalmanFilter MikeW/_Settings.h |
---|
7,12 → 7,12 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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 |
#define K_NICK 0 |
#define K_ROLL 1 |
#define K_GAS 2 |
#define K_GIER 3 |
#define K_POTI1 4 |
#define K_POTI2 5 |
#define K_POTI3 6 |
#define K_POTI4 7 |
/branches/KalmanFilter MikeW/analog.c |
---|
13,20 → 13,21 |
*/ |
#include "main.h" |
#include "KalmanFc.h" |
#include "FlightControl.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 UBat = 100; /* Initial Battery Guess */ |
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; |
unsigned int AdWertAirPressure_Raw = 1023; |
/* **************************************************************************** |
Functionname: ADC_Init */ /*! |
39,9 → 40,12 |
**************************************************************************** */ |
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 |
#ifdef INTERNAL_REFERENCE |
ADMUX = 64;/* Internal Reference 5V */ |
#else |
ADMUX = 0; /* External Reference */ |
#endif |
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE); |
} |
/* **************************************************************************** |
55,21 → 59,21 |
**************************************************************************** */ |
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); |
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); |
} |
/* **************************************************************************** |
83,68 → 87,76 |
**************************************************************************** */ |
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; |
static unsigned char kanal=0,state = 0; |
ANALOG_OFF; |
switch(state++) |
{ |
case 0: |
AdWertGier = ADC; |
AdWertGier_Raw = ADC; |
AccumulatedRoll_Z += (ADC - AdNeutralGier); |
AccumulatedRoll_Z_cnt++; |
kanal = 1; |
break; |
case 1: |
AdWertRoll = ADC; |
AdWertRoll_Raw = ADC; |
AccumulatedRoll_X += (ADC - AdNeutralRoll); |
AccumulatedRoll_X_cnt++; |
kanal = 2; |
break; |
case 2: |
AdWertNick = ADC; |
AdWertNick_Raw = ADC; |
AccumulatedRoll_Y += (ADC - AdNeutralNick); |
AccumulatedRoll_Y_cnt++; |
kanal = 4; |
break; |
case 3: |
#ifdef INTERNAL_REFERENCE |
UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */ |
#else |
UBat = (3 * UBat + ADC / 3) / 4; |
#endif |
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; |
#ifdef INTERNAL_REFERENCE |
/* Add 64 in order to use the internal 5v refenrence */ |
ADMUX += 64; |
#endif |
ANALOG_ON; |
} |
/branches/KalmanFilter MikeW/analog.h |
---|
1,7 → 1,27 |
/* |
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 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; |
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; |
void ADC_Init(void); |
void SucheLuftruckOffset(void); |
/branches/KalmanFilter MikeW/eeprom.c |
---|
7,158 → 7,158 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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); |
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 = 96; // 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); |
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); |
EE_Parameter.Kanalbelegung[K_NICK] = 1; |
EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
EE_Parameter.Kanalbelegung[K_GAS] = 3; |
EE_Parameter.Kanalbelegung[K_GIER] = 4; |
EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
EE_Parameter.GlobalConfig = CFG_HOEHENREGELUNG | CFG_DREHRATEN_BEGRENZER | CFG_ACHSENKOPPLUNG_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 100; // Wert : 0-250 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
EE_Parameter.Stick_P = 3; //2 // Wert : 1-6 |
EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
EE_Parameter.Gier_P = 8; // Wert : 1-20 |
EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 200; //80 // Wert : 0-250 |
EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 0; |
EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
EE_Parameter.AchsKopplung1 = 100; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt |
EE_Parameter.AchsGegenKopplung1 = 80; |
EE_Parameter.WinkelUmschlagNick = 100; |
EE_Parameter.WinkelUmschlagRoll = 100; |
EE_Parameter.GyroAccAbgleich = 16; // 1/k |
memcpy(EE_Parameter.Name, "Beginner\0", 12); |
} |
/branches/KalmanFilter MikeW/fc.h |
---|
9,7 → 9,6 |
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; |
21,54 → 20,54 |
#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]; |
}; |
{ |
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; |
/branches/KalmanFilter MikeW/flight.pnproj |
---|
1,0 → 0,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> |
<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.h"></File><File path="eeprom.c"></File><File path="kafi.c"></File><File path="kafi.h"></File><File path="matmatrix.c"></File><File path="mymath.c"></File><File path="mm3.h"></File><File path="mm3.c"></File><File path="main.c"></File><File path="Bob4_OSD.c"></File><File path="gps.c"></File><File path="matmatrix.h"></File><File path="FlightControl.c"></File><File path="FlightControl.h"></File></Project> |
/branches/KalmanFilter MikeW/gps.c |
---|
12,61 → 12,74 |
unless it is stated otherwise. |
*/ |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "main.h" |
#include "kafi.h" |
#include "mymath.h" |
#include <math.h> |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
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) |
int holdPosValid = 0; |
NAV_STATUS_t navStatus; |
NAV_POSLLH_t navPosLlh; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
volatile gpsInfo_t actualPos;// measured position (last gps record) |
volatile gpsInfo_t targetPos;// measured position (last gps record) |
volatile gpsInfo_t homePos;// measured position (last gps record) |
volatile gpsInfo_t holdPos; // measured position (last gps record) |
NAV_STATUS_t navStatus; |
NAV_POSLLH_t navPosLlh; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
uint8_t gpsState; |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
long distanceNS = 0; |
long distanceEW = 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 nick_gain = 0; |
int nick_gain_p, nick_gain_d; |
int roll_gain_p, roll_gain_d; |
int Override = 0; |
int TargetGier = 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; |
extern int RemoteLinkLost; |
volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered |
volatile uint8_t CK_A, CK_B;// UBX checksum bytes |
volatile uint8_t ignorePacket;// true when previous packet was not processed |
volatile unsigned short msgLen; |
volatile uint8_t msgID; |
void GPSscanData (void); |
void GPSupdate(void); |
void SetNewHeading(unsigned long maxDistance); |
/* **************************************************************************** |
Functionname: GPSupdate */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
74,208 → 87,271 |
**************************************************************************** */ |
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; |
} |
float SIN_H, COS_H; |
long max_p = 0; |
long max_d = 0; |
int SwitchPos = 0; |
/* Determine Selector Switch Position */ |
if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100) |
{ |
SwitchPos = 0; |
} |
else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) |
{ |
SwitchPos = 2;/* Target Mode */ |
} |
else |
{ |
SwitchPos = 1;/* Position Hold */ |
} |
/* Overide On / Off */ |
if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10) |
{ |
Override = 1; |
} |
else |
{ |
Override = 0; |
} |
/* Set Home Position */ |
if ((actualPos.state > 2) && |
(homePosValid == 0)) |
{ |
homePos.north = actualPos.north; |
homePos.east = actualPos.east; |
homePos.altitude = actualPos.altitude ; |
homePosValid = 1; |
} |
/* Set Target Position */ |
if ((actualPos.state > 2) && |
(SwitchPos == 0)) |
{ |
targetPos.north = actualPos.north; |
targetPos.east = actualPos.east; |
targetPos.altitude = actualPos.altitude ; |
targetPosValid = 1; |
} |
if ((actualPos.state < 3) && |
(SwitchPos == 0)) |
{ |
targetPosValid = 0; |
} |
/* Set Hold Position */ |
if ((actualPos.state > 2) && |
((Override == 1) || |
(SwitchPos == 2) )) /* Update the hold position in case we are in target mode */ |
{ |
holdPos.north = actualPos.north; |
holdPos.east = actualPos.east; |
holdPos.altitude = actualPos.altitude ; |
holdPosValid = 1; |
} |
if ((actualPos.state < 3) && |
(Override == 1)) |
{ |
holdPosValid = 0; |
} |
/* Indicate Valid GPS Position */ |
if ((actualPos.state > 2) && |
(SwitchPos == 0)) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 5000; |
BeepMuster = 0x0100; |
} |
} |
if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */ |
{ |
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); |
} |
else |
{ |
max_p = 8; |
max_d = 4; |
} |
/* Those seem to be good values */ |
max_p = 8; |
max_d = 4; |
//DebugOut.Analog[11] = max_p; |
//DebugOut.Analog[12] = max_d; |
static int GPSTrackingCycles = 0; |
long maxGainPos = 140; |
long maxGainVel = 140; |
/* Ramp up gain */ |
if (GPSTrackingCycles < 1000) |
{ |
GPSTrackingCycles++; |
} |
maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
/* Determine Offset from nominal Position */ |
if (actualPos.state > 2 ) |
{ |
if ((SwitchPos == 2) && |
(targetPosValid == 1) && |
(RemoteLinkLost == 0) && |
(Override == 0)) |
{ /* determine distance from target position */ |
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); |
} |
else if ((SwitchPos == 1)&& |
(holdPosValid == 1) && |
(RemoteLinkLost == 0) && |
(Override == 0)) |
{ /* determine distance from hold position */ |
distanceNS = actualPos.north - holdPos.north; // in 0.1m |
distanceEW = actualPos.east - holdPos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
} |
else if ((RemoteLinkLost == 1) && |
(homePosValid ==1)) /* determine distance from target position */ |
{/* Overide in case the remote link got lost */ |
distanceNS = actualPos.north - homePos.north; // in 0.1m |
distanceEW = actualPos.east - homePos.east; // in 0.1m |
velocityNS = actualPos.velNorth; // in 0.1m/s |
velocityEW = actualPos.velEast; // in 0.1m/s |
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
} |
else |
{ |
distanceNS = 0.0F; // in 0.1m |
distanceEW = 0.0F; // in 0.1m |
velocityNS = 0.0F; // in 0.1m/s |
velocityEW = 0.0F; // in 0.1m/s |
maxDistance = 0.0F; |
GPSTrackingCycles = 0; |
} |
/* 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, ((velocityNS * 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, ((velocityEW * max_d)/50))); //m/s |
TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
/* PD-Control */ |
nick_gain = nick_gain_p + nick_gain_d; |
roll_gain = -(roll_gain_p + roll_gain_d); |
/* Calculate Distance to Target */ |
SIN_H = (float) sin(status.Psi); |
COS_H =(float) cos(status.Psi); |
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)); |
/* Set New Heading */ |
SetNewHeading(maxDistance); |
} |
else |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
maxDistance = 0.0F; |
} |
} |
/* 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; |
if (maxDistance / 10 < 12) |
{ |
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 |
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; |
} |
void SetNewHeading(unsigned long maxDistance) |
/* Set New Heading */ |
{ |
int OffsetGier = 0; |
if (maxDistance / 10 > 8) |
{ |
OffsetGier = 4; |
} |
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; |
} |
} |
} |
/* **************************************************************************** |
289,68 → 365,68 |
**************************************************************************** */ |
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; |
// 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; |
} |
365,41 → 441,41 |
**************************************************************************** */ |
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; |
} |
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; |
} |
} |
/* **************************************************************************** |
413,133 → 489,133 |
**************************************************************************** */ |
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; |
} |
uint8_t c; |
uint8_t re; |
re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ? |
c = UDR1; |
if (re == 0) |
{ |
switch (gpsState) |
{ |
case GPS_EMPTY: |
if (c == SYNC_CHAR1) |
{ |
gpsState = GPS_SYNC1; |
} |
break; |
case GPS_SYNC1: |
if (c == SYNC_CHAR2) |
{ |
gpsState = GPS_SYNC2; |
} |
else if (c != SYNC_CHAR1) |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_SYNC2: |
if (c == CLASS_NAV) |
{ |
gpsState = GPS_CLASS; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CLASS:// msg ID seen: init packed receive |
msgID = c; |
CK_A = CLASS_NAV + c; |
CK_B = CLASS_NAV + CK_A; |
gpsState = GPS_LEN1; |
switch (msgID) |
{ |
case MSGID_STATUS: |
ubxP = (char*)&navStatus; |
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
ubxSp = (char*)&navStatus.packetStatus; |
ignorePacket = navStatus.packetStatus; |
break; |
/* |
case MSGID_POSLLH: |
ubxP = (char*)&navPosLlh; |
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
ubxSp = (char*)&navPosLlh.packetStatus; |
ignorePacket = navPosLlh.packetStatus; |
break; |
*/ |
case MSGID_POSUTM: |
ubxP = (char*)&navPosUtm; |
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
ubxSp = (char*)&navPosUtm.packetStatus; |
ignorePacket = navPosUtm.packetStatus; |
break; |
case MSGID_VELNED: |
ubxP = (char*)&navVelNed; |
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
ubxSp = (char*)&navVelNed.packetStatus; |
ignorePacket = navVelNed.packetStatus; |
break; |
default: |
ignorePacket = 1; |
ubxSp = (char*)0; |
} |
break; |
case GPS_LEN1:// first len byte |
msgLen = c; |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_LEN2; |
break; |
case GPS_LEN2:// second len byte |
msgLen = msgLen + (c * 256); |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_FILLING;// next data will be stored in packet struct |
break; |
case GPS_FILLING: |
CK_A += c; |
CK_B += CK_A; |
if ( !ignorePacket && ubxP < ubxEp) |
{ |
*ubxP++ = c; |
} |
if (--msgLen == 0) |
{ |
gpsState = GPS_CKA; |
} |
break; |
case GPS_CKA: |
if (c == CK_A) |
{ |
gpsState = GPS_CKB; |
} |
else |
{ |
gpsState = GPS_EMPTY; |
} |
break; |
case GPS_CKB: |
if (c == CK_B && ubxSp)// No error -> packet received successfully |
{ |
*ubxSp = 1;// set packetStatus in struct |
GPSscanData(); |
} |
gpsState = GPS_EMPTY;// ready for next packet |
break; |
default: |
gpsState = GPS_EMPTY;// ready for next packet |
} |
} |
else// discard any data if error occured |
{ |
gpsState = GPS_EMPTY; |
} |
} |
/branches/KalmanFilter MikeW/gps.h |
---|
1,95 → 1,95 |
#define _B1(bit) (1 << (bit)) |
#define _B0(bit) (0 << (bit)) |
#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 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 |
#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 |
#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; |
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; |
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; |
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; |
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; |
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; |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long VEL_N; // cm/s NED north velocity |
long VEL_E; // cm/s NED east velocity |
long VEL_D; // cm/s NED down velocity |
unsigned long Speed; // cm/s Speed (3-D) |
unsigned long GSpeed; // cm/s Ground Speed (2-D) |
long Heading; // deg (1e-05) Heading 2-D |
unsigned long SAcc;// cm/s Speed Accuracy Estimate |
unsigned long CAcc; // deg Course / Heading Accuracy Estimate |
uint8_t packetStatus; |
} NAV_VELNED_t; |
/branches/KalmanFilter MikeW/kafi.c |
---|
1,15 → 1,735 |
/* |
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 "kafi.h" |
#include "analog.h" |
#include "main.h" |
#include "FlightControl.h" |
#include "mm3.h" |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
volatile int KompassValue = 0; |
/* Kalman filter */ |
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; |
f32_t **matPs; /* Predicted system Noise */ |
f32_t **matP; /* Predicted system Noise */ |
f32_t **matP_tmp; |
f32_t **matQ; /* system noise covariance */ |
f32_t **matR; /* measurement noise covariance */ |
f32_t **matXd; /* estimated state */ |
f32_t **matXs; /* predicted state */ |
f32_t **matX_tmp1; |
f32_t **matX_tmp2; |
f32_t **matPhi; /* transition matrix to predict new state from current state */ |
f32_t **matB ; /* control matrix to predict new state from ext. control vector U */ |
f32_t **matU; /* control vector */ |
f32_t **matC; /* Jacobi matrix */ |
f32_t **matY; /* real measurements */ |
f32_t **matYs; /* predicted measurements */ |
f32_t **matdY; /* innovation */ |
f32_t **temp_meas_meas; |
f32_t **temp_meas_state; |
f32_t **temp_state_meas; |
f32_t **temp_meas_2; |
f32_t **temp_state_state; |
f32_t **matK; /* the Kalman gain matrix */ |
char fYValid[KAFI_DIM_Y]; /* measurement validity flag */ |
/* **************************************************************************** |
Functionname: AttitudeEstimation */ /*! |
Description: Main Kalman Filter Loop |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void FlightAttitudeEstimation() |
{ |
/* Predict the Measurent */ |
trPredictMeasurement(); |
/* Update the Jacobi Matrix */ |
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(); // Is an Identity matrix which will not change |
/* Update control matrix B */ |
trUpdateBU(); |
/* Predict new state Xs and Ps */ |
KAFIPrediction(); |
/* 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); |
/* Limit Angles to [-2Pi;...;+2Pi] */ |
trLimitAngles(); |
} |
/* **************************************************************************** |
Functionname: Kafi_Init */ /*! |
Description: Generate Kalman Matrizes |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void Kafi_Init() |
{ |
/* create kalman filter and associated matrizes */ |
ui32_t i, j; |
matPs = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
matQ = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
matR = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_Y); |
matXd = matrix( 1, KAFI_DIM_X, 1,1); /* estimated state */ |
matXs = matrix( 1, KAFI_DIM_X, 1,1); /* predicted state */ |
matX_tmp1 = matrix( 1, KAFI_DIM_X,1, 1); |
matX_tmp2 = matrix( 1, KAFI_DIM_X, 1,1); |
matPhi = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X);/* transition matrix to predict new state from current state */ |
matB = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_U); /* control matrix to predict new state from ext. control vector U */ |
matU = matrix( 1, KAFI_DIM_U, 1,1); /* control vector */ |
matC = matrix( 1, KAFI_DIM_Y, 1,KAFI_DIM_X); /* Jacobi matrix */ |
matY = matrix( 1, KAFI_DIM_Y, 1,1); /* real measurements */ |
matdY = matrix( 1, KAFI_DIM_Y, 1,1); /* innovation */ |
matYs = matrix( 1, KAFI_DIM_Y, 1,1); /* predicted measurements */ |
matP = matrix( 1, KAFI_DIM_X, 1,KAFI_DIM_X); |
matK = matrix(1, KAFI_DIM_X, 1, KAFI_DIM_Y); |
temp_state_state = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X ); |
temp_meas_state = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_X ); |
temp_state_meas = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_Y ); |
temp_meas_meas = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
temp_meas_2 = matrix( 1, KAFI_DIM_Y, 1, KAFI_DIM_Y ); |
matP_tmp = matrix( 1, KAFI_DIM_X, 1, KAFI_DIM_X); |
/* set phi to identity matrix */ |
for ( j = 0; j < KAFI_DIM_X; j++ ) |
{ |
for ( i = 0; i < KAFI_DIM_X; 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); |
matSetDiagonal(matR, _compass, _compass, 5.0F); |
#ifdef MELEXIS_GYRO |
/* set diagonal of system noise covariance Q, dim(X,X) */ |
matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
#else |
/* 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); |
#endif |
/* set diagonal init. estimation error (covariance) P0 */ |
matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
matSetDiagonal(matPs, _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); |
} |
/* **************************************************************************** |
Functionname: KAFIPrediction */ /*! |
Description: Predict new state Xs and Ps |
X_k = A * X_(k-1) + B * u_k |
P_k = A * P_(k-1) * A^T + Q |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void KAFIPrediction() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
/* Innovation: calculate Xd, and Pd */ |
/* X_k = A * X_(k-1) + B * u_k */ |
/* matX_tmp1 = matPhi * matXd */ |
/* |
matrix_mult(matPhi,matXd, matX_tmp1, |
KAFI_DIM_X, KAFI_DIM_X, 1); |
*/ |
/* matX_tmp2 = matB * matU */ |
matrix_mult( matB,matU, matX_tmp2, |
KAFI_DIM_X, KAFI_DIM_U, 1); |
/* matXs = matPhi * matXd + matB * matU */ |
/* |
matrix_add( matX_tmp1, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
matrix_add( matXd, matX_tmp2, matXs, KAFI_DIM_X, 1 ); |
*/ |
matXs[1][1] = matXd[1][1] + matX_tmp2[1][1]; |
matXs[2][1] = matXd[2][1] + matX_tmp2[2][1]; |
matXs[3][1] = matXd[3][1] + matX_tmp2[3][1]; |
/*P_k = A * P_(k-1)*A^T + Q */ |
/* |
matrix_mult_transpose( matP, matPhi, matP_tmp, // Can be skiped PHI is a ident matrix |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
matrix_mult( matPhi, matP_tmp, matP, |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_X ); |
matrix_add( matP, matQ, matPs, KAFI_DIM_X, KAFI_DIM_X ); |
*/ |
matPs[1][1] = matP[1][1] + matQ[1][1]; |
matPs[2][2] = matP[2][2] + matQ[2][2]; |
matPs[3][3] = matP[3][3] + matQ[3][3]; |
} |
/* **************************************************************************** |
Functionname: trInnovate */ /*! |
Description: Innovation: calculate Xd, and Pd |
K = Ps * C^T * (C * Ps * C^T + R)^-1 |
X = Xs + K (Y - Ys) |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trInnovate() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
#if 0 /* Runtime optimised. To be verified */ |
/* temp_meas_state = matC * matPs */ |
matrix_mult( matC, matPs, temp_meas_state, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
/* temp_meas_meas = matC * matPs^T * matC^T */ |
matrix_mult_transpose( matC, temp_meas_state, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
/* temp_meas_meas = matC * matPs^T * matC^T + matR */ |
/* |
matrix_add( temp_meas_meas, matR, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_Y ); |
*/ |
temp_meas_meas[1][1] += matR[1][1]; |
temp_meas_meas[2][2] += matR[2][2]; |
temp_meas_meas[3][3] += matR[3][3]; |
temp_meas_meas[4][4] += matR[4][4]; |
/* temp_meas_2 = (matC * matPs^T * matC^T + matR)^-1 */ |
take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
/* matK = matPs^T * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
matrix_transpose_mult( temp_meas_state, temp_meas_2, matK, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
/* temp_state_state = matK * matC * matPs */ |
matrix_mult( matK, temp_meas_state, temp_state_state, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
/* matP = matPs - matK * matC * matPs */ |
matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
#else |
/* temp_state_meas = matPs * matC^T */ |
matrix_mult_transpose( matPs, matC, temp_state_meas, |
KAFI_DIM_X, KAFI_DIM_X, KAFI_DIM_Y ); |
/* temp_meas_meas = matC * matPs * matC^T */ |
matrix_mult( matC, temp_state_meas, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_Y ); |
/* temp_meas_meas = matC * matPs * matC^T + matR */ |
/* |
matrix_add( temp_meas_meas, matR, temp_meas_meas, |
KAFI_DIM_Y, KAFI_DIM_Y ); |
*/ |
temp_meas_meas[1][1] += matR[1][1]; |
temp_meas_meas[2][2] += matR[2][2]; |
temp_meas_meas[3][3] += matR[3][3]; |
temp_meas_meas[4][4] += matR[4][4]; |
/* temp_meas_2 = (matC * matPs * matC^T + matR)^-1 */ |
take_inverse( temp_meas_meas, temp_meas_2, KAFI_DIM_Y ); |
/* matK = matPs * matC^T * (matC * matPs^T * matC^T + matR)^-1 */ |
matrix_mult( temp_state_meas, temp_meas_2, matK, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_Y ); |
/* temp_meas_state = matC * matPs*/ |
matrix_mult( matC, matPs, temp_meas_state, |
KAFI_DIM_Y, KAFI_DIM_X, KAFI_DIM_X ); |
/* temp_state_state = matK * matC * matPs */ |
matrix_mult( matK, temp_meas_state, temp_state_state, |
KAFI_DIM_X, KAFI_DIM_Y, KAFI_DIM_X ); |
/* matP = matPs - matK * matC * matPs */ |
matrix_sub( matPs, temp_state_state, matP, KAFI_DIM_X, KAFI_DIM_X ); |
#endif |
/* matdY = matY - matYs */ |
/* |
matrix_sub( matY, matYs, matdY, |
KAFI_DIM_Y, 1 ); |
*/ |
matdY[1][1] = matY[1][1] - matYs[1][1]; |
matdY[2][1] = matY[2][1] - matYs[2][1]; |
matdY[3][1] = matY[3][1] - matYs[3][1]; |
/* We will only update the Heading if we got a valid compass signal */ |
if(fYValid[_compass] == 1) |
{ |
matdY[4][1] = matY[4][1] - matYs[4][1]; |
} |
else |
{ |
matdY[4][1] = 0.0F; |
} |
/* matX_tmp1 = matK * (Y -Ys) */ |
matrix_mult( matK, matdY, matX_tmp1, |
KAFI_DIM_X, KAFI_DIM_Y, 1 ); |
/* matXd = matXs + matK * (Y -Ys) */ |
/* |
matrix_add( matXs, matX_tmp1, matXd, |
KAFI_DIM_X, 1 ); |
*/ |
matXd[1][1] = matXs[1][1] + matX_tmp1[1][1]; |
matXd[2][1] = matXs[2][1] + matX_tmp1[2][1]; |
matXd[3][1] = matXs[3][1] + matX_tmp1[3][1]; |
matXd[4][1] = matXs[4][1] + matX_tmp1[4][1]; |
} |
/* **************************************************************************** |
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); |
} |
/* **************************************************************************** |
Functionname: trUpdateJacobiMatrix */ /*! |
Description: |
@param[in] void |
@return void |
@pre - |
@post - |
@author Michael Walter |
*****************************************************************************/ |
void trUpdateJacobiMatrix(void) |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/* |
Simplified Version d/dt |
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); |
} |
/***************************************************************************** |
VARIABLES |
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); |
} |
/* **************************************************************************** |
Functionname: trMeasure */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void trMeasure() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
#ifdef USE_COMPASS |
static int updCompass = 10; |
f32_t Psi, Compass; |
#endif |
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; |
#ifdef INTERNAL_REFERENCE |
ADC_ax = AdWertAccNick * 0.044F; |
ADC_ay = -AdWertAccRoll * 0.044F; |
ADC_az = AdWertAccHoch * 0.044F; |
#else |
ADC_ax = AdWertAccNick * 0.047F; |
ADC_ay = -AdWertAccRoll * 0.047F; |
ADC_az = AdWertAccHoch * 0.047F; |
#endif |
matSetFull(matY, _ax, 0, ADC_ax); |
matSetFull(matY, _ay, 0, ADC_ay); |
matSetFull(matY, _az, 0, ADC_az); |
#ifdef USE_COMPASS |
if (!updCompass--) |
{ |
updCompass = 10; // update only at 2ms*50 = 100ms (10Hz) |
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; |
} |
#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 |
*/ |
static f32_t fSumGier = 0.0F; |
static f32_t fSumNick = 0.0F; |
static f32_t fSumRoll = 0.0F; |
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; |
fSumGier += ADC_r * 180.F / 3.14F; |
fSumNick += ADC_q * 180.F / 3.14F; |
fSumRoll += ADC_p * 180.F / 3.14F; |
DebugOut.Analog[3] = fSumNick; |
DebugOut.Analog[4] = fSumRoll; |
DebugOut.Analog[5] = fSumGier; |
matSetFull(matU, _p, 0, ADC_p ); |
matSetFull(matU, _q, 0, ADC_q); |
matSetFull(matU, _r, 0, ADC_r); |
} |
/* **************************************************************************** |
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; |
} |
} |
/* **************************************************************************** |
Functionname: trEstimateVelocity */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author |
**************************************************************************** */ |
void trEstimateVelocity() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
} |
/branches/KalmanFilter MikeW/kafi.h |
---|
15,12 → 15,16 |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "mat.h" |
#include "matmatrix.h" |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
extern int sollGier; |
/* *************************************************************************** */ |
/* Uses the Compass to Estimate Theta, Phi, Psi */ |
31,19 → 35,68 |
#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)*/ |
#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)*/ |
#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 MELEXIS_GYRO |
#define fCycleTime 12.5F; /* Use Different Cycle Times to account for Runtime / Gain Differences */ |
#else |
#ifdef USE_Extended_MM3_Measurement_Model |
#define fCycleTime 16.0F; |
#define fCycleTime 16.0F; |
#else |
#define fCycleTime 10.5F; |
#define fCycleTime 12.5F; |
#endif |
#endif |
/* Predict the Measurent */ |
void trPredictMeasurement(void); |
/* Update the Jacobi Matrix */ |
void trUpdateJacobiMatrix(void); |
/* Extract measurements and store them in vector matY. |
Measurement validity is indicated by fYValid[] */ |
void trMeasure(void); |
/* Innovation: calculate Xd, and Pd */ |
void trInnovate(void); |
/* Update transition matrix Phi */ |
void trUpdatePhi(void); |
/* Update control matrix B */ |
void trUpdateBU(void); |
/* Predict new state Xs and Ps */ |
void KAFIPrediction(void); |
/* Main Kalman Filter Loop */ |
void FlightAttitudeEstimation(void); |
/* Setup the Kalman Filter Parameter */ |
void Kafi_Init(void); |
/* Not done yet */ |
void trEstimateVelocity(void); |
/* Limit Angles to [-2Pi;...;+2Pi] */ |
void trLimitAngles(void); |
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; |
/*typedef*/ enum |
{ |
_p = 0, |
75,7 → 128,6 |
} /*trObservationVariables_e*/; |
#endif |
/*typedef*/ enum |
{ |
_Phi = 0, |
88,40 → 140,7 |
} /*trStateVariables_e*/; |
typedef struct |
{ |
f32_t Phi; |
f32_t Theta; |
f32_t Psi; |
i32_t iPhi10; |
i32_t iTheta10; |
i32_t iPsi10; |
f32_t dPhi; |
f32_t dTheta; |
f32_t dPsi; |
f32_t du; |
f32_t dv; |
f32_t dw; |
f32_t X; |
f32_t Y; |
f32_t Z; |
} status_t; |
status_t status; |
void trUpdatePhi(void); |
void trUpdateBU(void); |
void trMeasure(void); |
void trInnovate(void); |
void trResetKalmanFilter(void); |
void AttitudeEstimation(void); |
int KAFIInit(kafi_t *pkafi); |
void Kafi_Init(void); |
void trEstimateVelocity(void); |
void trLimitAngles(void); |
void trControl(void); |
/branches/KalmanFilter MikeW/main.c |
---|
1,75 → 1,210 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt und genannt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/* |
Copyright 2008, by Michael Walter |
All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
License along with this program. If not, see <http://www.gnu.org/licenses/>. |
Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
unless it is stated otherwise. |
*/ |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "main.h" |
#include "kafi.h" |
#include "mymath.h" |
#include "mm3.h" |
/***************************************************************************** |
(SYMBOLIC) CONSTANTS |
*****************************************************************************/ |
#define sin45 -0.707106 |
#define cos45 0.707106 |
/***************************************************************************** |
VARIABLES |
*****************************************************************************/ |
extern int RCQuality; |
int RemoteLinkLost; |
extern unsigned long maxDistance; |
unsigned char EEPromArray[E2END+1] EEMEM; |
void TestRemote(void); |
void IMU_Main(void); |
void TestBattery(void); |
void SendDebugData(void); |
void InitPorts(void); |
void GenerateDefaults(void); |
void TestIC2Link(void); |
void GetAirPressureOffset(void); |
void DeployRescue(void); |
extern void InitOSD(void); |
extern void InitGPS(void); |
extern void SendOSD(void); |
extern void MotorControl(void); |
extern void RemoteControl(void); |
extern void SendMotorData(void); |
extern void CalculateAverage(void); |
extern void GetMeasurements(void); |
extern void AttitudeEstimation(void); |
extern void PID_Regler(void); |
extern void PD_Regler(void); |
extern void SetNeutral(void); |
/* **************************************************************************** |
Functionname: main */ /*! |
Description: Hauptprogramm |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
int main (void) |
{ |
/* Controler Init */ |
InitPorts(); |
Kafi_Init(); |
Timer_Init(); |
UART_Init(); |
InitGPS(); |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
#ifdef USE_COMPASS |
MM3_Init(); |
#endif |
/* Enable Interrupts */ |
sei(); |
/* Generate Default Values */ |
GenerateDefaults (); |
/* Get Air Pressure Offset */ |
GetAirPressureOffset(); |
/* Determine Gyro / ACC Offsets */ |
SetNeutral(); |
DebugIn.Analog[1] = 1000; |
DebugIn.Digital[0] = 0x55; |
#ifdef USE_COMPASS |
/* Calibrate the MM3 Compass? */ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && |
(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && |
(MotorenEin == 0)) |
{ |
printf("\n\rCalibrating Compass"); |
MM3_Calibrate(); |
} |
#endif |
#ifdef USE_OSD |
/* Init the Bob-4 OnScreen Display */ |
InitOSD(); |
#endif |
/* Start the main Task */ |
IMU_Main(); |
return (1); |
} |
/* **************************************************************************** |
Functionname: IMU_Main */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void IMU_Main() |
{ |
ROT_ON |
I2CTimeout = 5000; |
while (1) |
{ |
static i32_t OldTime = 0; |
if (UpdateMotor) |
{ |
UpdateMotor=0; |
#ifdef USE_OSD |
/* G e n e r a t e O S D D a t a */ |
static char CntOSD = 0; |
if (CntOSD % 6 == 1) |
{ |
SendOSD(); |
} |
CntOSD++; |
#endif |
/* Set the cycle Time to 120ms / 125ms */ |
if (OldTime != 0) |
{ |
while (((Count8Khz - OldTime) *10) / 8 < 120); |
DebugOut.Analog[14] = ((Count8Khz - OldTime) *10) / 8; |
} |
OldTime = Count8Khz; |
/*GetMeasurements()*/ |
GetMeasurements(); |
/*EstimateFlightAttitude */ |
FlightAttitudeEstimation(); |
/* Set Nominal Value */ |
RemoteControl(); |
/* PID Control */ |
PD_Regler(); |
/* Send Motor Data */ |
SendMotorData(); |
/* TestRemote */ |
TestRemote(); |
/* Test IC2- / RC-Link */ |
TestIC2Link(); |
} |
/* Send Debug Data over RS232 */ |
SendDebugData(); |
/* Check the Batery for Undervoltage */ |
TestBattery(); |
/* DeployRescue */ |
//DeployRescue(); |
} |
} |
/* **************************************************************************** |
Functionname: DeployRescue */ /*! |
Description: Deploy a rescue parachute using a servo |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void DeployRescue() |
{ |
#if 0 |
/* Yaw or pitch are greater than 60 Deg abs */ |
if (((abs(status.iTheta10) > 600) || (abs(status.iPhi10) > 600)) && |
((abs(AverageRoll_X) > Threshhold) || |
(abs(AverageRoll_Y) > Threshhold) || |
(abs(AverageRoll_Z) > Threshhold) )) |
{ |
MotorenEin = 0; |
Delay_ms(1000); |
ReleaseServo(); |
} |
#endif |
} |
/* **************************************************************************** |
Functionname: ReadParameterSet */ /*! |
Description: -- Parametersatz aus EEPROM lesen --- |
number [0..5] |
81,11 → 216,11 |
**************************************************************************** */ |
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if (number > 5) |
{ |
number = 5; |
} |
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
if (number > 5) |
{ |
number = 5; |
} |
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
} |
/* **************************************************************************** |
100,15 → 235,14 |
**************************************************************************** */ |
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if(number > 5) |
{ |
number = 5; |
} |
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
if(number > 5) |
{ |
number = 5; |
} |
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
} |
/* **************************************************************************** |
Functionname: GetActiveParamSetNumber */ /*! |
Description: |
120,20 → 254,43 |
**************************************************************************** */ |
unsigned char GetActiveParamSetNumber(void) |
{ |
unsigned char set; |
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
if(set > 5) |
{ |
set = 2; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
} |
return(set); |
unsigned char set; |
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
if(set > 5) |
{ |
set = 2; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
} |
return(set); |
} |
/* **************************************************************************** |
Functionname: GetAirPressureOffset */ /*! |
Description: |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void GetAirPressureOffset(void) |
{ |
unsigned int timer; |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
printf("\n\rAbgleich Luftdrucksensor.."); |
timer = SetDelay(1000); |
SucheLuftruckOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
} |
/* **************************************************************************** |
Functionname: main */ /*! |
Description: Hauptprogramm |
Functionname: GenerateDefaults */ /*! |
Description: Generate the Default Paramter |
@return void |
@pre - |
140,184 → 297,213 |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
int main (void) |
void GenerateDefaults() |
{ |
unsigned int timer; |
//unsigned int timer2 = 0; |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // verzögern |
DDRC = 0x81; // SCL |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
DDRD |=0x80; // J7 |
PORTD = 0xF7; // LED |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
beeptime = 2000; |
StickGier = 0; StickRoll = 0; StickNick = 0; PPM_in[K_GAS] = 0; |
Kafi_Init(); |
Timer_Init(); |
UART_Init(); |
InitGPS(); |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
/* Init the MM3 */ |
MM3_Init(); |
sei(); |
VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; |
#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
{ |
printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); |
DefaultKonstanten1(); |
for (unsigned char i=0;i<6;i++) |
{ |
if(i==2) DefaultKonstanten2(); // Kamera |
if(i==3) DefaultKonstanten3(); // Beginner |
if(i>3) DefaultKonstanten2(); // Kamera |
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
} |
} |
VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; |
GRN_ON; |
#define EE_DATENREVISION 66 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
{ |
printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); |
DefaultKonstanten1(); |
for (unsigned char i=0;i<6;i++) |
{ |
if(i==2) DefaultKonstanten2(); // Kamera |
if(i==3) DefaultKonstanten3(); // Beginner |
if(i>3) DefaultKonstanten2(); // Kamera |
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 3); // default-Setting |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
} |
/* Init EE_Parameter */ |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
printf("\n\rAbgleich Luftdrucksensor.."); |
timer = SetDelay(1000); |
SucheLuftruckOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
ROT_ON |
Delay_ms(1000); |
SetNeutral(); |
ROT_OFF |
Delay_ms(1000); |
SetNeutral(); |
ROT_ON |
beeptime = 2000; |
DebugIn.Analog[1] = 1000; |
DebugIn.Digital[0] = 0x55; |
/* Calibrate the MM3 Compass? */ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && |
(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) && |
(MotorenEin == 0)) |
{ |
printf("\n\rCalibrating Compass"); |
MM3_Calibrate(); |
} |
/* Init the OSD */ |
// InitOSD(); |
/* **************************************************************************** |
Functionname: InitPorts */ /*! |
Description: Init the IO Ports |
I2CTimeout = 5000; |
while (1) |
{ |
static i32_t OldTime = 0; |
if (UpdateMotor) // ReglerIntervall |
{ |
UpdateMotor=0; |
/* G e n e r a t e O S D D a t a */ |
static char CntOSD = 0; |
if (CntOSD % 6 == 1) |
{ |
//SendOSD(); |
} |
CntOSD++; |
/* Set the cycle Time to 110ms */ |
if (OldTime != 0) |
{ |
#ifdef USE_Extended_MM3_Measurement_Model |
while (((Count8Khz - OldTime) *10) / 8 < 160); |
#else |
while (((Count8Khz - OldTime) *10) / 8 < 110); |
#endif |
DebugOut.Analog[7] = ((Count8Khz - OldTime) *10) / 8; |
} |
OldTime = Count8Khz; |
/* Average the Measurements */ |
CalculateAverage(); |
/* Do the Kalman Filterimg */ |
AttitudeEstimation(); |
/* MotorControl */ |
MotorControl(); |
/* Call the PID Control */ |
PID_Regler(); |
SendMotorData(); |
if(PcZugriff) |
{ |
PcZugriff--; |
} |
if(SenderOkay) |
{ |
SenderOkay--; |
} |
if(!I2CTimeout) |
{ |
I2CTimeout = 5; |
i2c_reset(); |
if((BeepMuster == 0xffff) && MotorenEin) |
{ |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} |
else |
{ |
I2CTimeout--; |
} |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void InitPorts() |
{ |
unsigned int timer = 0; |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // verzögern |
DDRC = 0x81; // SCL |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
DDRD |=0x80; // J7 |
PORTD = 0xF7; // LED |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
} |
/* **************************************************************************** |
Functionname: TestIC2Link */ /*! |
Description: Test IC2- / RC-Link |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void TestIC2Link() |
{ |
if(PcZugriff) |
{ |
PcZugriff--; |
} |
if(SenderOkay) |
{ |
SenderOkay--; |
} |
if(!I2CTimeout) |
{ |
I2CTimeout = 5; |
i2c_reset(); |
if((BeepMuster == 0xffff) && MotorenEin) |
{ |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} |
else |
{ |
I2CTimeout--; |
} |
} |
/* **************************************************************************** |
Functionname: SendDebugData */ /*! |
Description: Send Debug Data over RS232 |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void SendDebugData() |
{ |
if(SIO_DEBUG) |
{ |
DatenUebertragung(); |
BearbeiteRxDaten(); |
} |
else |
{ |
BearbeiteRxDaten(); |
} |
} |
/* **************************************************************************** |
Functionname: TestBattery */ /*! |
Description: Check the Battery Voltage |
@return void |
@pre - |
@post - |
@author H. Buss / I. Busker |
**************************************************************************** */ |
void TestBattery() |
{ |
unsigned int timer = 0; |
if(CheckDelay(timer)) |
{ /* Voltage is Below Threshhod ? */ |
if(UBat < EE_Parameter.UnterspannungsWarnung) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
timer = SetDelay(100); |
} |
DebugOut.Analog[15] = UBat; |
} |
/* **************************************************************************** |
Functionname: TestRemote */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void TestRemote() |
{ |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
static unsigned int TimeSinceRC_BelowThreshhold = 0; |
static unsigned int TimeSinceRC_AboveThreshhold = 0; |
if (MotorenEin == 1) |
{ |
if (RemoteLinkLost == 0) |
{ |
if (RCQuality < 60) |
{ |
if (TimeSinceRC_BelowThreshhold < 10000) |
{ |
TimeSinceRC_BelowThreshhold++; |
} |
if(SIO_DEBUG) |
{ |
DatenUebertragung(); |
BearbeiteRxDaten(); |
} |
else BearbeiteRxDaten(); |
if(CheckDelay(timer)) |
{ |
if(UBat < EE_Parameter.UnterspannungsWarnung) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
timer = SetDelay(100); |
} |
} |
return (1); |
} |
else |
{ |
TimeSinceRC_BelowThreshhold = 0; |
} |
if ((TimeSinceRC_BelowThreshhold > 500) || /* aprox. 5 seconds */ |
(SenderOkay < 100)) |
{ |
RemoteLinkLost = 1; |
TimeSinceRC_AboveThreshhold = 0; |
} |
} |
else |
{ |
if (RCQuality > 80) |
{ |
if (TimeSinceRC_AboveThreshhold < 10000) |
{ |
TimeSinceRC_AboveThreshhold++; |
} |
} |
else |
{ |
TimeSinceRC_AboveThreshhold = 0; |
} |
if (TimeSinceRC_AboveThreshhold > 100) /* aprox. 1 seconds */ |
{ |
RemoteLinkLost = 0; |
TimeSinceRC_BelowThreshhold = 0; |
} |
} |
} |
else |
{ |
RemoteLinkLost = 0; |
TimeSinceRC_BelowThreshhold = 0; |
TimeSinceRC_AboveThreshhold = 0; |
} |
/*DebugOut.Analog[14] = TimeSinceRC_BelowThreshhold;*/ |
/*DebugOut.Analog[15] = TimeSinceRC_AboveThreshhold;*/ |
} |
/branches/KalmanFilter MikeW/main.h |
---|
84,7 → 84,6 |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#endif //_MAIN_H |
/branches/KalmanFilter MikeW/makefile |
---|
68,8 → 68,8 |
########################################################################################################## |
# 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 |
SRC = main.c uart.c printf_P.c timer0.c analog.c Bob4_OSD.c FlightControl.c |
SRC += twimaster.c rc.c gps.c matmatrix.c mymath.c mm3.c kafi.c |
########################################################################################################## |
/branches/KalmanFilter MikeW/matmatrix.c |
---|
1,13 → 1,378 |
/* matmatrix.c |
The following functions are based on the Numerical Recipes. |
*/ |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "mat.h" |
#include <math.h> |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include "matmatrix.h" |
/***************************************************************************** |
FUNCTIONS |
*****************************************************************************/ |
/* **************************************************************************** |
Functionname: matrix_sub */ /*! |
Description: C = A - B |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_sub( f32_t **a, f32_t **b, f32_t **c, int m, int n ) |
{ |
int i,j; |
f32_t *a_ptr, *b_ptr, *c_ptr; |
for( j = 1; j <= m; j++) |
{ |
a_ptr = &a[j][1]; |
b_ptr = &b[j][1]; |
c_ptr = &c[j][1]; |
for( i = 1; i <= n; i++) |
{ |
*c_ptr++ = *a_ptr++ - *b_ptr++; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_add */ /*! |
Description: C = A + B |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_add( f32_t **a, f32_t **b, f32_t **c, int m, int n ) |
{ |
int i,j; |
f32_t *a_ptr, *b_ptr, *c_ptr; |
for( j = 1; j <= m; j++) |
{ |
a_ptr = &a[j][1]; |
b_ptr = &b[j][1]; |
c_ptr = &c[j][1]; |
for( i = 1; i <= n; i++) |
{ |
*c_ptr++ = *a_ptr++ + *b_ptr++; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_mult */ /*! |
Description: C = A x B |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_mult( f32_t **a, f32_t **b, f32_t **c, |
int a_rows, int a_cols, int b_cols ) |
{ |
int i, j, k; |
f32_t *a_ptr; |
f32_t temp; |
for( i = 1; i <= a_rows; i++) |
{ |
a_ptr = &a[i][0]; |
for( j = 1; j <= b_cols; j++ ) |
{ |
temp = 0.0; |
for( k = 1; k <= a_cols; k++ ) |
{ |
temp = temp + (a_ptr[k] * b[k][j]); |
} |
c[i][j] = temp; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_mult_transpose */ /*! |
Description: C = A x B^T |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_mult_transpose( f32_t **a, f32_t **b, f32_t **c, |
int a_rows, int a_cols, int b_cols ) |
{ |
int i, j, k; |
f32_t *a_ptr, *b_ptr; |
f32_t temp; |
for( i = 1; i <= a_rows; i++) |
{ |
a_ptr = &a[i][0]; |
for( j = 1; j <= b_cols; j++ ) |
{ |
b_ptr = &b[j][1]; |
temp = (f32_t)0; |
for( k = 1; k <= a_cols; k++ ) |
{ |
temp += a_ptr[ k ] * *b_ptr++; |
} |
c[i][j] = temp; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_mult_transpose */ /*! |
Description: C = A^T x B |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C, |
int a_rows, int a_cols, int b_cols ) |
{ |
int i, j, k; |
f32_t temp; |
for( i = 1; i <= a_rows; i++) |
{ |
for( j = 1; j <= b_cols; j++ ) |
{ |
temp = 0.0; |
for( k = 1; k <= a_cols; k++ ) |
{ |
temp += A[ k ][ i ] * B[ k ][ j ]; |
} |
C[ i ][ j ] = temp; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_copy */ /*! |
Description: B = A |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void matrix_copy( f32_t **src, f32_t **dst, int num_rows, int num_cols ) |
{ |
int i, j; |
for( i = 1; i <= num_rows; i++ ) |
{ |
for( j = 1; j <= num_cols; j++ ) |
{ |
dst[ i ][ j ] = src[ i ][ j ]; |
} |
} |
} |
/* **************************************************************************** |
Functionname: matrix_alloc */ /*! |
Description: B = A |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
f32_t **matrix(long row_low, long row_high, long col_low, long col_high) |
{ |
long i, NumOfRows=row_high-row_low+1,NumOfCols=col_high-col_low+1; |
f32_t **m; |
/* allocate pointers to rows */ |
m=(f32_t **)malloc((size_t)((NumOfRows+1) |
*sizeof(f32_t *))); |
m += 1; |
m -= row_low; |
/* allocate rows and set pointers to them */ |
m[row_low]=(f32_t *)malloc((size_t)((NumOfRows*NumOfCols+1) |
*sizeof(f32_t))); |
m[row_low] += 1; |
m[row_low] -= col_low; |
for(i=row_low+1;i<=row_high;i++) m[i]=m[i-1]+NumOfCols; |
{ /* init values with 0.0F */ |
int x, y; |
for(y = 1; y <= NumOfRows; y++) |
{ |
for(x = 1; x <= NumOfCols; x++) |
{ |
m[y][x] = 0.F; |
} |
} |
} |
/* return pointer to array of pointers to rows */ |
return m; |
} |
/* **************************************************************************** |
Functionname: free_matrix */ /*! |
Description: |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void free_matrix(f32_t **m, long row_low, long row_high, long col_low, long col_high) |
/* free a matrix allocated by matrix() */ |
{ |
free((char*) (m[row_low]+col_low-1)); |
free((char*) (m+row_low-1)); |
} |
/* **************************************************************************** |
Functionname: take_inverse */ /*! |
Description: B = A^-1 |
@param[in] |
@return void |
@pre - |
@post - |
@author Michael Walter |
**************************************************************************** */ |
void take_inverse( f32_t **A, f32_t **B, int size) |
{ |
gaussj( A, size, B); |
matrix_copy( A, B, size, size); |
} |
/* gaussj() |
This function performs gauss-jordan elimination to solve a set |
of linear equations, at the same time generating the inverse of |
the A matrix. |
(C) Copr. 1986-92 Numerical Recipes Software `2$m.1.9-153. |
*/ |
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;} |
void gaussj(f32_t **a, int n, f32_t **b) |
{ |
int i,icol,irow,j,k,l,ll; |
f32_t big,dum,pivinv,temp; |
int indxc[5]; |
int indxr[5]; |
int ipiv[5]; |
irow = 0; |
icol = 0; |
for (j=1;j<=n;j++) |
{ |
ipiv[j]=0; |
} |
for (i=1;i<=n;i++) |
{ |
big=0.0; |
for (j=1;j<=n;j++) |
{ |
if (ipiv[j] != 1) |
{ |
for (k=1;k<=n;k++) |
{ |
if (ipiv[k] == 0) |
{ |
if (fabs(a[j][k]) >= big) |
{ |
big=fabs(a[j][k]); |
irow=j; |
icol=k; |
} |
} |
} |
} |
} |
++(ipiv[icol]); |
if (irow != icol) |
{ |
for (l=1;l<=n;l++) |
{ |
SWAP(a[irow][l],a[icol][l]) |
} |
} |
indxr[i]=irow; |
indxc[i]=icol; |
pivinv=1.0/a[icol][icol]; |
a[icol][icol]=1.0; |
for (l=1;l<=n;l++) |
{ |
a[icol][l] *= pivinv; |
} |
for (ll=1;ll<=n;ll++) |
{ |
if (ll != icol) |
{ |
dum=a[ll][icol]; |
a[ll][icol]=0.0; |
for (l=1;l<=n;l++) |
{ |
a[ll][l] -= a[icol][l]*dum; |
} |
} |
} |
} |
for (l=n;l>=1;l--) |
{ |
if (indxr[l] != indxc[l]) |
{ |
for (k=1;k<=n;k++) |
{ |
SWAP(a[k][indxr[l]],a[k][indxc[l]]); |
} |
} |
} |
} |
#undef SWAP |
/branches/KalmanFilter MikeW/matmatrix.h |
---|
0,0 → 1,56 |
#ifndef __MATMATH_H__ |
#define __MATMATH_H__ |
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
#include "math.h" |
#include "float.h" |
/***************************************************************************** |
TYPEDEFS |
*****************************************************************************/ |
#define f32_t float |
#define ui32_t unsigned int |
#define i32_t int |
#define PI 3.1415926535897932384626433832795 |
#define matSetFull(matM,Row,Column,X) matM[Row+1][Column+1] = (X) |
#define matSetDiagonal(matM,Row,Column,X) matM[Row+1][Column+1] = (X) |
#define matGetFull(matM,Row,Column,X) *(X) = matM[Row+1][Column+1] |
#define matGetDiagonal(matM,Row,Column,X) *(X) = matM[Row+1][Column+1] |
/* B = A */ |
extern void matrix_copy( f32_t **A, f32_t **B, int rows, |
int cols ); |
/* C = A + B */ |
extern void matrix_add( f32_t **A, f32_t **B, f32_t **C, int m, int n ); |
/* C = A - B */ |
extern void matrix_sub( f32_t **A, f32_t **B, f32_t **C, int m, int n ); |
/* C = A x B , A(a_rows x a_cols), B(a_cols x b_cols) */ |
extern void matrix_mult( f32_t **A, f32_t **B, f32_t **C, |
int a_rows, int a_cols, int b_cols ); |
/* C = A x B^T, A(a_rows x a_cols), B(b_cols x a_cols) */ |
extern void matrix_mult_transpose( f32_t **A, f32_t **B, f32_t **C, |
int a_rows, int a_cols, int b_cols ); |
/* C = A^T x B, A(a_cols x a_rows), B(a_cols x b_cols) */ |
extern void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C, |
int a_rows, int a_cols, int b_cols ); |
/* B = A^-1 */ |
extern void take_inverse( f32_t **A, f32_t **B, int size); |
extern void gaussj( f32_t **A, int n, f32_t **B); |
/* Matrix allocation routines */ |
extern f32_t **matrix(long row_low, long row_high, long column_low, long column_high); |
/* Deallocation routines */ |
extern void free_matrix(f32_t **m, long row_low, long row_high, long column_low, long column_high); |
#endif |
/branches/KalmanFilter MikeW/menu.h |
---|
1,15 → 1,22 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 2008 Michael Walter |
// + only for non-profit use |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/* |
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 void LcdClear(void); |
extern void OSDClear(void); |
extern unsigned char DispPtr; |
extern unsigned char OSDPtr; |
extern char DisplayBuff[80]; |
extern unsigned char DispPtr; |
extern char OSDBuff[80]; |
extern unsigned char OSDPtr; |
extern unsigned char RemoteTasten; |
/branches/KalmanFilter MikeW/mm3.c |
---|
39,32 → 39,32 |
typedef struct |
{ |
uint8_t STATE; |
uint16_t DRDY; |
uint8_t AXIS; |
int16_t x_axis; |
int16_t y_axis; |
int16_t z_axis; |
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_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_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_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 |
78,7 → 78,7 |
/***************************************************/ |
uint8_t GetParamByte(uint8_t param_id) |
{ |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
86,7 → 86,7 |
/***************************************************/ |
void SetParamByte(uint8_t param_id, uint8_t value) |
{ |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
94,7 → 94,7 |
/***************************************************/ |
uint16_t GetParamWord(uint8_t param_id) |
{ |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
102,7 → 102,7 |
/***************************************************/ |
void SetParamWord(uint8_t param_id, uint16_t value) |
{ |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/*********************************************/ |
110,47 → 110,47 |
/*********************************************/ |
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; |
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; |
} |
/*********************************************/ |
158,50 → 158,50 |
/*********************************************/ |
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; |
} |
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; |
} |
} |
/*********************************************/ |
209,67 → 209,67 |
/*********************************************/ |
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; |
} |
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; |
} |
} |
279,72 → 279,72 |
/*********************************************/ |
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); |
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); |
} |
353,80 → 353,86 |
/*********************************************/ |
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 |
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; |
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; |
/* 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); |
// 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); |
#if 0 |
DebugOut.Analog[3] = Hx; |
DebugOut.Analog[4] = Hy; |
DebugOut.Analog[5] = Hz; |
#endif |
// 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; |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
} |
else // MM3_Timeout = 0 i.e now new data from external board |
{ |
// if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
heading = -1; |
} |
return heading; |
} |
/branches/KalmanFilter MikeW/mm3.h |
---|
5,12 → 5,12 |
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; |
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; |
/branches/KalmanFilter MikeW/mymath.c |
---|
9,35 → 9,35 |
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); |
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)); |
return (c_sin_8192(90 - angle)); |
} |
// Arcustangens returns degree in a range of +/. 180 deg |
45,35 → 45,35 |
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 |
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 |
81,39 → 81,39 |
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])); |
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; |
uint32_t s1, s2; |
uint8_t iter = 0; |
// initialization of iteration |
s2 = number; |
do // iterative formula to solve x^2 - n = 0 |
{ |
s1 = s2; |
s2 = number / s1; |
s2 += s1; |
s2 /= 2; |
iter++; |
//if(iter > 40) break; |
}while( ( (s1-s2) > 1) && (iter < 40)); |
return s2; |
} |
/branches/KalmanFilter MikeW/old_macros.h |
---|
1,7 → 1,7 |
/* |
For backwards compatibility only. |
Ingo Busker ingo@mikrocontroller.com |
For backwards compatibility only. |
Ingo Busker ingo@mikrocontroller.com |
*/ |
#ifndef cbi |
/branches/KalmanFilter MikeW/printf_P.c |
---|
97,15 → 97,15 |
{ |
if(PrintZiel == OUT_LCD) |
{ |
DisplayBuff[DispPtr++] = zeichen; return(1); |
DisplayBuff[DispPtr++] = zeichen; return(1); |
} |
else if (PrintZiel == OUT_OSD) |
{ |
OSDBuff[OSDPtr++] = zeichen; return(1); |
OSDBuff[OSDPtr++] = zeichen; return(1); |
} |
else |
{ |
return(uart_putchar(zeichen)); |
return(uart_putchar(zeichen)); |
} |
} |
152,7 → 152,7 |
void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */ |
{ |
va_list ap; |
va_list ap; |
register const char *fmt; /* format string */ |
register char ch; /* character from fmt */ |
register int n; /* handy integer (short term usage) */ |
/branches/KalmanFilter MikeW/printf_P.h |
---|
10,7 → 10,6 |
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);} |
/branches/KalmanFilter MikeW/rc.c |
---|
11,9 → 11,9 |
#include "rc.h" |
#include "main.h" |
volatile int PPM_in[11]; |
int PPM_in[11] = {0,0,0,0,0,0,0,0,0,0,0}; |
int RCQuality = 0; |
volatile unsigned char NewPpmData = 1; |
unsigned char NewPpmData = 1; |
//############################################################################ |
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input |
21,9 → 21,9 |
void rc_sum_init (void) |
//############################################################################ |
{ |
TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64 |
TIMSK1 |= _BV(ICIE1); |
return; |
TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64 |
TIMSK1 |= _BV(ICIE1); |
return; |
} |
//############################################################################ |
31,61 → 31,63 |
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)) |
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) |
{ |
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 see |
http://forum.mikrokopter.de/topic-post44807.html#post44807*/ |
if (abs(2 * (signal - PPM_org[index]) > (int) RC_Quality)) |
{ |
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 |
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; |
DebugOut.Analog[12] = RCQuality; |
index++; |
if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen |
if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10; // Servosignal an J4 anlegen |
if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen |
} |
} |
} |
/branches/KalmanFilter MikeW/rc.h |
---|
12,14 → 12,14 |
#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 |
#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_in[11]; |
extern int PPM_diff[11]; // das diffenzierte Stick-Signal |
volatile extern unsigned char NewPpmData; |
extern unsigned char NewPpmData; |
#endif //_RC_H |
/branches/KalmanFilter MikeW/timer0.c |
---|
10,82 → 10,82 |
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 |
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--) |
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) |
{ |
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) |
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) |
{ |
PORTC |= (1<<7); // Speaker an PORTC.7 |
pieper_ein = 1; |
} |
else |
{ |
PORTC &= ~(1<<7); |
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; |
} |
#if 0 |
if(PINC & 0x10) |
{ |
cntKompass++; |
} |
else |
{ |
if((cntKompass) && (cntKompass < 4000)) |
{ |
if(cntKompass < 10) |
{ |
cntKompass = 10; |
} |
KompassValue = (((int) cntKompass-10) * 36) / 35; |
} |
cntKompass = 0; |
} |
cntKompass = 0; |
} |
#endif |
} |
92,20 → 92,20 |
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; |
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; |
} |
// ----------------------------------------------------------------------- |
112,32 → 112,32 |
unsigned int SetDelay (unsigned int t) |
{ |
// TIMSK0 &= ~_BV(TOIE0); |
// TIMSK0 &= ~_BV(TOIE0); |
return(CountMilliseconds + t + 1); |
// TIMSK0 |= _BV(TOIE0); |
// TIMSK0 |= _BV(TOIE0); |
} |
// ----------------------------------------------------------------------- |
char CheckDelay(unsigned int t) |
{ |
// TIMSK0 &= ~_BV(TOIE0); |
// TIMSK0 &= ~_BV(TOIE0); |
return(((t - CountMilliseconds) & 0x8000) >> 9); |
// TIMSK0 |= _BV(TOIE0); |
// TIMSK0 |= _BV(TOIE0); |
} |
// ----------------------------------------------------------------------- |
void Delay_ms(unsigned int w) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while (!CheckDelay(akt)); |
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; |
unsigned int akt; |
akt = SetDelay(w); |
while (!CheckDelay(akt)) ANALOG_ON; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
147,23 → 147,23 |
{ |
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; |
} |
{ |
TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3; |
ServoValue = Parameter_ServoNickControl; |
if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (0)) / 512; |
else ServoValue -= ((long) EE_Parameter.ServoNickComp * (0)) / 512; |
if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin; |
else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax; |
OCR2A = ServoValue;// + 75; |
timer = EE_Parameter.ServoNickRefresh; |
} |
else |
{ |
TCCR2A =3; |
PORTD&=~0x80; |
} |
} |
/branches/KalmanFilter MikeW/timer0.h |
---|
1,4 → 1,3 |
#define TIMER_TEILER CK8 |
#define TIMER_RELOAD_VALUE 250 |
/branches/KalmanFilter MikeW/twimaster.c |
---|
18,8 → 18,8 |
void i2c_init(void) |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
//############################################################################ |
27,8 → 27,8 |
char i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
return(0); |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
return(0); |
} |
//############################################################################ |
36,25 → 36,25 |
void i2c_stop(void) |
//############################################################################ |
{ |
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
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); |
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); |
} |
//############################################################################ |
62,11 → 62,11 |
char i2c_write_byte(char byte) |
//############################################################################ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
return(0); |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
return(0); |
} |
//############################################################################ |
74,354 → 74,353 |
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; |
#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: |
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; |
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_stop(); |
if (motor<4) twi_state = 0; |
else motor = 0; |
i2c_start(); |
break; |
//Liest Daten von Motor |
i2c_write_byte(Motor_Rechts); |
break; |
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; |
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; |
TWCR |= 0x80; |
} |
/branches/KalmanFilter MikeW/twimaster.h |
---|
24,9 → 24,9 |
extern unsigned char motor_rx[8]; |
void i2c_reset(void); |
extern void i2c_init (void); // I2C initialisieren |
extern void i2c_init (void); // I2C initialisieren |
extern char i2c_start (void); // Start I2C |
extern void i2c_stop (void); // Stop I2C |
extern void i2c_stop (void); // Stop I2C |
extern char i2c_write_byte (char byte); // 1 Byte schreiben |
extern void i2c_reset(void); |
/branches/KalmanFilter MikeW/uart.c |
---|
37,18 → 37,18 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
SIGNAL(INT_VEC_TX) |
{ |
static unsigned int ptr = 0; |
unsigned char tmp_tx; |
if(!UebertragungAbgeschlossen) |
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++; // die [0] wurde schon gesendet |
tmp_tx = SendeBuffer[ptr]; |
if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF)) |
{ |
ptr = 0; |
UebertragungAbgeschlossen = 1; |
ptr = 0; |
UebertragungAbgeschlossen = 1; |
} |
UDR = tmp_tx; |
UDR = tmp_tx; |
} |
else ptr = 0; |
} |
58,54 → 58,54 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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) |
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 |
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 |
} |
NeuerDatensatzEmpfangen = 1; |
AnzahlEmpfangsBytes = buf_ptr; |
RxdBuffer[buf_ptr] = '\r'; |
if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando |
} |
} |
else |
switch(UartState) |
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; |
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; |
} |
} |
137,7 → 137,7 |
SendeBuffer[pt++] = '#'; // Startzeichen |
SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
SendeBuffer[pt++] = cmd; // Commando |
SendeBuffer[pt++] = cmd; // Commando |
while(len) |
{ |
180,53 → 180,55 |
// -------------------------------------------------------------------------- |
void BearbeiteRxDaten(void) |
{ |
if(!NeuerDatensatzEmpfangen) return; |
unsigned char tmp_char_arr2[2] ={0,0}; |
PcZugriff = 255; |
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; |
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 |
beeptime = 10000; |
BeepMuster = 0x0080; |
break; |
} |
NeuerDatensatzEmpfangen = 0; |
NeuerDatensatzEmpfangen = 0; |
} |
235,13 → 237,13 |
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); |
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); |
} |
// -------------------------------------------------------------------------- |
257,22 → 259,22 |
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); |
//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); |
} |
279,25 → 281,25 |
//--------------------------------------------------------------------------------------------- |
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; |
} |
if(!UebertragungAbgeschlossen) return; |
if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
{ |
SendOutData('G',MeineSlaveAdresse,(unsigned char *) &DebugIn,sizeof(DebugIn)); |
DebugGetAnforderung = 0; |
} |
if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen) |
{ |
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut)); |
DebugDataAnforderung = 0; |
Debug_Timer = SetDelay(2 * MIN_DEBUG_INTERVALL); |
} |
if(GetVersionAnforderung && UebertragungAbgeschlossen) |
{ |
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo)); |
GetVersionAnforderung = 0; |
} |
} |
/branches/KalmanFilter MikeW/uart.h |
---|
45,15 → 45,15 |
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 |
//#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__) |
76,20 → 76,20 |
//#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 |
#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 |
/branches/KalmanFilter MikeW/version.txt |
---|
1,3 → 1,5 |
------- |
V0.67 26.04.2008 M.Walter |
- erste öffentliche Version |
V0.68 16.09.2008 M.Walter |
- added Kafi Library |