| /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 |