| /branches/dongfang_FC_rewrite/.settings/org.eclipse.core.resources.prefs |
|---|
| 0,0 → 1,3 |
| #Fri Apr 30 22:47:51 CEST 2010 |
| eclipse.preferences.version=1 |
| encoding/<project>=ISO-8859-1 |
| /branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h |
|---|
| 1,26 → 1,28 |
| /* |
| #ifndef _ADXRS610_H |
| #define _ADXRS610_H |
| #include "sensors.h" |
| /* |
| / * |
| * Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board. |
| */ |
| * / |
| #define GYRO_HW_NAME "ADXR" |
| #define GYRO_HW_FACTOR 1.2288f |
| /* |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| */ |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 1.0f |
| /* |
| / * |
| * Same for yaw. |
| */ |
| * / |
| #define GYRO_YAW_CORRECTION 1.0f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
|---|
| 25,20 → 25,20 |
| for (axis=PITCH; axis<=YAW; axis++) { |
| if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW; |
| else factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
| if(rawGyroSum[axis] < factor * 510) DACValues[axis]--; |
| else if(rawGyroSum[axis] > limit * 515) DACValues[axis]++; |
| else numberOfAxesInRange++; |
| else factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
| if(rawGyroSum[axis] < 510 * factor) DACValues[axis]--; |
| else if(rawGyroSum[axis] > 515 * factor) DACValues[axis]++; |
| else numberOfAxesInRange++; |
| /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). |
| if(DACValues[axis] < 10) { |
| DACValues[axis] = 10; |
| } else if(DACValues[axis] > 245) { |
| DACValues[axis] = 245; |
| } |
| /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
| if(DACValues[axis] < 10) { |
| DACValues[axis] = 10; |
| } else if(DACValues[axis] > 245) { |
| DACValues[axis] = 245; |
| } |
| } |
| I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
| // Wait for I2C to finish transmission. |
| 46,12 → 46,12 |
| // Did it take too long? |
| if(CheckDelay(timeout)) { |
| printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
| break; |
| break; |
| } |
| } |
| analog_start(); |
| Delay_ms_Mess(i<10 ? 10 : 2); |
| } |
| Delay_ms_Mess(70); |
| 59,9 → 59,9 |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 3; |
| staticParams.DriftComp = 100; |
| staticParams.GyroAccFactor = 1; |
| staticParams.GyroAccTrim = 5; |
| staticParams.DriftComp = 250; |
| staticParams.GyroAccFactor = 10; |
| staticParams.GyroAccTrim = 1; |
| // Not used. |
| staticParams.AngleTurnOverPitch = 85; |
| /branches/dongfang_FC_rewrite/ENC-03_FC1.3.h |
|---|
| 1,24 → 1,26 |
| /* |
| #ifndef _ENC03_FC13_H |
| #define _ENC03_FC13_H |
| #include "sensors.h" |
| /* |
| / * |
| * Configuration for the ENC-03 gyros and oriented and wired on FC 1.3 (with DAC). |
| */ |
| * / |
| #define GYRO_HW_NAME "ENC" |
| #define GYRO_HW_FACTOR 1.304f |
| /* |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| */ |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 1.11f |
| /* |
| / * |
| * Same for yaw. |
| */ |
| * / |
| #define GYRO_YAW_CORRECTION 1.28f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/Flight-Ctrl_MEGA644p_NAVICTRL__ADXRS610_FC2.0_V0_74df_SVN0001.hex |
|---|
| 0,0 → 1,2052 |
| :100000000C943B040C9456040C9456040C94560423 |
| :100010000C9456040C9456040C9456040C945604F8 |
| :100020000C9456040C946F110C9456040C945604C2 |
| :100030000C942B220C9456040C9456040C945604E5 |
| :100040000C9456040C9456040C9493100C9456047F |
| :100050000C94DB060C9456040C94A2060C945604E3 |
| :100060000C94C5110C9456040C94D71F0C94560490 |
| :100070000C94AF350C9456040C9456040A0D3D3D77 |
| :100080003D3D3D3D3D3D3D3D3D3D3D3D3D3D3D3DA0 |
| :100090003D3D3D3D3D3D3D3D3D3D3D3D3D3D3D3D90 |
| :1000A0003D000A0D466C69676874436F6E74726F29 |
| :1000B0006C000A0D48617264776172653A2043757D |
| :1000C00073746F6D000D0A20202020204350553A94 |
| :1000D0002041746D6567613634340070000A0D5339 |
| :1000E0006F6674776172653A205625642E25642503 |
| :1000F00063000A0D3D3D3D3D3D3D3D3D3D3D3D3DAA |
| :100100003D3D3D3D3D3D3D3D3D3D3D3D3D3D3D3D1F |
| :100110003D3D3D3D3D3D3D000A0D3D3D3D3D3D3DAF |
| :100120003D3D3D3D3D3D3D3D3D3D3D3D3D3D3D3DFF |
| :100130003D3D3D3D3D3D3D3D3D3D3D3D3D000A0D8F |
| :10014000537570706F727420666F72204E6176699D |
| :100150004374726C000A0D436F6E74726F6C3A20B8 |
| :100160000048656164696E67486F6C64004E657530 |
| :100170007472616C20284143432D4D6F64652900E2 |
| :100180000A0A0D00416E676C65506974636820202F |
| :1001900020202020416E676C65526F6C6C202020FF |
| :1001A00020202020416E676C655961772020202037 |
| :1001B000202020204779726F506974636828504965 |
| :1001C000442920204779726F526F6C6C2850494443 |
| :1001D000292020204779726F596177202020202024 |
| :1001E000202020204779726F50697463682841434A |
| :1001F000292020204779726F526F6C6C2841432967 |
| :10020000202020204779726F5961772841432920A7 |
| :100210002020202041636350697463682028616E48 |
| :10022000676C6529416363526F6C6C2028616E674F |
| :100230006C65292055426174202020202020202038 |
| :10024000202020205069746368205465726D20203E |
| :1002500020202020526F6C6C205465726D2020206D |
| :1002600020202020596177205465726D20202020A5 |
| :10027000202020205468726F74746C65205465725D |
| :100280006D202020307468204F20436F72722070E0 |
| :1002900069746368307468204F20436F72722072F3 |
| :1002A0006F6C6C204472696674436F6D7044656C4A |
| :1002B000746120504472696674436F6D7044656C5C |
| :1002C00074612052414450697463684779726F4F7A |
| :1002D000666673204144526F6C6C4779726F4F664B |
| :1002E000667320204D312020202020202020202037 |
| :1002F000202020204D3220202020202020202020BF |
| :10030000202020204D3320202020202020202020AD |
| :10031000202020204D34202020202020202020209C |
| :1003200020202020436F6E74726F6C5961772020FB |
| :1003300020202020416363205A20202020202020DC |
| :10034000202020204472696674436F6D7050697478 |
| :10035000636820204472696674436F6D70526F6CDD |
| :100360006C2020205069746368204E6F697365208B |
| :1003700020202020526F6C6C204E6F697365202006 |
| :1003800020202020020100070603020105020100CF |
| :100390000706030201045B25695D005B25695D00BA |
| :1003A0002B204D696B726F4B6F70746572202B0040 |
| :1003B00048573A5625642E25642053573A25642E13 |
| :1003C000256425630053657474696E673A2025645B |
| :1003D00020257300493243204572726F722121211A |
| :1003E000004D697373696E6720424C2D4374726CC3 |
| :1003F0003A25640028632920486F6C6765722042A3 |
| :10040000757373004174746974756465004E696333 |
| :100410006B3A20202020202025356900526F6C6C1B |
| :100420003A20202020202025356900486561646934 |
| :100430006E673A2020202535690043313A2534691A |
| :10044000202043323A253469200043333A25346969 |
| :10045000202043343A253469200043353A25346955 |
| :10046000202043363A253469200043373A25346941 |
| :10047000202043383A25346920004E693A253469F2 |
| :100480002020526F3A253469200047733A25346999 |
| :10049000202059613A253469200050313A253469C9 |
| :1004A000202050323A253469200050333A253469EF |
| :1004B000202050343A2534692000566F6C746167EF |
| :1004C000653A20202533692E253169560052432D87 |
| :1004D0004C6576656C3A2025356900436F6D706117 |
| :1004E00073732020202020202000436F75727365D5 |
| :1004F0003A202020202535690048656164696E67CF |
| :100500003A202020253569004F6666436F75727367 |
| :10051000653A2025356900506F313A20253369202E |
| :10052000506F353A2025336900506F323A20253319 |
| :100530006920506F363A2025336900506F333A20D6 |
| :1005400025336920506F373A2025336900506F34C6 |
| :100550003A2025336920506F383A20253369004509 |
| :10056000787465726E436F6E74726F6C2020004EEB |
| :10057000693A2534692020526F3A253469200047B2 |
| :10058000733A253469202059613A2534692000489E |
| :10059000693A253469202043663A253469200042AF |
| :1005A0004C2D4374726C204572726F727320002060 |
| :1005B0002533642020253364202025336420202522 |
| :1005C0003364200020253364202025336420202537 |
| :1005D0003364202025336420002025336420202527 |
| :1005E0003364202025336420202533642000424CCE |
| :1005F0002D4374726C20666F756E64200020256335 |
| :100600002020202563202020256320202025632012 |
| :100610000020256320202025632020202563202022 |
| :1006200020256320002025632020202D2020202D40 |
| :100630002020202D200031300031310031320000E7 |
| :100640000047008F00D7001E016601AF01F701408F |
| :10065000028902D2021C036703B203FD034A049716 |
| :1006600004E40433058205D30524067706CB06206F |
| :10067000077607CE0727088208DE083D099D09FF97 |
| :1006800009640ACB0A340BA00B0F0C800CF50C6D1F |
| :100690000DE90D680EEC0E730F001092102811C5B5 |
| :1006A0001168121113C2137B143C150616DA16B921 |
| :1006B00017A3189B19A11AB61BDD1C171E671FCEA6 |
| :1006C000205022F023B2259A27AE29F62B782E3E11 |
| :1006D000315534CC37B63B2C404E45464B5052BE7C |
| :1006E0005A0565D971FF7FFF7FFF7FFF7FFF7FFF87 |
| :1006F0007FFF7FFF7F000047008F00D6001E01654F |
| :1007000001AC01F3013A028102C7020E03540399BE |
| :1007100003DF0324046904AE04F20436057905BC42 |
| :1007200005FE0540068206C306040744078307C288 |
| :100730000700083E087B08B708F2082D096809A1E0 |
| :1007400009DA09120A490A7F0AB50AE90A1D0B509B |
| :100750000B820BB40BE40B130C420C6F0C9C0CC7FC |
| :100760000CF20C1B0D440D6B0D920DB70DDB0DFE45 |
| :100770000D210E420E610E800E9E0EBA0ED60EF0A8 |
| :100780000E090F210F380F4D0F610F740F860F9751 |
| :100790000FA60FB50FC20FCE0FD80FE10FEA0FF063 |
| :1007A0000FF60FFA0FFE0FFF0F00100A0D466F75C0 |
| :1007B0006E6420424C2D4374726C3A2000256420F4 |
| :1007C000000A0D0A0D2121204D495353494E47205F |
| :1007D000424C2D4354524C3A202564202121000ADA |
| :1007E0000D496E697420506172616D657465722087 |
| :1007F000696E20454550524F4D000A0D5573696E84 |
| :100800006720506172616D65746572205365742054 |
| :100810002564000A0D47656E65726174696E672014 |
| :1008200064656661756C74204D69786572205461E9 |
| :10083000626C65000A0D4D697865722D436F6E66B6 |
| :1008400069673A202725732720282575204D6F7466 |
| :100850006F727329000A0D3D3D3D3D3D3D3D3D3DDF |
| :100860003D3D3D3D3D3D3D3D3D3D3D3D3D3D3D3DB8 |
| :100870003D3D3D3D3D0011241FBECFEFD0E1DEBF29 |
| :10088000CDBF12E0A0E0B1E0E6E0FFE702C00590D6 |
| :100890000D92A831B107D9F716E0A8E1B2E001C086 |
| :1008A0001D92A737B107E1F70C9458040C9400008F |
| :1008B000CFEFD0E1DEBFCDBFF8940E941A3A80930B |
| :1008C000CF020E94273A282F8093DF0184B7877FC9 |
| :1008D00084BF809160008861809360001092600006 |
| :1008E0002A3009F47DC1243109F47AC1289A0E9482 |
| :1008F0004C1C0E9465100E9437110E941A068091BC |
| :10090000CF02813009F469C10E94F7210E94791158 |
| :100910000E947B1F0E94663B78948CE790E09F9337 |
| :100920008F931F920E94D00D0F900F900F9082EA2C |
| :1009300090E09F938F931F920E94D00D0F900F9085 |
| :100940000F9082EB90E09F938F931F920E94D00DA7 |
| :100950000F900F900F9085EC90E09F938F931F92D4 |
| :100960000E94D00D8091CF020F900F900F90813098 |
| :1009700009F428C185EC90E09F938F938AE490E07E |
| :100980009F938F931F921F928DED90E09F938F9373 |
| :100990001F920E94D00D8DB79EB709960FB6F8949E |
| :1009A0009EBF0FBE8DBF82EF90E09F938F931F92EB |
| :1009B0000E94D00D0E94813488EC90E00E94FC10CF |
| :1009C000EC01429A2091DF010F900F900F902C3094 |
| :1009D00008F0F4C029982A3009F4F2C0243109F44F |
| :1009E000EFC02898CE010E9405118823D9F388EC26 |
| :1009F00090E00E94FC10EC014298439A2091DF01A4 |
| :100A00002A3009F4F1C0243109F4EEC0289A2C30C0 |
| :100A100008F0E8C0299ACE010E9405118823D9F375 |
| :100A200088EC90E00E94FC10EC01CE010E940511C0 |
| :100A30008823D9F343980E94672188E191E09F932E |
| :100A40008F931F920E94D00D0F900F900F908EE306 |
| :100A500091E09F938F931F920E94D00D0E94DA1C09 |
| :100A60000E943C260E9467110E94F22C80ED97E0C4 |
| :100A70000E94573A0F900F900F9085E591E09F9359 |
| :100A80008F931F920E94D00D809165050F900F905B |
| :100A90000F9082FFCCC081E691E09F938F931F92CD |
| :100AA0000E94D00D0F900F900F9080E891E09F93DF |
| :100AB0008F931F920E94D00D0E94301788E893E117 |
| :100AC00090936C0180936B010F900F900F90809129 |
| :100AD0004402882321F080910701882371F48091DA |
| :100AE0003F028823A1F70E941B3E80914402882385 |
| :100AF000B1F380910701882391F3109244025C9A2C |
| :100B00000E943D2D5C982091DF012A3009F471C0CC |
| :100B1000243109F46EC0289A80916B0190916C0188 |
| :100B2000019790936C0180936B0180916B01909180 |
| :100B30006C01892B29F080917B02882309F46CC019 |
| :100B40002A3009F458C0243109F455C028988091FE |
| :100B50006B0190916C01892B09F455C0809144027E |
| :100B6000882309F443C08091680280FF3FC00E943F |
| :100B7000F108CE010E9405118823E9F08091080157 |
| :100B8000909109011816190664F480917605282FB2 |
| :100B900033278091080190910901821793070CF483 |
| :100BA00043C00E94DD3D84E080933F0284E190E0F9 |
| :100BB0000E94FC10EC010E94CB1C91CF299A0BCF14 |
| :100BC000289A10CF8BED90E09F938F931F920E94F5 |
| :100BD000D00D0F900F900F90CDCE0E944B3594CE3C |
| :100BE000289885CE299817CF289811CF0E940C0BF2 |
| :100BF000BECF289891CF289A80916B0190916C017B |
| :100C0000892B09F0ABCF0E94B71F85E090E090934D |
| :100C10006C0180936B01A2CF2A3021F0243111F0B6 |
| :100C2000289A9CCF28989ACF0E949F3ABACF8DE6F7 |
| :100C300091E033CFCF93CFB72AE230E0F8948091A0 |
| :100C4000C1008F778093C1008091C1008F7B80931A |
| :100C5000C100589A5098599A519A832F9927809396 |
| :100C6000C5002093C4008091C00082608093C000C2 |
| :100C700088E18093C1008091C2008F778093C20089 |
| :100C80008091C2008F7B8093C2008091C2008F7DD3 |
| :100C90008093C2008091C2008F7E8093C2008091B9 |
| :100CA000C200877F8093C2008091C1008B7F8093B8 |
| :100CB000C1008091C20084608093C2008091C20014 |
| :100CC00082608093C2008091C00087FF0CC0809139 |
| :100CD000C6008091C000882334F48091C6008091C2 |
| :100CE000C0008823A4F38091C10080688093C10074 |
| :100CF0008091C10080648093C10080910001909137 |
| :100D000001010E94FC109093720480937104109270 |
| :100D10002F0210922D0210922C0210922B0291E0C1 |
| :100D20009093020110928F038AE48093900384E6EB |
| :100D3000809393038AE08093910390939203CFBFB3 |
| :100D4000CF9108951F920F920FB60F9211248F9397 |
| :100D50009F93EF93FF93809102018823A9F48091E0 |
| :100D60003A0290913B020196FC01E552FC4FE08172 |
| :100D7000ED3079F08639910561F090933B028093D4 |
| :100D80003A02E093C6000EC010923B0210923A0263 |
| :100D900009C010923B0210923A0281E08093020156 |
| :100DA000E093C600FF91EF919F918F910F900FBE3E |
| :100DB0000F901F9018951F920F920FB60F9211244B |
| :100DC0002F933F934F935F938F939F93AF93BF93D3 |
| :100DD000CF93DF93EF93FF935091C60080912F0242 |
| :100DE000882309F073C040913E024423B9F4533282 |
| :100DF000E1F05D3011F1E42FFF27E751FD4F508303 |
| :100E00004F5F40933E0280913C0290913D02850FDE |
| :100E1000911D90933D0280933C0258C0463948F39F |
| :100E200080933E0280932F0251C05093E90281E0EB |
| :100E300080933E0283E290E0ECCFA42FBB27A75122 |
| :100E4000BD4FFD013297808120913C0230913D02DF |
| :100E5000281B3109ED0121978881281B3109C9011F |
| :100E60009F7090933D0280933C020024880F991F4D |
| :100E7000001C880F991F001C892F902D982F935CC0 |
| :100E80002F733070235C8081891729F010922F0214 |
| :100E900010923E021BC088818217C1F75C934F5F9E |
| :100EA00040932E0281E080932F028091EB028235E5 |
| :100EB00079F788E190E02CE00FB6F894A89580933C |
| :100EC00060000FBE2093600010923E02FF91EF91F0 |
| :100ED000DF91CF91BF91AF919F918F915F914F9192 |
| :100EE0003F912F910F900FBE0F901F901895AC015E |
| :100EF000A0E0B0E09D01A817B90748F4EBEDF3E0DE |
| :100F00008191280F311D1196A417B507C8F33F70C2 |
| :100F1000FD01E552FC4FC9010024880F991F001CF8 |
| :100F2000880F991F001C892F902D835C8083119658 |
| :100F3000FD01E552FC4F2F733070822F835C80835C |
| :100F4000A452BC4F8DE08C93109202018091DB0380 |
| :100F50008093C6000895BF92CF92DF92EF92FF92E6 |
| :100F60000F931F93CF93DF93CDB7DEB72C859D856D |
| :100F70007E85BB2483E28093DB039F599093DC033F |
| :100F80002093DD0303E010E0772309F4ADC0CF84A4 |
| :100F9000D88843E1E42EF12CEC0EFD1EA989BA8914 |
| :100FA0007150109709F4A0C0109709F43CC0F601E5 |
| :100FB000EB0DF11DB3949081119789F47723A1F182 |
| :100FC000F70122E030E0E20EF31ED701C080D180AD |
| :100FD000E20EF31E0D90BC91A02DBB247150109712 |
| :100FE00019F1F601EB0DF11DB3944081119709F44D |
| :100FF00066C01097D1F0F601EB0DF11DB39460813E |
| :101000001197A1F4772391F0F7013296D701CD9093 |
| :10101000DC9032E0E32EF12CEE0EFF1EA081B181B8 |
| :10102000BB24715003C090E040E060E0F801E5525D |
| :10103000FC4F892F86958695835C80830F5F1F4FB9 |
| :10104000F801E552FC4F892F9927837090708295A3 |
| :101050009295907F9827807F982755279A013295FF |
| :1010600022952F7023273F702327822B835C808358 |
| :101070000F5F1F4FF801E552FC4F4F705070440F47 |
| :10108000551F440F551F862F99270024880F991F3D |
| :10109000001C880F991F001C892F902D842B835CC6 |
| :1010A00080830F5F1F4FF801E552FC4F6F73635C45 |
| :1010B00060830F5F1F4F109709F076CF15C077231D |
| :1010C00009F4B3CFF701A2E0B0E0EA0EFB1ED701AE |
| :1010D000C080D18022E030E0E20EF31E0D90BC9182 |
| :1010E000A02DBB24715085CFC8010E947707DF91E6 |
| :1010F000CF911F910F91FF90EF90DF90CF90BF9015 |
| :101100000895A3E07A2F40912E02465009F45FC063 |
| :10111000E72FFF27E751FD4F80818D537F5FE72F3A |
| :10112000FF27E751FD4F20812D537F5FE72FFF27DA |
| :10113000E751FD4F30813D537F5FE72FFF27E75198 |
| :10114000FD4F60816D537F5F9927880F991F880F2E |
| :10115000991F522F52955F70582B822F99278F70AD |
| :10116000907082959295907F9827807F9827232F63 |
| :1011700026952695282B832F99278370907000241D |
| :10118000969587950794969587950794982F802D27 |
| :10119000682B41504F3FD9F0EA2FFF27E751FD4F11 |
| :1011A0005083AF5F41504F3F91F0EA2FFF27E75147 |
| :1011B000FD4F2083AF5F41504F3F49F0EA2FFF279B |
| :1011C000E751FD4F6083AF5F442309F0A1CF8CEE60 |
| :1011D00092E090932D0280932C02A350A0932B02B7 |
| :1011E0000895CF93DF93CDB7DEB722970FB6F8946B |
| :1011F000DEBF0FBECDBF80912A02882319F0815037 |
| :1012000080932A0280912F02882309F4E9C10E9469 |
| :1012100081088091EA02823609F44FC08091EB0286 |
| :10122000992787369105E9F188369105D4F48236FD |
| :10123000910509F4FCC0833691050CF09EC08136FF |
| :10124000910591F5E0912C02F0912D02E081E032C0 |
| :1012500008F48EC18FE1809303018FEF8093B60471 |
| :1012600023C08C36910509F4D4C08D3691050CF05D |
| :10127000A0C088369105C1F48FEF8093B604E09149 |
| :101280002C02F0912D02808190916502892B809330 |
| :101290006502882311F01092300281E080933702BA |
| :1012A00003C081E08093380210922D0210922C022C |
| :1012B00010922B0210922F0293C18091EB0299277A |
| :1012C0008037910509F4C3C0813791050CF47DC0C6 |
| :1012D0008337910509F4F1C0843791050CF0D3C030 |
| :1012E0008137910509F09ACFE0912C02F0912D02FF |
| :1012F00080818F3F09F466C18081882309F03BC15A |
| :1013000081E08083E0912C02F0912D0280810E9487 |
| :10131000CD33E0912C02F0912D02808189838BE402 |
| :101320008A83809102018823E1F38DE590E09F9309 |
| :101330008F938DE595E09F938F9321E030E03F936D |
| :101340002F93CE0102969F938F933F932F93CE01BD |
| :10135000820F931F9F938F9383E08F9381E08F93EE |
| :1013600081E58F930E94AB07EDB7FEB73F960FB6AE |
| :10137000F894FEBF0FBEEDBF51CF8336910509F43F |
| :101380006AC08436910509F08FCFE0912C02F0916C |
| :101390002D0290818AE0989FC00111249093010151 |
| :1013A00080930001892B09F47FCF81E0809335027F |
| :1013B0007BCF8637910509F462C08837910509F023 |
| :1013C00073CF81E0809331026FCF8D36910509F4A0 |
| :1013D000AFC08E36910509F021CF8091020188239C |
| :1013E000E1F38DE490E09F938F938AEB95E09F93D8 |
| :1013F0008F9381E08F938F938EE48F930E94AB073E |
| :101400008DB79EB707960FB6F8949EBF0FBE8DBFDF |
| :1014100005CF8FEF8093B604E0912C02F0912D025E |
| :1014200080818093660281E0809336023DCFA09157 |
| :101430002C02B0912D028BE0EBEAF4E00D900192CA |
| :101440008A95E1F78091B4048093730406CF81E01C |
| :1014500080933302E3CEE0912C02F0912D02908133 |
| :101460008AE0989FC001112490931902809318027A |
| :10147000892B09F419CF81E08093340215CF81E0E4 |
| :101480008093390211CF8437910509F0C7CE80913E |
| :101490002B02853108F475C0E0912C02F0912D02E9 |
| :1014A00080E1AAE1B2E001900D928A95E1F78FEF19 |
| :1014B00080932A028093B604B1CE8091680280FDA9 |
| :1014C000ADCE20912C0230912D02D9018C91882330 |
| :1014D00039F08C91863020F4F90181818B3491F0C0 |
| :1014E0001982809102018823E1F381E090E09F93CB |
| :1014F0008F93CE0101969F938F9381E08F938F936B |
| :1015000083E57BCF2E5F3F4F8DE5EDE5F5E0D9011B |
| :101510000D9001928A95E1F722503040F9018081C7 |
| :101520000E94ED330E94143489830E945C3AD9CF23 |
| :10153000A0912C02B0912D028C91813099F11982E9 |
| :1015400081E090E09F938F93CE0101969F938F93BC |
| :1015500081E08F938F938DE48F930E94AB07ADB79B |
| :10156000BEB717960FB6F894BEBF0FBEADBF56CE2E |
| :10157000E093030172CE8081863008F4C7CE85E007 |
| :10158000C0CEE0912C02F0912D0280819181A28148 |
| :10159000B38180931A0290931B02A0931C02B09314 |
| :1015A0001D0285CF8DE4EAEBF5E00D9001928A955E |
| :1015B000E1F70E943834809102018823E1F381E051 |
| :1015C0008983BECF82E090E00E94B033E0912C028C |
| :1015D000F0912D028083E0912C02F0912D028CCEAF |
| :1015E00022960FB6F894DEBF0FBECDBFDF91CF912C |
| :1015F0000895CF93C82F8A3029F08091C00085FFCD |
| :10160000FCCF04C08DE00E94F90AF7CFC093C6005A |
| :1016100080E090E0CF910895CF93DF93CDB7DEB710 |
| :1016200060970FB6F894DEBF0FBECDBF8091020168 |
| :10163000882309F443C280913902882309F099C0B4 |
| :1016400080913702882329F080910201882309F0D4 |
| :1016500052C180913602882329F0809102018823AB |
| :1016600009F01EC1809103018F3FA1F19927829556 |
| :101670009295907F9827807F98278C579E4F40E1C6 |
| :1016800050E0BC01CE0101960E94B23E80E190E0A4 |
| :101690009F938F93CE0101969F938F9381E090E06B |
| :1016A0009F938F9383E091E09F938F9382E08F933A |
| :1016B00081E08F9381E48F930E94AB078FEF80933B |
| :1016C00003012DB73EB7255F3F4F0FB6F8943EBFDD |
| :1016D0000FBE2DBF80917304882329F080910201F1 |
| :1016E000882309F0C3C08091000190910101892BEA |
| :1016F00009F03FC180913502882329F080910201D1 |
| :10170000882309F0B1C18091180290911902892BA8 |
| :1017100009F025C180913402882329F080910201CB |
| :10172000882309F030C180913802882329F0809104 |
| :101730000201882309F07FC080913202882329F0BA |
| :1017400080910201882309F069C0809133028823C7 |
| :1017500029F080910201882309F046C0809131026E |
| :10176000882309F4ABC180910201882309F4A6C142 |
| :1017700020C080910201882309F462CF8AE090E0C2 |
| :101780009F938F938FE893E09F938F9381E08F9344 |
| :101790008F9386E58F930E94AB07109239022DB785 |
| :1017A0003EB7295F3F4F0FB6F8943EBF0FBE2DBF27 |
| :1017B00047CF80E190E09F938F9380ED92E09F93DD |
| :1017C0008F9381E08F938F9388E58F930E94AB076F |
| :1017D000109231022DB73EB7295F3F4F0FB6F894F4 |
| :1017E0003EBF0FBE2DBF6AC184E190E09F938F93EF |
| :1017F0008BE095E09F938F9381E08F938F9380E5AB |
| :101800008F930E94AB07109233028DB79EB7079655 |
| :101810000FB6F8949EBF0FBE8DBFA0CF1F9281E080 |
| :101820008F9384E58F930E94AB07109232020F9042 |
| :101830000F900F908ACF8BE090E09F938F938BEA6D |
| :1018400094E09F938F9381E08F938F9387E48F939E |
| :101850000E94AB07109238022DB73EB7295F3F4F69 |
| :101860000FB6F8943EBF0FBE2DBF66CF81E090E06B |
| :101870009F938F9383E794E09F938F9381E08F935F |
| :101880008F9382E48F930E94AB07109273048DB7FD |
| :101890009EB707960FB6F8949EBF0FBE8DBF23CF9D |
| :1018A0000E94391780E590E09F938F938AE191E041 |
| :1018B0009F938F9321E030E03F932F938AE691E04E |
| :1018C0009F938F933F932F9386E692E09F938F93FE |
| :1018D00083E08F9381E08F938CE48F930E94AB071A |
| :1018E000109236028DB79EB70F960FB6F8949EBF32 |
| :1018F0000FBE8DBFB7CE0E94391784E190E09F9351 |
| :101900008F939091300284E1989FC0011124865EEC |
| :101910009E4F9F938F9381E090E09F938F9380E3FE |
| :1019200092E09F938F9382E08F9381E08F9388E47E |
| :101930008F930E94AB07809130028F5F2DB73EB727 |
| :10194000255F3F4F0FB6F8943EBF0FBE2DBF8430CA |
| :1019500008F4AFC010923002109237027ACE809114 |
| :101960008D0390918E030E940511882309F0D6CE35 |
| :10197000D1CE80917104909172040E940511882348 |
| :1019800009F0BCCEB7CE8EE090E09F938F938FE7A7 |
| :1019900093E09F938F9381E08F938F9383E48F9352 |
| :1019A0000E94AB078091410590914205A0914305AB |
| :1019B000B0914405BC01CD0129E830E040E050E0A1 |
| :1019C0000E94403F3093800320937F038091450520 |
| :1019D00090914605A0914705B0914805BC01CD0105 |
| :1019E00029E830E040E050E00E94403F309382031D |
| :1019F0002093810320913D0530913E0540913F05A4 |
| :101A000050914005DA01C90163E0880F991FAA1FB0 |
| :101A1000BB1F6A95D1F7820F931FA41FB51F820FBA |
| :101A2000931FA41FB51FBC01CD012FEA34E040E095 |
| :101A300050E00E94403F30938403209383038091C1 |
| :101A40001802909119020E94FC1090938E038093CB |
| :101A50008D03109234028DB79EB707960FB6F89497 |
| :101A60009EBF0FBE8DBF5FCE82E490E09F938F93A9 |
| :101A700089E993E09F938F9381E08F938F9384E420 |
| :101A80008F930E94AB0780910001909101010E9409 |
| :101A9000FC109093720480937104109235022DB75C |
| :101AA0003EB7295F3F4F0FB6F8943EBF0FBE2DBF24 |
| :101AB0002ACE8093300210923702CBCD60960FB6BB |
| :101AC000F894DEBF0FBECDBFDF91CF910895982F60 |
| :101AD00080917404813031F0892F0E94F90A282FF7 |
| :101AE00033270CC080916402E82FFF27E65EFE4F8B |
| :101AF00090838F5F8093640221E030E0C9010895F4 |
| :101B00000F931F93CF93DF938C01EB01672B71F041 |
| :101B1000F80181918F010E94670D219739F0F8013A |
| :101B200081918F010E94670D219791F7DF91CF91ED |
| :101B30001F910F9108950F931F93CF93DF938C0103 |
| :101B4000EB01672B81F0F8010F5F1F4F84910E941A |
| :101B5000670D219741F0F8010F5F1F4F84910E949C |
| :101B6000670D219781F7DF91CF911F910F91089514 |
| :101B7000CF93C82F181634F480E20E94670DC1502D |
| :101B80001C16D4F3CF910895CF93C82F181634F4B0 |
| :101B900080E30E94670DC1501C16D4F3CF910895C5 |
| :101BA0002F923F924F925F926F927F928F929F926D |
| :101BB000AF92BF92CF92DF92EF92FF920F931F935B |
| :101BC000CF93DF93CDB7DEB7E0970FB6F894DEBFC3 |
| :101BD0000FBECDBF26968FAD26972896EEADFFADF2 |
| :101BE00028978824992454018093740448E4C42ECF |
| :101BF000D12CCC0EDD1E7F01C701F70114911123FA |
| :101C000031F0153221F00894E11CF11CF6CFB70138 |
| :101C1000681B790B09F095C0112309F43DC20894A3 |
| :101C2000E11CF11C1FA63FA4232C0FEF39A6F701DE |
| :101C30000894E11CF11C14911537C9F1812F8062C1 |
| :101C40008837A9F1103209F440C0133209F48DC06D |
| :101C50001A3209F479C01D3209F476C01B32C9F179 |
| :101C60001E32C9F1103309F48AC0812F81538930A3 |
| :101C700008F07EC060E070E0CB01880F991F880FEC |
| :101C8000991F880F991F860F971F680F791F610F83 |
| :101C9000711D60537040F7010894E11CF11C149110 |
| :101CA000812F80538A3040F3262E153739F630FEC7 |
| :101CB0003DC0F60184E090E0C80ED91E808091807E |
| :101CC000A280B380103209F0C0CF89A5882309F023 |
| :101CD000AECF19A7ACCFF7010894E11CF11C149109 |
| :101CE0001A3209F451C060E070E014C0CB01880FD3 |
| :101CF000991F880F991F880F991F860F971F680FCC |
| :101D0000791F610F711D60537040F7010894E11C49 |
| :101D1000F11C1491812F80538A3040F3EFEF6F3F15 |
| :101D20007E0714F46FEF7FEF062F86CFF60182E077 |
| :101D300090E0C80ED91E808191814C01AA24BB2459 |
| :101D400081CF0E949B0D68CF1A3251F4F60182E0D8 |
| :101D500090E0C80ED91E208022200CF068CF21947C |
| :101D600090E1392AEFED3E2262CF98E0392A5FCF29 |
| :101D70001836C9F01C36D1F481E0382A58CF34FC2B |
| :101D800056CFF0E23F2A53CFF60182E090E0C80E32 |
| :101D9000D91E608171819FEF6F3F790714F46FEF57 |
| :101DA0007FEF062F44CF24E0322A41CF133609F4C7 |
| :101DB0004DC1143409F41FC1143609F41CC119367D |
| :101DC00009F419C11F3409F40FC11F3609F40CC1FD |
| :101DD000103709F4F9C0133709F4B2C0153509F406 |
| :101DE000ECC0153709F4E9C0183509F444C01837B8 |
| :101DF00009F441C0112309F44FC1CE0101969DA7FA |
| :101E00008CA7198381E0482E19A6632C7724042D12 |
| :101E10005FA4541857FC2DC029A5222331F10F5F70 |
| :101E2000050DC30180739070892B09F42FC1222303 |
| :101E300009F027C166FC1BC1C301807390708097B5 |
| :101E400009F410C1852D0E94C40D842D992787FDAA |
| :101E50009095BC018CA59DA50E94800D64FECCCE02 |
| :101E6000822D801B0E94B80DC7CE66FED9CF0E5FB3 |
| :101E7000D7CF5524D1CFE0E1EEA733FE07C08114C0 |
| :101E80009104A104B10411F0F0E43F2A19A60FA7B0 |
| :101E900007FD02C02FED3222CE0189969DA78CA7A7 |
| :101EA00081149104A104B10419F49FA5992361F14F |
| :101EB000EEA54E2E55246624772418AA8414950482 |
| :101EC000A604B70410F0F1E0F8ABC501B401A3011A |
| :101ED00092010E941E3FDC01CB01082F8A30F0F4F2 |
| :101EE000005DECA5FDA50293FDA7ECA7C501B4011B |
| :101EF000A30192010E941E3F49015A01F8A9FF2344 |
| :101F0000E1F62EA5283079F0632C7724CE010196D6 |
| :101F1000482EFCA54F1A28E2420E79CF095A1835EF |
| :101F200001F70F7DDECF632C772463FEEFCF003304 |
| :101F300069F380E3ECA5FDA58293FDA7ECA7E6CFAE |
| :101F4000F60182E090E0C80ED91E0190F081E02DEC |
| :101F5000FDA7ECA7EF2B81F4FE013196FDA7ECA7BE |
| :101F600088E289838EE6818385E78B838CE68C8388 |
| :101F70008D8389E28E831F8207FD15C0802F9927EC |
| :101F800087FD9095AC0160E070E08CA59DA50E9456 |
| :101F9000BF3E009731F0482EFCA54F1A04150CF0F7 |
| :101FA00033CF402E31CFECA5FDA501900020E9F7FD |
| :101FB00031974E2E2CA5421A27CF153511F481E00A |
| :101FC000382A9AE09EA762CFF60182E090E0C80E20 |
| :101FD000D91E808191814C01AA24BB2490E19EA747 |
| :101FE000E0E43E2A18E752CF1F3411F4E1E03E2A24 |
| :101FF000F8E0FEA74BCF143411F491E0392A30FEFB |
| :1020000018C0F60184E090E0C80ED91E808091804F |
| :10201000A280B380B7FE0AC0B094A094909480943C |
| :10202000811C911CA11CB11C8DE289A79AE09EA77E |
| :102030002ECFF60182E090E0C80ED91E80819181FA |
| :102040004C01AA2497FCA094BA2CE4CFFE0131964F |
| :10205000FDA7ECA7F60182E090E0C80ED91E8081B2 |
| :102060008983D0CE822D801B0E94C40DEBCE80E3ED |
| :102070008AA71BA762E070E0CE018A960E94800DBD |
| :10208000DBCE61E070E0CE018996F8CF822D801B17 |
| :102090000E94B80D29A5CBCEE0960FB6F894DEBF0E |
| :1020A0000FBECDBFDF91CF911F910F91FF90EF90A9 |
| :1020B000DF90CF90BF90AF909F908F907F906F9068 |
| :1020C0005F904F903F902F9008959FB7F894809124 |
| :1020D000DF018A3019F03F9A479802C0529A5A9805 |
| :1020E00084B1886184B985B1877E85B984B58F7ADA |
| :1020F00084BD84B5836A84BD85B5877385BD85B588 |
| :10210000887F826085BD17BC88E788BD16BC80913A |
| :102110006E00897F80936E0080916E008160809355 |
| :102120006E009FBF08951F920F920FB60F92112459 |
| :102130002F933F938F939F9380913F02882329F0A1 |
| :1021400080913F02815080933F02809147028150ED |
| :102150008F3F79F180934702809140029091410234 |
| :10216000892BD9F080914002909141020197909380 |
| :10217000410280934002809140029091410220915F |
| :1021800004013091050182239323892B61F0809112 |
| :10219000DF018A3039F1479A26C08FEF9FEF909385 |
| :1021A0000501809304018091DF018A30C9F04798CE |
| :1021B0001AC089E0809347028091060191E0892747 |
| :1021C00080930601882311F4909344028091450284 |
| :1021D0009091460201969093460280934502BCCFAF |
| :1021E0005A9801C05A9A9F918F913F912F910F90C9 |
| :1021F0000FBE0F901F901895209145023091460216 |
| :10220000280F391FC9010197089520914502309187 |
| :102210004602821B930B892F990F990B80780895A2 |
| :10222000CF93DF930E94FC10EC01CE010E940511B8 |
| :102230008823D9F3DF91CF910895CF93DF930E9444 |
| :10224000FC10EC010BC080910701882339F09093BA |
| :10225000070180917A00886C80937A00CE010E94F9 |
| :102260000511982F882379F3DF91CF9108959FB7B7 |
| :10227000F894579A5F983E9A469A8091B0008F7072 |
| :102280008093B0008091B00083608093B000809113 |
| :10229000B1008B738093B1008091B1008B6080930B |
| :1022A000B1001092B2008FEF8093B3008091B00024 |
| :1022B00080688093B000809170008A7F8093700066 |
| :1022C000809170008260809370009FBF089581E0CC |
| :1022D00080934802089510924802469A08951F92EA |
| :1022E0000F920FB60F9211240F900FBE0F901F90F8 |
| :1022F00018959FB7F89411B812B88FEF80937E00AD |
| :1023000080917C008F7180937C0080917C00807E26 |
| :10231000826080937C0087E080937A0080917B00CC |
| :10232000887F80937B0080917A00886C80937A000C |
| :102330009FBF08959C01FB018081918182179307C3 |
| :1023400044F0309521953F4F80819181821793070A |
| :102350001CF431832083089555278081918148178B |
| :10236000590738F480819181841B950B9183808378 |
| :10237000089511821082089537B520910A01329F85 |
| :1023800090011124820F931F08951F920F920FB690 |
| :102390000F921124AF92BF92CF92DF92EF92FF92F1 |
| :1023A0000F931F932F933F934F935F936F937F935D |
| :1023B0008F939F93AF93BF93CF93DF93EF93FF934D |
| :1023C00080911701E82FFF27EE0FFF1FEC5AFD4FFA |
| :1023D000808191812091780030917900820F931F44 |
| :1023E00091838083A09153028A2F9927AF5F8D300C |
| :1023F000910509F46DC08E3091050CF048C0A09392 |
| :1024000053028B30910509F474C18C3091050CF4A2 |
| :1024100059C180910B02882309F438C280910B01C5 |
| :1024200090910C012091620230916302821B930B08 |
| :102430009093760480937504809190049927019776 |
| :1024400020914F0230915002289FB001299F700DBA |
| :10245000389F700D1124409175045091760480913D |
| :102460009004282F3327CB01840F951FB9010E94B8 |
| :102470000B3F7093500260934F0241E066E874E0B6 |
| :1024800080917504909176040E949A1109C081315F |
| :1024900091050CF45DC0419709F401C2A093530269 |
| :1024A00020915302E22FFF27EC57FC4F9491909319 |
| :1024B000170180917C00807E892B80937C002223F1 |
| :1024C00009F438C380917A00886C80937A0032C313 |
| :1024D000A093530280910C02882309F0EAC08091F6 |
| :1024E00060029091610220910D0130910E01821BDA |
| :1024F000930B9093780480937704809190049927AC |
| :1025000001972091510230915202289FB001299FDA |
| :10251000700D389F700D112440917704509178040C |
| :1025200080919004282F3327CB01840F951FB90188 |
| :102530000E940B3F709352026093510241E068E8A1 |
| :1025400074E080917704909178040E949A11A8CF4A |
| :10255000A09353020F970CF0E4C080911801909162 |
| :102560001901009709F0E2C1C0915A02D0915B02B3 |
| :1025700021E0C039D20708F019C287B5E82FFF273C |
| :1025800080910A01282F33278EE696E08C1B9D0B45 |
| :10259000B9010E94F73EE61BF70B3197EB30F105CE |
| :1025A0000CF47EC287B599278E1B9F0B909319015F |
| :1025B00080931801E7BDCE010E94BC1190939A044C |
| :1025C0008093990480910A014AE0849F900111242C |
| :1025D0008091990490919A048217930708F0B7C1EB |
| :1025E00080910A01849FC00111245C01CC24B7FCB6 |
| :1025F000C094DC2C8091990490919A047C0100276E |
| :10260000112780910A01282F332744275527DA0103 |
| :10261000C901F3E0880F991FAA1FBB1FFA95D1F7D4 |
| :10262000820F931FA41FB51F820F931FA41FB51FF6 |
| :10263000E81AF90A0A0B1B0BC801B70129E130E0BF |
| :1026400040E050E00E94CC3EDC01CB01A80EB91E58 |
| :10265000CA1EDB1E8091950490919604A091970468 |
| :10266000B0919804A80EB91ECA1EDB1EA092950454 |
| :10267000B0929604C0929704D092980480918104FD |
| :102680008E5F80938104809181048E3009F4C6C1ED |
| :1026900080910801909109019093B2038093B10356 |
| :1026A0008091790490917A049093D2038093D1031E |
| :1026B000F7CE80910D0190910E0120916002309132 |
| :1026C000610215CF089709F0EBCE80910D028823A7 |
| :1026D00009F49BC180910F019091100120915E023D |
| :1026E00030915F02821B930B90937A04809379045C |
| :1026F000D7CE209154023091550230938F0420930D |
| :102700008E0480911002882309F4FBC0809115018A |
| :1027100090911601821B930B9093800480937F0409 |
| :10272000BFCEA051CA2EDD24D601AC0DBD1DA657CB |
| :10273000BB4FE2E0F0E0EC19FD09EE0FFF1FEC5A91 |
| :10274000FD4F8081918111969C938E930D90BC9149 |
| :10275000A02D8091650587FD47C1F601E25FFD4F21 |
| :102760008081882309F0D8C0F601EC0DFD1DEF5ED5 |
| :10277000FE4F80819181ED01C81BD90BCC0FDD1F6D |
| :10278000CC0FDD1F7601EC0CFD1C87010E551B4F95 |
| :1027900080919C0499270197D8012D913C91289F05 |
| :1027A000A001299F500D389F500D112480919C0449 |
| :1027B000282F3327CA018C0F9D1FB9010E940B3FA0 |
| :1027C000EB01C70185589B4F45E0BC01CE010E943B |
| :1027D0009A11F701EE57FB4F80919B0499270197BF |
| :1027E00020813181289FB001299F700D389F700D85 |
| :1027F0001124D8018D919C91AE01481B590B8091F9 |
| :102800009B04282F3327CB01840F951FB9010E9409 |
| :102810000B3F71836083F801D183C083F701E657D2 |
| :10282000FB4FA081B181F601E25FFD4F80818823DB |
| :1028300009F482C021E131E0E20EF31EF701C0810C |
| :10284000D181CA1BDB0BCC0FDD1FCC0FDD1FF601C6 |
| :10285000EC0DFD1DEF56FB4F8091A10499270197C8 |
| :1028600020813181289FA001299F500D389F500D54 |
| :1028700011248091A104282F3327CA018C0F9D1F9A |
| :10288000B9010E940B3F718360830ACE809162027E |
| :102890009091630220910B0130910C01C7CD809182 |
| :1028A0000801909109019C01220F331F280F391F45 |
| :1028B00080915C0290915D0263E070E00E94F73EBF |
| :1028C000260F371F369527953695279530930901A2 |
| :1028D0002093080181E08093070180914D0290913F |
| :1028E0004E02019690934E0280934D021092530235 |
| :1028F00087E0E4E5F2E011921192815087FFFBCF6F |
| :10290000CFCD8091150190911601281B390B309382 |
| :10291000800420937F04C4CDF601EC0DFD1DEF5E15 |
| :10292000FE4FC081D181CA1BDB0B28CF019790934A |
| :10293000190180931801B4CD21E131E0E20EF31EBC |
| :10294000F70180819181ED01C81BD90B7CCF80916B |
| :102950000A014AEF849F900111248091990490917B |
| :102960009A042817390708F0A9C080910A01849FAA |
| :10297000C00111245C01CC24B7FCC094DC2C8091F4 |
| :10298000990490919A047C010027112780910A01F3 |
| :102990009927AA27BB27BC01CD012AEF30E040E0F0 |
| :1029A00050E00E94CC3EDC01CB0142CE36E0CF3677 |
| :1029B000D30708F400CE27B580910A01482F552788 |
| :1029C000CE0180599140BA010E94F73EFB01E20F0F |
| :1029D000F11D3197EA3FF1050CF09FC087B5DF018B |
| :1029E000A81BB109CD01E2CDA032B1050CF081C028 |
| :1029F0008EE490E0A89FF001A99FF00DB89FF00D24 |
| :102A00001124DF01A45CB940A8CE80915E029091B0 |
| :102A10005F0220910F013091100164CE80919D04DE |
| :102A200090919E04A0919F04B091A0049C01AD01DF |
| :102A3000E3E0220F331F441F551FEA95D1F7281BEF |
| :102A4000390B4A0B5B0B8091950490919604A091F1 |
| :102A50009704B0919804280F391F4A1F5B1FDA01B1 |
| :102A6000C9010496A11DB11DB7FD5EC073E0B59507 |
| :102A7000A795979587957A95D1F780939D04909324 |
| :102A80009E04A0939F04B093A0041092950410920A |
| :102A90009604109297041092980410928104F8CD35 |
| :102AA00087B5882309F487CD87B5815087BD81E03C |
| :102AB00090E090931901809318017DCD8091990445 |
| :102AC00090919A04AA27BB272091950430919604EF |
| :102AD0004091970450919804820F931FA41FB51F33 |
| :102AE0008093950490939604A0939704B0939804D0 |
| :102AF000C5CDFFE0AD3DBF070CF42FCEAC5DBF40B0 |
| :102B00008EE490E0A89F9001A99F300DB89F300DF2 |
| :102B10001124D901A452B04F20CE87B58E3F08F0C2 |
| :102B20004ACD87B58F5FC2CFDA01C9010B96A11DCF |
| :102B3000B11D9CCFFF91EF91DF91CF91BF91AF91EC |
| :102B40009F918F917F916F915F914F913F912F91C5 |
| :102B50001F910F91FF90EF90DF90CF90BF90AF90BB |
| :102B60000F900FBE0F901F901895EF92FF920F934A |
| :102B70001F93CF93DF93CDB7DEB72C970FB6F894A2 |
| :102B8000DEBF0FBECDBF7E010894E11CF11C8CE0BE |
| :102B9000D7011D928A95E9F79091EE01892F8370F4 |
| :102BA0008F5F80939C04292F3327C9018C7090700C |
| :102BB00095958795959587958F5F8093A104C90119 |
| :102BC0008073907095958795959587959595879540 |
| :102BD000959587958F5F80939B040024220F331F68 |
| :102BE000001C220F331F001C232F302D822F8F5FDC |
| :102BF000809390040E94543B00E084E190E00E94A6 |
| :102C00001D11F70112E06AE874E0DB018D919D91DE |
| :102C1000BD019C01442737FD4095542F808191814F |
| :102C2000A281B381820F931FA41FB51F819391933B |
| :102C3000A193B193115017FFE8CF0F5F0032E8F274 |
| :102C4000B70112E083ECE82E83E0F82EE1E1F1E039 |
| :102C5000DB012D913D914D915D91BD01DA01C901DD |
| :102C60004096A11DB11DB7FD13C005E0B595A79510 |
| :102C7000979587950A95D1F791838083819191915A |
| :102C8000D7018D939D937D01115017FFE1CF06C0B1 |
| :102C9000DA01C9018F96A11DB11DE7CF10927E0404 |
| :102CA00010927D0480917D0490917E0490937C0429 |
| :102CB00080937B0484E090E00E94BF3390930C01EA |
| :102CC00080930B0186E090E00E94BF3390930E0149 |
| :102CD00080930D0188E090E00E94BF339093100133 |
| :102CE00080930F0184E690E00E941D112C960FB690 |
| :102CF000F894DEBF0FBECDBFDF91CF911F910F9132 |
| :102D0000FF90EF900895AF92BF92CF92DF92EF9233 |
| :102D1000FF920F931F93CF93DF93CDB7DEB72C971E |
| :102D20000FB6F894DEBF0FBECDBF7E010894E11C44 |
| :102D3000F11C8CE0D7011D928A95E9F700E08AE04A |
| :102D400090E00E941D11F70112E065E774E0DB01DD |
| :102D50008D919D91BD019C01442737FD4095542FD5 |
| :102D600080819181A281B381820F931FA41FB51F1F |
| :102D700081939193A193B193115017FFE8CF0F5F07 |
| :102D80000A30E8F27BE0A72E72E0B72E670112E06E |
| :102D90006BE0E62E61E0F62EF60181919191A19112 |
| :102DA000B1916F010596A11DB11DBC01CD012AE0B5 |
| :102DB00030E040E050E00E94403FA901D7012D9152 |
| :102DC0003C91F50181915F01882369F0C901841B61 |
| :102DD000950BD70111969C938E9312967D011150FD |
| :102DE00017FFDACF04C0C901840F951FF2CF60919D |
| :102DF0000B0170910C0184E090E00E94C633609159 |
| :102E00000D0170910E0186E090E00E94C633609142 |
| :102E10000F017091100188E090E00E94C63310927B |
| :102E20008904109288048091880490918904909379 |
| :102E300087048093860484E690E00E941D112C96FE |
| :102E40000FB6F894DEBF0FBECDBFDF91CF911F91BB |
| :102E50000F91FF90EF90DF90CF90BF90AF900895CB |
| :102E600090E2EAE1F1E08FE49193815087FFFCCF9B |
| :102E700008951F93CF93DF932091650220FF08C030 |
| :102E800080916602882309F4ECC081508093660229 |
| :102E900021FF0AC09091660280916A01981709F497 |
| :102EA000EFC19F5F90936602822F992780FF02C037 |
| :102EB00081FDD4C00E9430179091660280916A0112 |
| :102EC000891718F480936602982F9A30A0F581E153 |
| :102ED00080936402892F99279F938F9386E993E0CB |
| :102EE0009F938F9381E08F930E94D00D0F900F904E |
| :102EF0000F900F900F9090916602C92FDD27C73079 |
| :102F0000D10509F4C0C1C830D105FCF4C230D105E7 |
| :102F100009F4F6C1C330D1050CF4A8C0C330D10503 |
| :102F200009F445C3249709F4F2C3915090936A01C0 |
| :102F300010926602ABC480E180936402892F9927C6 |
| :102F40009F938F938BE993E0CBCFCB30D10509F4DE |
| :102F50003DC2CC30D1050CF0F8C0C830D10509F421 |
| :102F600093C3299711F7109264028091D80290912F |
| :102F7000D9029F938F938091D0029091D1029F9319 |
| :102F80008F9387E195E09F938F9311E01F930E94A9 |
| :102F9000D00D84E1809364022DB73EB7295F3F4F87 |
| :102FA0000FB6F8943EBF0FBE2DBF8091DA0290910C |
| :102FB000DB029F938F938091D2029091D3029F93D3 |
| :102FC0008F9389E295E09F938F931F930E94D00D7A |
| :102FD00088E2809364028DB79EB707960FB6F89487 |
| :102FE0009EBF0FBE8DBF8091DC029091DD029F934A |
| :102FF0008F938091D4029091D5029F938F938BE30E |
| :1030000095E09F938F931F930E94D00D8CE3809344 |
| :1030100064022DB73EB7295F3F4F0FB6F8943EBF0D |
| :103020000FBE2DBF8091DE029091DF029F938F93A0 |
| :103030008091D6029091D7029F938F938DE495E073 |
| :103040009F938F931F930E94D00D8DB79EB70796C5 |
| :103050000FB6F8949EBF0FBE8DBF18C410926602C3 |
| :1030600029CF80916A018093660212CF209709F0E0 |
| :103070005CCF1092640280EA93E09F938F9311E0FB |
| :103080001F930E94D00D84E1809364020F900F90F3 |
| :103090000F9085EC90E09F938F938AE490E09F934C |
| :1030A0008F93DF93CF938091DF016AE00E94EB3E24 |
| :1030B000892F99279F938F938091DF010E94EB3E88 |
| :1030C00099279F938F9380EB93E09F938F931F9308 |
| :1030D0000E94D00D88E2809364022DB73EB7235F33 |
| :1030E0003F4F0FB6F8943EBF0FBE2DBF8BEB95E060 |
| :1030F0009F938F930E94143499279F938F9385EC0D |
| :1031000093E09F938F931F930E94D00D80916B014A |
| :1031100090916C012DB73EB7295F3F4F0FB6F894E1 |
| :103120003EBF0FBE2DBF069708F4BCC190917B0235 |
| :10313000992309F489C38CE380936402892F99272A |
| :103140009F938F9381EE93E0D0C0CC30D10509F4EA |
| :10315000B2C12D9709F0E9CE109264028EEE95E08F |
| :103160009F938F93C1E0CF930E94D00D84E1809311 |
| :1031700064020F900F900F908091D70499278D963D |
| :103180009F938F938091D20499278D969F938F93CD |
| :103190008091CD0499278D969F938F938091C80439 |
| :1031A00099278D969F938F938DEF95E09F938F93A3 |
| :1031B000CF930E94D00D88E2809364022DB73EB772 |
| :1031C000255F3F4F0FB6F8943EBF0FBE2DBF8091D5 |
| :1031D000EB0499278D969F938F938091E60499270E |
| :1031E0008D969F938F938091E10499278D969F935D |
| :1031F0008F938091DC0499278D969F938F9381E123 |
| :1032000096E09F938F93CF930E94D00D8CE3809391 |
| :1032100064028DB79EB70B960FB6F8949EBF0FBE93 |
| :103220008DBF8091F00499278D969F938F9385E2AF |
| :1032300096E09F938F93CF930E94D00D0F900F90A5 |
| :103240000F900F900F908091F504882309F010C320 |
| :103250008091FA04882309F0FDC28091FF0488233D |
| :1032600009F414C388E4809364028CE396E09F938E |
| :103270008F93CF930E94D00D0F900F900F9006C3A5 |
| :103280001092660211CE84E180936402809108015D |
| :10329000909109016AE070E00E940B3F9F938F9329 |
| :1032A00080910801909109016AE070E00E940B3F53 |
| :1032B0007F936F938AEB94E09F938F9311E01F931A |
| :1032C0000E94D00D88E2809364028DB79EB7079666 |
| :1032D0000FB6F8949EBF0FBE8DBF80918302909170 |
| :1032E00084029F938F938DEC94E09F938F931F9311 |
| :1032F0000E94D00D0F900F900F900F900F90C6C2AC |
| :103300001092640284E094E09F938F9311E01F93E6 |
| :103310000E94D00D84E1809364020F900F900F9073 |
| :103320008091410590914205A0914305B0914405DB |
| :10333000BC01CD012AE535E040E050E00E94403F6D |
| :103340005F934F933F932F938DE094E09F938F93E0 |
| :103350001F930E94D00D88E2809364028DB79EB7C0 |
| :1033600007960FB6F8949EBF0FBE8DBF809145059E |
| :1033700090914605A0914705B0914805BC01CD014B |
| :103380002AE535E040E050E00E94403F5F934F93D4 |
| :103390003F932F938CE194E09F938F931F930E9410 |
| :1033A000D00D8CE3809364022DB73EB7295F3F4F69 |
| :1033B0000FB6F8943EBF0FBE2DBF80917301909160 |
| :1033C00074019F938F938BE294E08FCF10926402ED |
| :1033D0008FE595E09F938F9311E01F930E94D00D8E |
| :1033E00084E1809364020F900F900F908091AF045E |
| :1033F000992787FD90959F938F938091AE0499278D |
| :1034000087FD90959F938F938FE695E09F938F9381 |
| :103410001F930E94D00D88E2809364022DB73EB7BF |
| :10342000295F3F4F0FB6F8943EBF0FBE2DBF80916E |
| :10343000B004992787FD90959F938F938091B10455 |
| :1034400099279F938F938FE795E09F938F931F9377 |
| :103450000E94D00D8CE3809364028DB79EB70796CF |
| :103460000FB6F8949EBF0FBE8DBF8091B50499270B |
| :103470009F938F938091B204992787FD90959F9396 |
| :103480008F938FE895E09F938F931F930E94D00DA9 |
| :103490002DB73EB7295F3F4F0FB6F8943EBF0FBE22 |
| :1034A0002DBFF4C18CE38093640284ED93E09F937D |
| :1034B0008F931F93DFCE109264028FE995E09F9364 |
| :1034C0008F9311E01F930E94D00D84E180936402DA |
| :1034D0000F900F900F908091D80499279F938F930E |
| :1034E0008091D30499279F938F938091CE0499273D |
| :1034F0009F938F938091C90499279F938F938FEA0D |
| :1035000095E09F938F931F930E94D00D88E2809344 |
| :1035100064028DB79EB70B960FB6F8949EBF0FBE90 |
| :103520008DBF8091EC0499279F938F938091E7043E |
| :1035300099279F938F938091E20499279F938F936C |
| :103540008091DD0499279F938F9384EC95E09F935E |
| :103550008F931F930E94D00D8CE3809364022DB74C |
| :103560003EB7255F3F4F0FB6F8943EBF0FBE2DBF4D |
| :103570008091000599279F938F938091FB04992751 |
| :103580009F938F938091F60499279F938F938091B7 |
| :10359000F10499279F938F9389ED95E09F938F93E3 |
| :1035A0001F930E94D00D8DB79EB70B9651CD1092F0 |
| :1035B000640280910F05909110059F938F938091E5 |
| :1035C0000D0590910E059F938F938AE394E09F934E |
| :1035D0008F9311E01F930E94D00D84E180936402C9 |
| :1035E0008DB79EB707960FB6F8949EBF0FBE8DBFDE |
| :1035F00080911305909114059F938F9380911105ED |
| :10360000909112059F938F938AE494E09F938F93F8 |
| :103610001F930E94D00D88E2809364022DB73EB7BD |
| :10362000295F3F4F0FB6F8943EBF0FBE2DBF80916C |
| :103630001705909118059F938F9380911505909190 |
| :1036400016059F938F938AE594E09F938F931F9322 |
| :103650000E94D00D8CE3809364028DB79EB70796CD |
| :103660000FB6F8949EBF0FBE8DBF80911B05909141 |
| :103670001C059F938F938091190590911A059F9334 |
| :103680008F938AE694E0FFCE109264028BED94E073 |
| :103690009F938F9311E01F930E94D00D84E180933C |
| :1036A00064020F900F900F908091710190917201C0 |
| :1036B0009F938F938AEE94E09F938F931F930E9422 |
| :1036C000D00D88E2809364020F900F900F900F90BE |
| :1036D0000F9080917301909174019F938F9389EF64 |
| :1036E00094E09F938F931F930E94D00D8CE380935F |
| :1036F00064020F900F900F900F900F9080919C029A |
| :1037000090919D029F938F9388E095E0EECD10926B |
| :10371000640280915E05E82FFF27EE0FFF1FE55F33 |
| :10372000FA4F808191819F938F9380915D05E82F5F |
| :10373000FF27EE0FFF1FE55FFA4F808191819F9376 |
| :103740008F938AE794E09F938F9311E01F930E94D9 |
| :10375000D00D84E1809364028DB79EB707960FB6B3 |
| :10376000F8949EBF0FBE8DBF80916005E82FFF27A4 |
| :10377000EE0FFF1FE55FFA4F808191819F938F933A |
| :1037800080915F05E82FFF27EE0FFF1FE55FFA4FDF |
| :10379000808191819F938F938AE894E09F938F9388 |
| :1037A0001F930E94D00D88E2809364022DB73EB72C |
| :1037B000295F3F4F0FB6F8943EBF0FBE2DBF8091DB |
| :1037C0006205E82FFF27EE0FFF1FE55FFA4F8081AC |
| :1037D00091819F938F9380916105E82FFF27EE0FD2 |
| :1037E000FF1FE55FFA4F808191819F938F938AE954 |
| :1037F00094E09F938F931F930E94D00D8CE380934E |
| :1038000064028DB79EB707960FB6F8949EBF0FBEA1 |
| :103810008DBF80916405E82FFF27EE0FFF1FE55F46 |
| :10382000FA4F808191819F938F9380916305E82F58 |
| :10383000FF27EE0FFF1FE55FFA4F808191819F9375 |
| :103840008F938AEA94E01FCE8CE38093640284EF26 |
| :1038500093E02DCE84E48093640289E396E09F9305 |
| :103860008F93CF930E94D00D0F900F900F90F5CCB7 |
| :1038700080E48093640286E396E09F938F93CF93D6 |
| :103880000E94D00D0F900F900F90E2CC1092650225 |
| :10389000DF91CF911F91089587B18C6087B94298CD |
| :1038A00043981092A9041092A80480E88093A7047A |
| :1038B0008093A6040895E82F6B3F88F0273E78F0A8 |
| :1038C00047FD1EC084E090E002C0880F991FEA9572 |
| :1038D000E2F7982F909588B1892388B908956B3FB6 |
| :1038E00058F42E2F3327F901E855FB4F8081815082 |
| :1038F0008F3F89F0808308952A3098F747FDE2CF03 |
| :1039000084E090E002C0880F991FEA95E2F728B1A1 |
| :10391000282B28B9089561506083F901EA55FB4FBF |
| :1039200080818130C1F086958083F901EA55FB4F93 |
| :103930008081842341F084E090E002C0880F991FC9 |
| :103940002A95E2F7E4CF84E090E002C0880F991F47 |
| :103950002A95E2F7BECF80E8E7CF809167028150D9 |
| :103960008F3F19F080936702089584E08093670287 |
| :103970002091F901409193056091940580E00E94A7 |
| :103980005B1C2091FA01409195056091960581E0BC |
| :103990000E945B1C08950E94AD1C08958091AA04AA |
| :1039A0009927089580916C029927089580916B0260 |
| :1039B000992708950E94601E0E94D43508950F93A0 |
| :1039C0001F93CF93C0E000ED12E08C2F0E947B2468 |
| :1039D000F801819391938F01CF5FC830B0F3CF91FD |
| :1039E0001F910F9108951F93CF93DF9310E0C0E0D4 |
| :1039F000D0E0812F0E947B2497FD17C0FE01EC0FC1 |
| :103A0000FD1FE053FD4F2081318128173907CCF489 |
| :103A10002F3F3105B4F42F5F3F4F3183208321C006 |
| :103A2000812F0E947B2497FFE9CF80E090E0FE0188 |
| :103A3000EC0FFD1FE053FD4F20813181281739071E |
| :103A40003CF3FE01EC0FFD1FE053FD4F208131815F |
| :103A50001216130634F4821793071CF421503040D9 |
| :103A6000DCCF1F5F21961830D8F2DF91CF911F91E4 |
| :103A70000895CF930E949824C82F0E94CA1E8C17C5 |
| :103A800008F48C2F9927CF910895EF92FF920F930E |
| :103A90001F93CF93DF930E9451230E947C1E0E94AC |
| :103AA00001360E9478247C010E946E1E8C01F70171 |
| :103AB00080819181F80120813181820F931F909341 |
| :103AC000730280937202F70182819381F80122814F |
| :103AD0003381820F931F9093750280937402F701D4 |
| :103AE00024813581F80184819581820F931F0E9482 |
| :103AF000C53A0E94B33690936F0280936E02F7012D |
| :103B000086819781F80126813781820F931F9093D8 |
| :103B1000710280937002809172029091730290936F |
| :103B2000B4038093B30380917402909175029093D3 |
| :103B3000B6038093B5030E94391D843008F093C00A |
| :103B400010926D02C4E6D0E061E0E6E7F2E0A2E7A1 |
| :103B5000B2E04D915D91CA0157FD76C0959587956C |
| :103B60009595879597FD77C0208131812817390772 |
| :103B7000F0F5CA0157FD86C097FD89C095958795D8 |
| :103B8000959587958536910508F448C0D183C08303 |
| :103B90006150329667FFDDCF0E949824833008F091 |
| :103BA0006DC000E00E94CA1E833008F058C090E04B |
| :103BB000002329F190E080916C02801709F469C01C |
| :103BC00090936B0200936C020E94C5248093AA0418 |
| :103BD00080916B02882369F18091990380618093C1 |
| :103BE0009903002369F180919A0380615CC02115DB |
| :103BF000310571F22150304031832083C9CF9923A0 |
| :103C000081F0202F80916C02891709F44AC020931B |
| :103C10006B0290936C020E94711ED8CF91838083B7 |
| :103C2000B7CF00936B0200936C0280916B028823E4 |
| :103C300099F6809199038F7E80939903002399F6DA |
| :103C400080919A038F7E2FC00396959587959595C1 |
| :103C5000879597FF89CF909581959F4F85CF0E943B |
| :103C6000751E982FA5CF0E94F31C0E949F3780914C |
| :103C70006D020E94292580936D0264CF0E94BA24B0 |
| :103C8000082F90CF909581959F4F97FF77CF039600 |
| :103C900075CF91E090936B0200936C020E94C52453 |
| :103CA00095CF21E0B4CF80939A03DF91CF911F91FC |
| :103CB0000F91FF90EF9008950E94E72599270895AE |
| :103CC0001092B5041092AE041092AF041092B0049A |
| :103CD0001092B10485E58093AB04089587EB94E0DE |
| :103CE00008958091B504992708958091B304992788 |
| :103CF000089580E090E008958091B6048823C9F18A |
| :103D000081508093B6048091AE04282F332727FD7D |
| :103D1000309580916C05482F5527249FC001259F21 |
| :103D2000900D349F900D11249093B8048093B704A4 |
| :103D30008091AF04992787FD9095849F9001859F7E |
| :103D4000300D949F300D11243093BA042093B904A0 |
| :103D50008091B10499279093BC048093BB04809117 |
| :103D6000B004992787FD90959093BE048093BD047D |
| :103D700008951092BE041092BD041092BC041092DB |
| :103D8000BB041092BA041092B9041092B8041092B5 |
| :103D9000B70408958091B604813580F4882389F4AE |
| :103DA0008091B50480FF07C08091F801813818F038 |
| :103DB00081E090E0089580E090E0089584E090E054 |
| :103DC000089583E090E008951092C0041092BF041B |
| :103DD0001092C2041092C10408952AE535E040E033 |
| :103DE00050E00E94403FCA01B90128E631E040E0BE |
| :103DF00050E00E94403F9B01AC0197FD1EC061E076 |
| :103E00002A3531054105510554F084EB90E0A0E0DE |
| :103E1000B0E0821B930BA40BB50B9C01AD01E22F0C |
| :103E2000FF27EE0FFF1FEB50F94F85919491613002 |
| :103E300019F0909581959F4F089550954095309534 |
| :103E400021953F4F4F4F5F4F6FEFDACF653A21EE2D |
| :103E5000720721E0820720E092073CF06C5E744A12 |
| :103E6000854090400E94ED1E08956C557E418E4F16 |
| :103E70009F4F0E94ED1E0895CF93C1E02AE535E0E3 |
| :103E800040E050E00E94403F2A3531054105510590 |
| :103E9000CCF084EB90E0A0E0B0E0821B930BA40B8D |
| :103EA000B50B9C01AD01CFEFE22FFF27EE0FFF1FF7 |
| :103EB000E15CF94F85919491C130D9F0909581954D |
| :103EC0009F4F17C0263A8FEF38078FEF48078FEFC5 |
| :103ED00058072CF42C543F4F4F4F5F4FE5CF57FFFF |
| :103EE000E3CF50954095309521953F4F4F4F5F4F11 |
| :103EF000DACFCF9108959FB7F8943998389A88B15E |
| :103F0000836088B98091B9008C7F8093B9008AE280 |
| :103F10008093B80010927F0210927D0210927C0272 |
| :103F2000E7ECF4E08BE0108211821282148281505F |
| :103F3000359687FFF8CF9FBF089580937F0285EA6B |
| :103F40008093BC00089580937F0284E98093BC0035 |
| :103F500008958093BB0085E88093BC00089585ECAC |
| :103F60008093BC00089585E88093BC00089580E0AC |
| :103F70000E94A31F10927F028091BB0080937D025C |
| :103F800010927D0210927C0280E88093BC00109217 |
| :103F9000BD001092BA001092BB001092B9001092AE |
| :103FA000B8000E947B1F80E00E949D1F08951F9211 |
| :103FB0000F920FB60F9211242F933F934F935F935D |
| :103FC0006F937F938F939F93AF93BF93EF93FF93E1 |
| :103FD00080917F02282F33278F5F80937F022530C7 |
| :103FE000310509F43FC026303105FCF4223031059B |
| :103FF00009F47EC0233031050CF060C02115310575 |
| :1040000009F48DC021303105F9F480917D029927A2 |
| :10401000FC01EE0FFF1FEE0FFF1FE80FF91FE95322 |
| :10402000FB4F80810E94A91F41C12930310509F44D |
| :1040300094C02A303105A4F12A30310509F404C1B5 |
| :104040002B30310509F408C180E00E94A31F8AE0EB |
| :1040500090E090936C0180936B0110927D0210921E |
| :104060007C0224C180917C029927FC01EE0FFF1F86 |
| :10407000EE0FFF1FE80FF91FE953FB4F8091BB00C4 |
| :10408000848380917C028F5F80937C0280917C028C |
| :104090008C3010F010927C0280E00E94A31F06C1B9 |
| :1040A0002730310509F4D4C02830310569F68091F4 |
| :1040B0007A02880F805F0E94A91FF8C02330310563 |
| :1040C00009F46AC02430310509F0BECF80917C022A |
| :1040D0009927FC01EE0FFF1FEE0FFF1FE80FF91FDE |
| :1040E000E953FB4F8091BB0083830E94B31FDEC066 |
| :1040F0008091B900803309F47DC080E00E94A31F45 |
| :104100008AE090E090936C0180936B0180917D0236 |
| :104110008F5F80937D0280E00E949D1FC7C08091C9 |
| :104120007D02E82FFF27EE0FFF1FEE0FFF1FE65463 |
| :10413000FA4F85851816CCF480917D028C3008F4F6 |
| :1041400052C010927D0283E080937F0280917C02B6 |
| :10415000880F8D5A0E94A91FA9C080917A02E82F6A |
| :10416000FF27ED53FB4F80815DCF80917D028C3026 |
| :1041700018F780917D028F5F80937D0280917D0290 |
| :10418000E82FFF27EE0FFF1FEE0FFF1FE654FA4F39 |
| :104190008585181654F7D0CF8091B900803409F482 |
| :1041A00072C080917C029927FC01EE0FFF1FEE0F79 |
| :1041B000FF1FE80FF91FE953FB4F118280917C022A |
| :1041C0008F5F80937C0280917C028C3010F0109283 |
| :1041D0007C0281810E94A31F8091800280937B02D8 |
| :1041E0001092800263C080917D02880F8E5A0E94D7 |
| :1041F000A91F5CC080918002882329F480917D02F0 |
| :104200008F5F8093800280917D029927FC01EE0FE1 |
| :10421000FF1FEE0FFF1FE80FF91FE953FB4F8281CD |
| :104220008F5F8283882309F068CF80917D02992770 |
| :10423000FC01EE0FFF1FEE0FFF1FE80FF91FE95300 |
| :10424000FB4F8FEF828359CF80E80E94A91F2EC0B9 |
| :1042500088E90E94A91F2AC080E00E94A31F8AE06B |
| :1042600090E090936C0180936B0180917A02823090 |
| :1042700018F010927A021AC08F5F80937A0287E05A |
| :104280000E949D1F13C080917C029927FC01EE0FB4 |
| :10429000FF1FEE0FFF1FE80FF91FE953FB4F80913F |
| :1042A0007C028C5F81830E94AF1F96CFFF91EF91BC |
| :1042B000BF91AF919F918F917F916F915F914F913E |
| :1042C0003F912F910F900FBE0F901F901895AF92B6 |
| :1042D000BF92CF92DF92FF920F931F93CF93DF9302 |
| :1042E0008BEA97E09F938F931F920E94D00D0F90BF |
| :1042F0000F900F90E7ECF4E06BE0F62E108235960D |
| :10430000FA94F7FEFBCF80E00E949D1F80E197E2C8 |
| :10431000A0E0B0E0FC013197F1F710927C026C0153 |
| :104320005DEBA52E57E0B52E08EC14E0C0E0D0E020 |
| :104330004BE0F42E04C0FA942196F7FC1EC080E0F6 |
| :104340000E949D1FC6010197F1F7F80180810B5F64 |
| :104350001F4F882381F32196DF93CF932197BF923C |
| :10436000AF921F920E94D00D0F900F900F900F9060 |
| :104370000F90FA942196F7FEE2CF31ECA32E37E0AE |
| :10438000B32E27ECC22E25E0D22E08EC14E0C0E0BC |
| :10439000D0E09BE0F92EF8019081992391F4F60189 |
| :1043A0008081181674F42196DF93CF932197BF92E2 |
| :1043B000AF929F930E94D00D0F900F900F900F908F |
| :1043C0000F90F8011182FA9421960B5F1F4F84E041 |
| :1043D00090E0C80ED91EF7FEDECFDF91CF911F917E |
| :1043E0000F91FF90DF90CF90BF90AF9008959FB74F |
| :1043F000F89456985E9A8AB188638AB98BB1877CA3 |
| :104400008BB98091CF02813011F0539A5B988091E3 |
| :1044100080008C708093800080918100837E8093E7 |
| :10442000810080918100836C809381008091820063 |
| :104430008F738093820080916F00887F80936F00DC |
| :1044400080916F00806280936F00109284021092BE |
| :1044500083029FBF08951F920F920FB60F921124EF |
| :10446000EF92FF920F931F932F933F934F935F937E |
| :104470006F937F938F939F93AF93BF93CF93DF936C |
| :10448000EF93FF93209186003091870080918702FF |
| :1044900090918802281B390B8091860090918700AB |
| :1044A0009093880280938702C9018D549440835F62 |
| :1044B0009A4170F4809185029091860204970CF0E5 |
| :1044C00071C0C1E0D0E0D0938602C09385028AC05B |
| :1044D000C0918502D0918602C930D1050CF082C00E |
| :1044E000C9018B5F9040845B914008F059C0265D04 |
| :1044F0003140FE01EC0FFD1FE55FFA4F8081918195 |
| :10450000F901E81BF90BCF01F7FD65C0069774F4BC |
| :104510008091830290918402883C91050CF455C0EF |
| :1045200088EC90E09093840280938302FE01EC0F6C |
| :10453000FD1FE55FFA4F80819181019728173907A8 |
| :1045400034F080819181019682179307B4F5790147 |
| :104550008091830290918402833C910534F18E0115 |
| :104560000C0F1D1FF801E55FFA4F808191819701C3 |
| :10457000281B390BC90163E070E00E940B3FCB019F |
| :10458000880F991F860F971F015E1A4FF8019183BC |
| :104590008083FE01EC0FFD1FE55FFA4FF182E082A0 |
| :1045A000219691CF10926D018CCFFE01EC0FFD1F73 |
| :1045B000E15EFA4F11821082ECCF2F5F3F4F233024 |
| :1045C000310568F4EE24FF24C3CF809183029091DB |
| :1045D00084020A96A7CF909581959F4F97CFE08050 |
| :1045E000F180B6CFFF91EF91DF91CF91BF91AF9165 |
| :1045F0009F918F917F916F915F914F913F912F91FB |
| :104600001F910F91FF90EF900F900FBE0F901F9092 |
| :10461000189580915F05E82FFF27EE0FFF1FE55FDC |
| :10462000FA4F8081918186359105ACF080916005CB |
| :10463000E82FFF27EE0FFF1FE55FFA4F8081918182 |
| :10464000863591052CF5808191818B5A9F4F1CF105 |
| :1046500080E090E00895808191818B5A9F4FACF467 |
| :1046600080916005E82FFF27EE0FFF1FE55FFA4FEF |
| :1046700080819181863591058CF4808191818B5A5E |
| :104680009F4F34F786E090E0089580E090E0089531 |
| :1046900082E090E0089584E090E0089588E090E062 |
| :1046A0000895DF92EF92FF920F931F93CF93DF93C2 |
| :1046B00080918302909184020097F9F080918302A7 |
| :1046C0009091840201979093840280938302809159 |
| :1046D0006D01815080936D0180916D018F3FF1F0EC |
| :1046E0000E940923982F80918202891709F4DFC064 |
| :1046F0009093820210928102F3C090930A058093F6 |
| :10470000090590930805809307059093060580930B |
| :1047100005059093040580930305E2C080915D0533 |
| :10472000E82FFF27EE0FFF1FDF01A55FBA4F809133 |
| :104730006C05682F77278D919C91869F9001879FAC |
| :10474000300D969F300D1124E15EFA4F80916D057A |
| :10475000482F552780819181849FF001859FF00D1E |
| :10476000949FF00D11242E1B3F0B309304052093D2 |
| :10477000030580915E05E82FFF27EE0FFF1FDF0185 |
| :10478000A55FBA4F8D919C91869F9001879F300DB8 |
| :10479000969F300D1124E15EFA4F80819181849FB4 |
| :1047A000B001859F700D949F700D1124261B370B4F |
| :1047B000309306052093050580915F05E82FFF27BC |
| :1047C000EE0FFF1FDF01A15EBA4F8091ED01282F90 |
| :1047D00033278D919C91829FA001839F500D929FC2 |
| :1047E000500D1124E55FFA4F0190F081E02DE40FA8 |
| :1047F000F51FE858FF4FF7FD62C0F0930805E093FE |
| :10480000070580916005E82FFF27EE0FFF1FDF01EE |
| :10481000A55FBA4F8D919C91CC27DD27C81BD90B82 |
| :10482000E15EFA4F80819181C81BD90BD0906E0553 |
| :10483000ED2CFF2400271127BE01882777FD8095E6 |
| :10484000982F9E01D7FD47C0442737FD4095542F30 |
| :104850000E94CC3E9B01AC01C801B7010E94CC3E36 |
| :1048600097FD34C0E9E09595879577956795EA952A |
| :10487000D1F78D2D99278C9FF0018D9FF00D9C9F76 |
| :10488000F00D1124CF01F7FD1FC0959587959595E3 |
| :104890008795680F791F70930A05609309050E9438 |
| :1048A0000923982F80918202891709F021CF8091E6 |
| :1048B0008102883CA8F48F5F8093810211C010921E |
| :1048C0000805109207059DCF0396DFCF61507E4FFC |
| :1048D0008F4F9F4FC7CF222733272C1B3D0BB4CFC1 |
| :1048E000DF91CF911F910F91FF90EF90DF9008958E |
| :1048F00083E095E00895843080F4E82FFF27E35AA1 |
| :10490000FA4F8481E82FFF27EE0FFF1FE55FFA4F74 |
| :10491000808191818D589F4F0895E82FFF27EE0FDA |
| :10492000FF1FE55FFA4F828193818D589F4F089555 |
| :104930008091830290918402803A91058CF4809159 |
| :104940008302909184028C3891056CF480918302EB |
| :1049500090918402883791054CF481E090E00895AD |
| :1049600084E090E0089583E090E0089582E090E094 |
| :104970000895089580918102883C19F080E090E0CC |
| :104980000895809182029927089580915D05E82F0E |
| :10499000FF27EE0FFF1FE55FFA4F8081918187347B |
| :1049A0009105B4F080915E05E82FFF27EE0FFF1F01 |
| :1049B000E55FFA4F80819181873491050CF040C00A |
| :1049C000808191818A5B9F4F94F584E090E0089507 |
| :1049D000808191818A5B9F4FACF480915E05E82FC6 |
| :1049E000FF27EE0FFF1FE55FFA4F8081918187342B |
| :1049F00091054CF5808191818A5B9F4FF4F486E0AC |
| :104A000090E0089580915E05E82FFF27EE0FFF1FCD |
| :104A1000E55FFA4F80819181873491055CF4808154 |
| :104A200091818A5B9F4F94F080E090E0089583E04D |
| :104A300090E0089581E090E0089587E090E0089587 |
| :104A400082E090E0089588E090E0089585E090E0AD |
| :104A50000895682F40915E05E42FFF27EE0FFF1F9A |
| :104A6000E55FFA4F70918505272F3327808191816B |
| :104A7000281739070CF04AC08091A80582FF46C06C |
| :104A80006462E42FFF27EE0FFF1FE55FFA4F872FC9 |
| :104A9000992722273327281B390B80819181821780 |
| :104AA00093070CF079C08091A80583FF75C06862F8 |
| :104AB000A0915D05EA2FFF27EE0FFF1FE55FFA4F7C |
| :104AC000272F332780819181281739070CF04EC09A |
| :104AD0008091A80580FF4AC06161EA2FFF27EE0F91 |
| :104AE000FF1FE55FFA4F872F992722273327281BBF |
| :104AF000390B8081918182179307F4F48091A80586 |
| :104B000081FF1AC06261462F55275FC062FFB9CF8F |
| :104B1000E42FFF27EE0FFF1FE55FFA4F872F99273E |
| :104B200020918605821B9109208131812817390740 |
| :104B30000CF0A7CF6B7DA5CF462F552741FF45C071 |
| :104B4000EA2FFF27EE0FFF1FE55FFA4F872F992708 |
| :104B5000909581959F4F20918605821B9109208118 |
| :104B60003181821793078CF56D7ECDCF60FFB5CF75 |
| :104B7000EA2FFF27EE0FFF1FE55FFA4F872F9927D8 |
| :104B800020918605821B91092081318128173907E0 |
| :104B90000CF0A3CF6E7EA1CF63FF8ACFE42FFF2757 |
| :104BA000EE0FFF1FE55FFA4F872F992790958195AC |
| :104BB0009F4F20918605821B9109208131818217A8 |
| :104BC00093070CF075CF677D73CFCA01089580916C |
| :104BD0005D05E82FFF27EE0FFF1FE55FFA4F80818D |
| :104BE00091818D5E9F4F14F010926E0180819181B2 |
| :104BF0008A5B9F4F54F480916E01882331F481E0E9 |
| :104C000080936E0181E090E0089580E090E0089547 |
| :104C1000E82FFF27EE0FFF1FE15BFD4F80819181A1 |
| :104C2000AA2797FDA095BA2FBC01CD0123EC30E057 |
| :104C300040E050E00E94CC3EDC01CB01BC01CD0144 |
| :104C4000089580E00E940826DC01CB018093410595 |
| :104C500090934205A0934305B093440581E00E94E0 |
| :104C60000826DC01CB018093450590934605A0936F |
| :104C70004705B093480508951092F5011092F4018C |
| :104C800010928A0210928902109234051092330514 |
| :104C900010928E0210928D0210928C0210928B0252 |
| :104CA000109298021092970210929602109295021A |
| :104CB0000E94B5150E9421261092550510925605A6 |
| :104CC00010925705109258058091730190917401CC |
| :104CD0009093720180937101AA2797FDA095BA2F36 |
| :104CE000BC01CD012FEA34E040E050E00E94CC3E10 |
| :104CF000DC01CB0180933D0590933E05A0933F05D9 |
| :104D0000B093400508954F925F926F927F928F9279 |
| :104D10009F92AF92BF92CF92DF92EF92FF920F934A |
| :104D20001F93CF93DF9341E065E7462E64E0562E54 |
| :104D3000E1E9F2E059E4652E55E0752E32E8832E64 |
| :104D400034E0932E2DE4A22E25E0B22E91E9C92E57 |
| :104D500094E0D92E85E3E82E85E0F82E0BE812E0EA |
| :104D6000C2EAD4E089919991D8012D913D918D01AC |
| :104D7000820F931F67E070E00E940B3FD7016D9395 |
| :104D80007D937D01D6018D919D916D01820F931FC1 |
| :104D900067E070E00E940B3FD5016D937D935D014C |
| :104DA000D4018D919D914D01D3018D939D933D0132 |
| :104DB00080819181D2012D913D912D01820F931F10 |
| :104DC00081939193415047FFCDCF80918F02909175 |
| :104DD000900201969093900280938F0280917F04BD |
| :104DE000909180042091890230918A02820F931F52 |
| :104DF00090933C0580933B051092070180917A00C7 |
| :104E0000886C80937A00DF91CF911F910F91FF9072 |
| :104E1000EF90DF90CF90BF90AF909F908F907F905A |
| :104E20006F905F904F9008952F923F924F925F92B4 |
| :104E30006F927F928F929F92AF92BF92CF92DF92AA |
| :104E4000EF92FF920F931F93CF93DF93CDB7DEB70F |
| :104E500028970FB6F894DEBF0FBECDBF6091410515 |
| :104E60007091420580914305909144050E94261F50 |
| :104E70009A83898360914505709146058091470525 |
| :104E8000909148050E94261F8C0160914505709104 |
| :104E9000460580914705909148050E94ED1E7C01D2 |
| :104EA00060914105709142058091430590914405C0 |
| :104EB0000E943C1F98878F8380914D0590914E05ED |
| :104EC0003C01882477FC8094982CC801AA2797FD80 |
| :104ED000A095BA2F8B839C83AD83BE83C401B3019D |
| :104EE0002B813C814D815E810E94CC3E5B016C0137 |
| :104EF00080913B0590913C051C01442437FC409473 |
| :104F0000542C0027F7FC0095102FC201B101A80115 |
| :104F100097010E94CC3EDC01CB01A81AB90ACA0A4B |
| :104F2000DB0AD7FC4AC0ACE0D594C794B794A794E9 |
| :104F3000AA95D1F7B0925205A0925105C401B301D0 |
| :104F4000A80197010E94CC3E97FD4AC05B016C010D |
| :104F5000F9E0D594C794B794A794FA95D1F76091E6 |
| :104F600045057091460580914705909148050E943E |
| :104F7000261F9C01442737FD4095542FC201B101E3 |
| :104F80000E94CC3E97FD27C0E9E0959587957795DF |
| :104F90006795EA95D1F76A0D7B1D8C1D9D1DAF812C |
| :104FA000B8859D01442737FD4095542F0E94CC3E83 |
| :104FB0009B01AC0197FF19C009C08FEF9FE0A0E0F3 |
| :104FC000B0E0A80EB91ECA1EDB1EADCF215030487E |
| :104FD0004F4F5F4F0AC061507E4F8F4F9F4FD4CFCE |
| :104FE00061507E4F8F4F9F4FB1CF7FE055954795D2 |
| :104FF000379527957A95D1F780914F059091500577 |
| :10500000820F931F909354058093530580914D0513 |
| :1050100090914E05AA2797FDA095BA2FBC01CD010E |
| :10502000A80197010E94CC3EDC01CB01E981FA8105 |
| :105030007F010027F7FC0095102FBC01CD01A801CE |
| :1050400097010E94403F59016A0180913B05909170 |
| :105050003C05AA2797FDA095BA2FBC01CD012B8155 |
| :105060003C814D815E810E94CC3EDC01CB01BC01C4 |
| :10507000CD01A80197010E94403FDA01C9018A0DC4 |
| :105080009B1D90933A058093390528960FB6F894A6 |
| :10509000DEBF0FBECDBFDF91CF911F910F91FF906B |
| :1050A000EF90DF90CF90BF90AF909F908F907F90C8 |
| :1050B0006F905F904F903F902F900895CF93DF9324 |
| :1050C00080916D02882321F48091650586FD8BC057 |
| :1050D00080914D0590914E05909352058093510516 |
| :1050E00080914F05909150059093540580935305FE |
| :1050F00020913B0530913C05C90130933A0520933E |
| :105100003905EC01EE27D7FDE095FE2F80913D0596 |
| :1051100090913E05A0913F05B0914005C80FD91F61 |
| :10512000EA1FFB1FC901AA2797FDA095BA2F20915E |
| :105130005505309156054091570550915805820FFD |
| :10514000931FA41FB51F8093550590935605A093F8 |
| :105150005705B0935805C83186E9D80786E0E807B7 |
| :1051600080E0F807CCF1C851D649E640F040C09342 |
| :105170003D05D0933E05E0933F05F0934005E1E403 |
| :10518000F5E061E0C1E5D5E089919991AA2797FD05 |
| :10519000A095BA2F2081318142815381280F391F78 |
| :1051A0004A1F5B1F293483EC380783E0480780E0FF |
| :1051B00058072CF020593648474050401FC0293B23 |
| :1051C0008CE338078CEF48078FEF5807BCF4205763 |
| :1051D0003947484F5F4F12C0F7FFC9CFC85ED94665 |
| :1051E000E94FFF4FC4CF0E94142720913B05309117 |
| :1051F0003C058091390590913A0583CF2083318316 |
| :10520000428353836150349667FFBECFDF91CF91C5 |
| :1052100008952F923F924F925F926F927F928F925A |
| :105220009F92AF92BF92CF92DF92EF92FF920F9335 |
| :105230001F93CF93DF93CDB7DEB727970FB6F894C0 |
| :10524000DEBF0FBECDBFE0916D02EE2379F48091F9 |
| :10525000F101482F552722273327241B350B809136 |
| :10526000790490917A04821793071CF58091990331 |
| :10527000877F8093990380919A03877F80939A0315 |
| :105280001092AE031092AD031092B0031092AF03D0 |
| :105290001092BC031092BB031092BE031092BD0388 |
| :1052A0001092980210929702109296021092950214 |
| :1052B000ADC08091790490917A0448175907B4F2EF |
| :1052C000809199038860809399036091710521E032 |
| :1052D0008091700290917102805C9F4F813891059E |
| :1052E00028F001C066952A95EAF72E2F8091760264 |
| :1052F000909177028134910540F4809178029091E9 |
| :1053000079028134910508F47AC0669580919A03F8 |
| :10531000877F80939A031982262E332444245524B0 |
| :1053200081E495E09F838E83E5E9F2E0FB83EA83E5 |
| :1053300089E993E09D838C8389810E9408265B0123 |
| :105340006C0129E830E040E050E00E94403FEC81F1 |
| :10535000FD81358B248BEE81FF816080718082809E |
| :10536000938088EE93E0A0E0B0E082199309A4094D |
| :10537000B509BC01CD01A40193010E94CC3E7B0183 |
| :105380008C01C201B101A60195010E94CC3EDC0155 |
| :10539000CB01E80EF91E0A1F1B1FC801B70128EE3A |
| :1053A00033E040E050E00E94403FEE81FF812083E7 |
| :1053B0003183428353832081318126193709EA8161 |
| :1053C000FB8180819181820F931F81939193FB8355 |
| :1053D000EA83EC81FD8133A322A3F981FF5FF98386 |
| :1053E0008C819D8102969D838C83EE81FF81349612 |
| :1053F000FF83EE83F981F23008F49ECF07C02223A9 |
| :1054000009F484CF80919A03886083CF27960FB6E2 |
| :10541000F894DEBF0FBECDBFDF91CF911F910F91EA |
| :10542000FF90EF90DF90CF90BF90AF909F908F90C4 |
| :105430007F906F905F904F903F902F9008959F9234 |
| :10544000AF92BF92CF92DF92EF92FF920F931F9392 |
| :10545000CF93DF938091750190917601019709F0C8 |
| :1054600069C080E091E09093760180937501992462 |
| :1054700080918C0590918D05A82EBB24E92EFF24E8 |
| :10548000C92DDD27BE016C0F7D1FFB01EB56FD4FC3 |
| :1054900020813181C901880F991F880F991F880FBA |
| :1054A000991F821B930B9C01442737FD4095542F75 |
| :1054B000DA01C90180589F4FAF4FBF4FB7FD48C0B9 |
| :1054C000892F9A2FAB2FBB27A7FDBA956C01FB0143 |
| :1054D000E557FD4FB5010E940B3F20813181260F1A |
| :1054E000371F882799278E199F09281739073CF5F8 |
| :1054F000918380839E012C0F3D1F890107561C4F0D |
| :10550000C601B5010E940B3FF80177A366A3F9011C |
| :10551000E557FD4F80819181F80193AF82AF2B5603 |
| :105520003D4FF901118210829394F1E0F91508F0D2 |
| :10553000A7CF15C0909376018093750110C0E21635 |
| :10554000F3061CF4F182E082D5CF31832083D2CFE1 |
| :10555000DA01C90181589E4FAF4FBF4FB1CFDF91E4 |
| :10556000CF911F910F91FF90EF90DF90CF90BF9060 |
| :10557000AF909F9008950E9483260E945E2880919C |
| :105580005105909152059093A8038093A7038091B1 |
| :105590005305909154059093AA038093A903809199 |
| :1055A000390590913A059093AC038093AB038091B9 |
| :1055B0003505909136059093A2038093A1038091C5 |
| :1055C0003705909138059093A4038093A3038091AD |
| :1055D0003B0590913C059093A6038093A5030E9400 |
| :1055E00009290E941F2A08958F929F92AF92BF921D |
| :1055F000CF92DF92EF92FF920F931F93CF93DF939F |
| :1056000080919902882321F08091680280FF53C124 |
| :105610008091410590914205A0914305B0914405C8 |
| :10562000B7FD87C169E0B595A795979587956A9568 |
| :10563000D1F76C0197FD8BC180914505909146058E |
| :10564000A0914705B0914805B7FD6EC159E0B595E9 |
| :10565000A795979587955A95D1F7BC0197FD60C19D |
| :10566000C616D7060CF418C1B601D7FC19C15B01E8 |
| :10567000B594A794B594A794B594A7940894A11C45 |
| :10568000B11C809173019091740197FD69C1845E92 |
| :105690009D4F7C010027F7FC0095102FC0913D0520 |
| :1056A000D0913E05E0913F05F0914005BE01CF014C |
| :1056B0002FEA34E040E050E00E94403FE21AF30A53 |
| :1056C000040B150BC801B70128E631E040E050E0BB |
| :1056D0000E94403FDC01CB013CE4832E3FEF932E40 |
| :1056E000880E991E80913B0590913C0597FD34C131 |
| :1056F0008138910514F088249924E0906F01F0908E |
| :105700007001E114F104A9F429E1C216D1048CF46A |
| :105710008091330590913405880D991D909334053F |
| :105720008093330580919A0290919B02892B09F016 |
| :10573000D2C0C0913D05D0913E05E0913F05F0916A |
| :105740004005C401880F991F880F991F880F991F62 |
| :10575000B5010E940B3F9B01442737FD4095542F14 |
| :105760002C0F3D1F4E1F5F1F20933D0530933E05BC |
| :1057700040933F0550934005C091E4016C2F77277B |
| :105780006C9DC0016D9D900D7C9D900D112497FD29 |
| :10579000C2C06C01D594C794D594C794D594C794CE |
| :1057A000D594C794D594C7946C197D096B0177FD86 |
| :1057B000B4C080916F0190917001009709F073C09F |
| :1057C000009176021091770280917802909179028F |
| :1057D000080F191F169507951695079516950795A5 |
| :1057E000005C1F4FCA01B9012FEA34E040E050E0ED |
| :1057F0000E94403FDA01C90120917101309172018C |
| :10580000442737FD4095542F821B930BA40BB50BF7 |
| :10581000845E9D4FAF4FBF4FBC01CD0128E631E004 |
| :1058200040E050E00E94403FDC01CB01845B9040AF |
| :105830008C9D90018D9D300D9C9D300D1124C901D2 |
| :10584000B8010E940B3F83E0C89F60011124C61677 |
| :10585000D7067CF1882799278C199D096817790745 |
| :105860000CF4BC01CB01AA2797FDA095BA2F20917B |
| :105870005505309156054091570550915805820FB6 |
| :10588000931FA41FB51F8093550590935605A093B1 |
| :105890005705B09358056BC06B01B601D7FEE7CE34 |
| :1058A000695F7F4FE4CE01979093700180936F0101 |
| :1058B0005EC0B601D7CF0E945C1E882309F457C092 |
| :1058C000809199028F5F80939902853008F04BC0D8 |
| :1058D0000E945C3A4CC088EC90E00E94573A0091DC |
| :1058E000730110917401C801AA2797FDA095BA2FE2 |
| :1058F000BC01CD012FEA34E040E050E00E94CC3EF4 |
| :10590000FC01EB011093720100937101F0929B0274 |
| :10591000E0929A0216CF4F963CCF84EF91E0C4CF2D |
| :10592000709561957F4F9CCE81509E4FAF4FBF4F7A |
| :105930008DCE81509E4FAF4FBF4F69E0B595A79573 |
| :10594000979587956A95D1F76C0197FF75CED0949E |
| :10595000C194D108D39470CE909581959F4FC8CEB5 |
| :1059600088249924BFCE88EE93E00E94573ADF91B5 |
| :10597000CF911F910F91FF90EF90DF90CF90BF904C |
| :10598000AF909F908F900895AC018091EF01282FE8 |
| :1059900033272130310571F022303105E4F0223017 |
| :1059A000310581F02330310511F0CA010895461701 |
| :1059B00057075CF4640F751F77FD11C0CB019595F7 |
| :1059C0008795089564177507ACF3641B750BCA01BE |
| :1059D000861B970B0895232B41F7CA0108956F5F2B |
| :1059E0007F4FECCF809168028460809368028FEFD4 |
| :1059F000809304021092050280E2809306020E94C6 |
| :105A0000DF1C08950F931F93E62FF22F282F3327C3 |
| :105A10008CE398E2B9010E940B3F70937801609388 |
| :105A20007701E0935B0540935A05F0935905009385 |
| :105A30005C051F910F9108950F936091E601262F49 |
| :105A4000265F8091650582FF02C040E002C0409160 |
| :105A5000E701665F8091E9010091F0018F5F0E948C |
| :105A6000022D0F9108950F9308E72AE5402F622F2A |
| :105A700081E20E94022D0F9108952F923F924F9242 |
| :105A80005F926F927F928F929F92AF92BF92CF92CE |
| :105A9000DF92EF92FF920F931F93CF93DF93CDB7D7 |
| :105AA000DEB762970FB6F894DEBF0FBECDBF0E947F |
| :105AB000BB2A0E94451D60906E0270906F020E948A |
| :105AC000391D833008F031C48091DF018A3009F438 |
| :105AD0003DC3843109F43AC328980E94713A8091F9 |
| :105AE000A9029091AA02009709F43AC301979093F2 |
| :105AF000AA028093A9028091690290916A02815062 |
| :105B0000914008F002C5809168028E7F8093680200 |
| :105B100020916D02222339F0809184059927861502 |
| :105B200097050CF43C014091700250917102BA014A |
| :105B300057FD3CC480916E059927880F991F880FE7 |
| :105B4000991F861797070CF0E1C388EE93E09093B6 |
| :105B5000700180936F01E0916505E4FD06C081E06E |
| :105B600090E090939B0280939A02CA01AA2797FD26 |
| :105B7000A095BA2F40915505509156056091570553 |
| :105B800070915805481B590B6A0B7B0B403BBCE3DB |
| :105B90005B07BFEF6B07BFEF7B070CF0AAC380EB7F |
| :105BA0009CE3AFEFBFEF8093550590935605A0930C |
| :105BB0005705B09358058E2F992788729070892BBE |
| :105BC00009F0BFC255243327C52CDD2480E190E0C5 |
| :105BD0000C2C02C0880F991F0A94E2F7282339235E |
| :105BE000232B09F42EC3F601EC0DFD1D21E030E05E |
| :105BF0002C0F3D1FE20FF31F1182108286010C0D46 |
| :105C00001D1DB5E0CB2ED12CCC0EDD1EC00ED11E3D |
| :105C1000A1E0EA2EF12CEC0EFD1EE00EF11EF801C3 |
| :105C2000EB5CFA4F80819181AA2797FDA095BA2F4E |
| :105C300020915B05332744275527BC01CD010E94E5 |
| :105C4000CC3E97FD6AC3F6E0959587957795679565 |
| :105C5000FA95D1F7D7014D915C91460F571F075B1D |
| :105C60001A4F8091E501282F3327F8018081918117 |
| :105C7000829FB001839F700D929F700D112477FD5C |
| :105C800051C37595679575956795759567957595E4 |
| :105C90006795640F751FF0EC60307F070CF0C6C28B |
| :105CA00080E090ECD6018D939C935394B1E0B515B0 |
| :105CB00008F0B9C280913B0590913C05880F991F6F |
| :105CC000AA2797FDA095BA2F20915905332744277D |
| :105CD0005527BC01CD010E94CC3E7B018C0197FD74 |
| :105CE0005CC356E015950795F794E7945A95D1F75C |
| :105CF00080915C059927AA27BB27209155053091F3 |
| :105D000056054091570550915805BC01CD010E94A0 |
| :105D1000CC3EDC01CB01BC01CD0120EF35E540E0FC |
| :105D200050E00E94403FB701620F731F30EC6030BB |
| :105D300073070CF0D6C260E070EC80917B02882380 |
| :105D4000A1F08091690290916A020297C09748F48D |
| :105D50001614170434F481E090E090936A02809363 |
| :105D6000690280916F05682E7724660C771C660C9B |
| :105D7000771C8091700290917102880F991F880F93 |
| :105D8000991F4B01881A990A51EA651671040CF49F |
| :105D90009DC2C30177FCB7C395958795222733270A |
| :105DA000281B390B821693060CF0F1C24901809131 |
| :105DB0007005A82EBB24AA0CBB1CAA0CBB1CC301DB |
| :105DC0008A199B09881699060CF4CEC1C50186195B |
| :105DD0009709881599050CF4C7C18091F701682FC0 |
| :105DE000772788279927940197FCC8C137FDCCC134 |
| :105DF00035952795260D371D442737FD4095542F9F |
| :105E00000E94CC3E97FDCEC1A6E0959587957795EB |
| :105E10006795AA95D1F75B01552422243324261ACD |
| :105E2000370A80917701909178019C01442755278A |
| :105E30002D873E874F87588B40905A05442009F4A0 |
| :105E4000DBC1C52CDD24B601660F771F660F771FF7 |
| :105E50006F557D4FF601EC0DFD1DA1E0B0E0AC0FDC |
| :105E6000BD1FAE0FBF1FEE58FD4F8D919C9120813D |
| :105E70003181821B930B9C01442737FD4095542FA1 |
| :105E8000FB0180819181A281B381820F931FA41FA6 |
| :105E9000B51F80839183A283B383F601EE0FFF1FAA |
| :105EA000EE0FFF1FEF55FD4F80819181A281B381DD |
| :105EB000803026E092072FEFA2072FEFB2070CF0F9 |
| :105EC00090C180E096E0AFEFBFEF80839183A28323 |
| :105ED000B383F601EC0DFD1D09E010E00C0F1D1F52 |
| :105EE0000E0F1F1FA5E0B0E0AC0FBD1FAE0FBF1F10 |
| :105EF000EE58FD4FED90FC9080819181E81AF90AEF |
| :105F0000CC0CDD1CCC0CDD1C41EA52E0C40ED51ECD |
| :105F1000F60180819181A281B381BC01CD012D85E3 |
| :105F20003E854F8558890E94403FE20EF31EE214E1 |
| :105F3000F3040CF070C1D8012D923C925394B1E05F |
| :105F4000B51508F07BCF29843A843092B4032092AF |
| :105F5000B3038B859C859093B6038093B503909291 |
| :105F6000B8038092B7037092BA036092B903BB245E |
| :105F700080916F05482E552480917005E82FFF27EA |
| :105F8000FA8BE98BA0902A02C4C0CB2CDD248601B9 |
| :105F9000000F111F000F111F06541A4FF80125851D |
| :105FA00012160CF0BCC0C301AA2797FDA095BA2F0A |
| :105FB000332727FD3095432F532FBC01CD010E947D |
| :105FC000CC3E97FD6AC146E0959587957795679594 |
| :105FD0004A95D1F77B01C101AA2797FDA095BA2F59 |
| :105FE000F8012685332727FD3095432F532FBC0119 |
| :105FF000CD010E94CC3E97FD60C136E09595879516 |
| :10600000779567953A95D1F7E60EF71E8B859C85B7 |
| :10601000AA2797FDA095BA2FF8012785332727FDDA |
| :106020003095432F532FBC01CD010E94CC3E97FDEC |
| :106030003FC126E095958795779567952A95D1F785 |
| :10604000E60EF71EC401AA2797FDA095BA2FF80106 |
| :106050002089332727FD3095432F532FBC01CD01D5 |
| :106060000E94CC3EDC01CB01B7FD1EC116E0B59508 |
| :10607000A795979587951A95D1F786010C0D1D1D4B |
| :1060800005551D4FF801608171818E0D9F1D0E9485 |
| :10609000C42CD8018D939C9397FD04C17C01F59489 |
| :1060A000E794F594E794E414F5040CF08BC07201C6 |
| :1060B0001E141F040CF091C0FFEFEF16F10421F045 |
| :1060C0001CF07FEFE72EF12CF601EE0FFF1FEE0F15 |
| :1060D000FF1FEC0DFD1DE953FB4FE08223E02B1564 |
| :1060E00098F0D601AC0DBD1DA756BC4FF601EE0FC2 |
| :1060F000FF1FEE0FFF1FEC0DFD1DE953FB4F8081CD |
| :106100009927FD0197A786A7B394FBE0FB1508F438 |
| :1061100099C18091680280FD38CFCB2CDD24AA2064 |
| :1061200009F445C0F601EE0FFF1FEE0FFF1FEC0D47 |
| :10613000FD1DE953FB4FD601A65EBD4F8C918083B8 |
| :10614000CDCF0E94F42A20916D023CCD289A0E9466 |
| :10615000713A8091A9029091AA02009709F0C6CCE9 |
| :10616000809168028E7ED2CC4C018091F701682F1D |
| :10617000772788279927940197FE38CE222733273F |
| :106180002819390937FF34CE2F5F3F4F35952795B2 |
| :10619000260D371D442737FD4095542F0E94CC3ED5 |
| :1061A00097FF32CE615C7F4F8F4F9F4F2DCEF60110 |
| :1061B000EE0FFF1FEE0FFF1FEC0DFD1DE953FB4F10 |
| :1061C000A0828CCFE989FA89EE15FF050CF070CF1B |
| :1061D0007F011E141F040CF46FCFE1E0EE2EF12CB2 |
| :1061E00073CF81509A4FA040B0400CF472CE80E043 |
| :1061F0009AEFA0E0B0E069CEC52CDD24B601660FB1 |
| :10620000771F660F771F6F557D4FF601EC0DFD1D53 |
| :10621000A5E0B0E024CEAE14BF040CF053C0F801EA |
| :10622000B182A0828BCE20916D02CDCCB0E46130E2 |
| :106230007B070CF46EC080E090E4F60191838083CC |
| :1062400034CD86010C0D1D1D41E050E04C0F5D1F4B |
| :10625000040F151FF601EE0FFF1FEE0FFF1FEF5B80 |
| :10626000FA4F80915A05282F3327442755278081DC |
| :106270009181A281B381BC01CD010E94CC3EDC01A1 |
| :10628000CB01BC01CD0128EF3AE240E050E00E9492 |
| :10629000403FF80131832083B1CC615C7F4F8F4F49 |
| :1062A0009F4F91CE0396FACECF96A11DB11DDECEA3 |
| :1062B000615C7F4F8F4F9F4FBCCE615C7F4F8F4F94 |
| :1062C0009F4F9BCED801ED92FC9238CE80EB881682 |
| :1062D0008FEF98060CF049C080EB882E8FEF982E38 |
| :1062E00066CD40E4613074070CF427CD60E070E4C3 |
| :1062F00024CD4135F3EC5F07F0E06F07F0E07F0756 |
| :10630000ECF180E593ECA0E0B0E04DCCE0916505C8 |
| :106310002CCCD6016D937C93C8CC615C7F4F8F4FA2 |
| :106320009F4F91CC615F7F4FACCC0E94391D833071 |
| :1063300008F041C059E2651671040CF457C0809111 |
| :10634000680280FF53C08091690290916A02AFEFAA |
| :106350008F3F9A0729F0019690936A028093690211 |
| :106360000E94E03A0E941C2DD3CB91E5891691043E |
| :106370000CF41DCD00E5802E912C19CD40935505D0 |
| :1063800050935605609357057093580514CC8815A3 |
| :1063900099050CF00CCD4C010ACD2FE330E040E024 |
| :1063A00050E0E20EF31E041F151F9BCC66277727D3 |
| :1063B000641B750BBFCB809168028F7E809368024F |
| :1063C00080917805282F332788EE91E0289FA0013F |
| :1063D000299F500D389F500D11245093AA024093CD |
| :1063E000A90259E2651671040CF0A9CF80916902E7 |
| :1063F00090916A028F3F910511F008F07EC01092D3 |
| :10640000A5021092A6021092A7021092A802109262 |
| :10641000A1021092A2021092A3021092A4028A3F3B |
| :10642000910509F09DCF81E090E090939B028093CD |
| :106430009A02109255051092560510925705109227 |
| :1064400058058ECF80E00E949D1F80917901815078 |
| :1064500009F050C088E180937901809141059091C5 |
| :106460004205A0914305B0914405BC01CD0129E846 |
| :1064700030E040E050E00E94403F30939C03209386 |
| :106480009B038091450590914605A0914705B091E9 |
| :106490004805BC01CD0129E830E040E050E00E9411 |
| :1064A000403F30939E0320939D0380913D05909142 |
| :1064B0003E05A0913F05B0914005BC01CD012FEAFA |
| :1064C00034E040E050E00E94403F3093A00320932E |
| :1064D0009F0380917B0490917C049093D8038093D8 |
| :1064E000D70380917D0490917E049093DA0380938A |
| :1064F000D90317C08093790114C08091680282602B |
| :10650000809368022DCF019647CC80917705682E45 |
| :106510007724809168028061809368020E94332D05 |
| :10652000F7CA62960FB6F894DEBF0FBECDBFDF91FB |
| :10653000CF911F910F91FF90EF90DF90CF90BF9080 |
| :10654000AF909F908F907F906F905F904F903F9013 |
| :106550002F900895EBE7F5E083E01192815087FFDB |
| :10656000FCCFEFE8F5E083E01192815087FFFCCF8C |
| :106570009AE090937E0585ED80938F0582E080936D |
| :10658000900588E78093910590939205089582E0A5 |
| :1065900080935D0581E080935E0583E080935F05D5 |
| :1065A00074E07093600525E020936105E6E0E093D8 |
| :1065B000620587E08093630558E05093640584E4A6 |
| :1065C000809365054EE1409366058BEF80936805E7 |
| :1065D0008AE0809369054093670540936B0570934B |
| :1065E0006A0550936C058CE080936D0580936E0571 |
| :1065F00080936F0586EE8093700580E88093720526 |
| :1066000080E58093730534E6309374056FE56093FD |
| :10661000760583E28093770540937805109279059B |
| :1066200080E280937A0530937F0588E2809380052D |
| :106630001092A90592E39093810586E980938205E3 |
| :1066400020938305909384052AE5209385059093F4 |
| :1066500086051092A8052093870583E4809388051A |
| :106660001092890590938E056093930583EF809334 |
| :1066700095058FE080939405809396058DEF809328 |
| :106680009705309398052093990520939A052093B8 |
| :106690009B058BE480939C0580939D0580939E05CC |
| :1066A00010929F05E093A0055093A1052093A205A9 |
| :1066B0004093A3053093A4053093A5057093A605D8 |
| :1066C00008950E94C7320E94553B0E94AA3280EC76 |
| :1066D0008093650586E0AAE7B1E0EEEAF5E00D906B |
| :1066E00001928A95E1F708950E94C7320E94553BB6 |
| :1066F0000E94AA3280EC8093650583E080936A054E |
| :1067000084E1809394058093960587E0A1E8B1E049 |
| :10671000EEEAF5E00D9001928A95E1F708950E9466 |
| :10672000C7320E94553B0E94AA3280EC80936505D7 |
| :1067300083E080936A0584E18093780586E4809302 |
| :1067400089058EE1809394058093960589E0A9E8F8 |
| :10675000B1E0EEEAF5E00D9001928A95E1F7089537 |
| :10676000DC01A050B0400E945B3F802D9927089526 |
| :10677000DC01A050B040062E0E946D3F0895DC0160 |
| :10678000A050B0400E94683FCF010895DC01A050A6 |
| :10679000B0400B010E94793F0895982F81508530B9 |
| :1067A00008F093E0EDE5F5E08DE5989FD001112428 |
| :1067B000A95FBF4F182E0E94633F0E944C1C089592 |
| :1067C000863010F085E003C0882309F481E0A2E060 |
| :1067D000B0E0082E0E946D3F0895282F863008F102 |
| :1067E00025E0EDE5F5E08DE5289FD0011124A95FB6 |
| :1067F000BF4F182E0E947E3FA2E6B0E08DE590E0EC |
| :106800000C010E94793FEDE5F5E0A0E5B0E088E0FD |
| :10681000182E0E947E3F822F0E94E0330E944C1C63 |
| :1068200008958823F1F6089522E030E0D9010E940E |
| :106830005B3F802D863028F083E0D901082E0E942E |
| :106840006D3F9927089528EE33E0D9010E945B3F00 |
| :10685000802D813019F080E090E00895EAEBF5E0BA |
| :106860008DE4D901182E0E94633F81E090E00895E5 |
| :10687000EAEBF5E08091BA05813019F080E090E014 |
| :106880000895A8EEB3E08DE4182E0E947E3F81E0CB |
| :1068900090E0089581E08093BA0580E0EAEBF5E0AE |
| :1068A00009C090E4958716861786108A8F5F349604 |
| :1068B000803158F48430A8F390E0958716861786C7 |
| :1068C000108A8F5F34968031A8F390E49093C805C6 |
| :1068D0009093CA0580EC8093CC059093CE0580936D |
| :1068E000D1058093D2059093D5058093D60587E096 |
| :1068F000A3E9B1E0EBEBF5E00D9001928A95E1F7A9 |
| :106900000895FF920F931F93CF9381E0F82EA1E09B |
| :10691000B0E00E945B3F802D8B3409F452C08FEDB4 |
| :1069200097E09F938F931F920E94D00D0F900F902E |
| :106930000F90A8EEB3E08FEF082E0E946D3F90E01D |
| :1069400020E530E0D9010E945B3F802D8C3008F0BB |
| :10695000FF249F5F943010F4FF20A1F7C1E001E015 |
| :1069600010E00230110541F0033011050CF052C067 |
| :106970000130110509F464C00E947433FF2089F0CE |
| :10698000EDE5F5E020E030E097E0D901A05BBF4FF6 |
| :106990000E945B3F802D819391502F5F3F4F97FF67 |
| :1069A000F4CF8C2F0E94ED33CF5F0F5F1F4FC630A7 |
| :1069B000C0F281E00E94E0336BE481E090E00E944D |
| :1069C000B8330E9414340E94CD330E9414349927A6 |
| :1069D0009F938F938AEF97E09F938F931F920E94CC |
| :1069E000D00D0E942334282F0F900F900F900F90FE |
| :1069F0000F908823B1F01092A00290E0E7ECF5E050 |
| :106A0000CFE08081349618160CF49F5FC150C7FF09 |
| :106A1000F8CF19C00330110509F0AECF0E948F33B3 |
| :106A2000ADCF83E198E09F938F932F930E94D00D79 |
| :106A30000E944A340E9438340F900F900F90DBCFA1 |
| :106A40000E9461339BCF9093A002892F99279F9337 |
| :106A50008F938BEB95E09F938F9384E398E09F93C4 |
| :106A60008F931F920E94D00D8DB79EB707960FB6D9 |
| :106A7000F8949EBF0FBE8DBF85E598E09F938F93DE |
| :106A80001F920E94D00D0F900F900F90CF911F91E9 |
| :106A90000F91FF9008954FB72AE230E0F89480916B |
| :106AA000C9008F778093C9008091C9008F7B809344 |
| :106AB000C9008091C9008F7D8093C9005A9A52986D |
| :106AC0005B9A539A832F99278093CD002093CC0013 |
| :106AD0008091C80082608093C80088E18093C900DB |
| :106AE0008091CA008F778093CA008091CA008F7B03 |
| :106AF0008093CA008091CA008F7D8093CA008091E4 |
| :106B0000CA008F7E8093CA008091CA00877F8093DD |
| :106B1000CA008091C9008B7F8093C9008091CA0010 |
| :106B200084608093CA008091CA0082608093CA000A |
| :106B30008091C80087FF0CC08091CE008091C80072 |
| :106B4000882334F48091CE008091C8008823A4F378 |
| :106B50008091C90080688093C9004FBF08951F923B |
| :106B60000F920FB60F9211248F938091CE008F91C8 |
| :106B70000F900FBE0F901F9018958091170690915F |
| :106B80001806A0911906B0911A0620919D04309123 |
| :106B90009E0440919F045091A004821B930BA40B70 |
| :106BA000B50BBC01CD01089580919D0490919E0488 |
| :106BB000A0919F04B091A0048093170690931806AB |
| :106BC000A0931906B0931A060E94BD35DC01CB01D3 |
| :106BD00080930B0690930C06A0930D06B0930E06BF |
| :106BE00010920706109208061092090610920A06E3 |
| :106BF00010920F06109210061092110610921206B3 |
| :106C00000895EF92FF920F931F93CF93DF930E940B |
| :106C1000BD359B01AC018091070690910806A091BB |
| :106C20000906B0910A0682179307A407B50744F432 |
| :106C300020930706309308064093090650930A06EE |
| :106C40008091650581FF48C08091990381608093A0 |
| :106C500099038091E1018852803B08F45BC09091D8 |
| :106C6000CA02993208F04DC0983209F467C09F5F9C |
| :106C70009093CA028091C3028F5F803309F047C0AE |
| :106C80001092C30280916A05E82EFF240027112785 |
| :106C9000C0910B06D0910C06E0910D06F0910E0606 |
| :106CA000A80197012C0F3D1F4E1F5F1F80911306F7 |
| :106CB00090911406A0911506B091160682179307BD |
| :106CC000A407B5077CF120930B0630930C06409384 |
| :106CD0000D0650930E0640C0809199038E7F8093DD |
| :106CE00099039091E10184E6989FC0011124AA279D |
| :106CF000BB278093130690931406A0931506B093B8 |
| :106D000016068091C3028F5F803309F4B9CF809358 |
| :106D1000C30222C01092CA0280919A038E7F809390 |
| :106D20009A03EFCF9E01AF012E193F09400B510B83 |
| :106D3000281739074A075B077CF0C5CF2093130655 |
| :106D400030931406409315065093160680919A03CB |
| :106D5000816080939A038BCFDF91CF911F910F9128 |
| :106D6000FF90EF900895CF92DF92FF920F931F93C1 |
| :106D7000CF93DF936C010E94BD35DC01CB01C09144 |
| :106D80000B06D0910C06E0910D06F0910E06C81B83 |
| :106D9000D90BEA0BFB0B2091CB023091CC028C017A |
| :106DA000021B130B8093CB029093CC02A093CD02D5 |
| :106DB000B093CE02101611060CF0A7C0F090990304 |
| :106DC00022E0F22A80919A038D7F80939A0380912A |
| :106DD00069059927AA27BB27BC01CD019E01AF01F8 |
| :106DE0000E94CC3EDC01CB01BC01CD0128EE33E09A |
| :106DF00040E050E00E94403F809167059927809FC6 |
| :106E0000B001819F700D909F700D1124261B370BD0 |
| :106E10008091660599278217930734F090958195A4 |
| :106E20009F4F281739070CF49C018091650599271D |
| :106E300080FF08C081FF5AC08091E1018852803BE9 |
| :106E400008F054C08091C6029091C702A091C80278 |
| :106E5000B091C902BC01CD0128E130E040E050E032 |
| :106E60000E94CC3E9B01AC01C801AA2797FDA095CA |
| :106E7000BA2F820F931FA41FB51FBC01CD0129E1BA |
| :106E800030E040E050E00E94403FFA01E9012093E9 |
| :106E9000C6023093C7024093C8025093C9022091A2 |
| :106EA0009B0130919C0188E190E0289FA001299FDF |
| :106EB000500D389F500D1124CA018C0D9D1D69E1A4 |
| :106EC00070E00E94F73E70939C0160939B0180915B |
| :106ED000690290916A02885E934028F0C330D10520 |
| :106EE000E105F10534F04BEFF4221EC0C20ED31EB3 |
| :106EF000A9CFCE5FDF4FEF4FFF4FACF37093C502CA |
| :106F00006093C40224E0F22A0FC017FD03C0F09082 |
| :106F100099035DCF80919A03826080939A03F090E9 |
| :106F200099034DEFF42253CFF0929903C601DF91FC |
| :106F3000CF911F910F91FF90DF90CF900895CF9345 |
| :106F4000DF93E0916805EF3F29F0EB3F08F04BC17C |
| :106F5000E093E101E0916705EF3F09F451C1EB3F98 |
| :106F600030F0FF27EE0FFF1FE652FF4FE081E09366 |
| :106F7000E001EE2309F049C1E093E001E0916905E9 |
| :106F8000EF3F09F44DC1EB3F30F0FF27EE0FFF1F3D |
| :106F9000E652FF4FE081E093E201EE2309F045C1A4 |
| :106FA000E093E201E0916B05EF3F59F0EB3F08F40D |
| :106FB0005AC1FF27EE0FFF1FE652FF4F80818093DB |
| :106FC000E301E0917205EF3F59F0EB3F08F451C146 |
| :106FD000FF27EE0FFF1FE652FF4F80818093E401F1 |
| :106FE000E0917305EF3F09F44BC1EB3F30F0FF2711 |
| :106FF000EE0FFF1FE652FF4FE081E093E601EB301A |
| :1070000008F01EC1EAE0E093E601E0917405EF3F6D |
| :1070100059F0EB3F08F42AC1FF27EE0FFF1FE6529D |
| :10702000FF4F80818093E701E0917505EF3F59F0B4 |
| :10703000EB3F08F428C1FF27EE0FFF1FE652FF4F7A |
| :1070400080818093E501E0917A05EF3F59F0EB3FB5 |
| :1070500008F41CC1FF27EE0FFF1FE652FF4F80818F |
| :107060008093E901AAEEB1E0CBE7D5E093E0E991A6 |
| :10707000EF3F51F0EB3F08F4DFC0FF27EE0FFF1F9B |
| :10708000E652FF4F80818C939150119697FFEFCF7E |
| :10709000AEEEB1E0CFE8D5E093E0E991EF3F51F0FB |
| :1070A000EB3F08F4CBC0FF27EE0FFF1FE652FF4F68 |
| :1070B00080818C939150119697FFEFCFE0917F05DF |
| :1070C000EF3F59F0EB3F08F4E7C0FF27EE0FFF1F3B |
| :1070D000E652FF4F80818093F201E0918405EF3FFB |
| :1070E00059F0EB3F08F4DBC0FF27EE0FFF1FE6521D |
| :1070F000FF4F80818093F301E0918705EF3F59F0C6 |
| :10710000EB3F08F4CFC0FF27EE0FFF1FE652FF4F03 |
| :1071100080818093F401E0918805EF3F59F0EB3FC7 |
| :1071200008F4C3C0FF27EE0FFF1FE652FF4F808118 |
| :107130008093F501E0918905EF3F59F0EB3F08F4AA |
| :10714000B7C0FF27EE0FFF1FE652FF4F80818093ED |
| :10715000F601E0918E05EF3F59F0EB3F08F4ABC02C |
| :10716000FF27EE0FFF1FE652FF4F80818093F7014C |
| :10717000E0919405EF3F09F4A5C0EB3F30F0FF2705 |
| :10718000EE0FFF1FE652FF4FE081E093F901E2307E |
| :1071900008F05EC0E1E0E093F901E0919605EF3F71 |
| :1071A00009F45EC0EB3F30F0FF27EE0FFF1FE65201 |
| :1071B000FF4FE081E093FA01E23008F045C0E1E0E2 |
| :1071C000E093FA01E091A705EF3F09F47EC0EB3FA1 |
| :1071D00008F45FC0FF27EE0FFF1FE652FF4F8081CC |
| :1071E0008093F80172C0FF27EE0FFF1FE652FF4F9A |
| :1071F00080818093E101E0916705EF3F09F0AFCE18 |
| :10720000E091E001EE2309F4B7CEE43608F4B6CEFF |
| :1072100084E68093E001E0916905EF3F09F0B3CE89 |
| :10722000E091E201EE2309F4BBCEE43608F4BACED5 |
| :1072300084E68093E201B6CEEC9326CFEC933ACF6E |
| :10724000EF3F09F0E2CEDFCEEF3F09F0BBCFB8CF82 |
| :10725000EF3F09F4A0CFE0919605EF3F09F0A2CFF0 |
| :10726000E091FA01A9CFE093E301ABCEE093E7010F |
| :10727000DBCEE093E401E0917305EF3F09F0B5CE7A |
| :10728000E091E601BCCEE093E501DDCEE093E901BB |
| :10729000E9CEE093F80119C0E093F2011ECFE0932C |
| :1072A000F3012ACFE093F40136CFE093F50142CF0A |
| :1072B000E093F6014ECFE093F701E0919405EF3FA4 |
| :1072C00009F05BCFE091F90162CFDF91CF91089592 |
| :1072D0008B3F38F0E82FFF27EE0FFF1FE652FF4FDE |
| :1072E0008081861710F4862F03C0481708F4842F76 |
| :1072F00099270895AF92BF92CF92DF92EF92FF92BB |
| :107300000F931F93CF93DF93EDE5CE2ED12C77E231 |
| :10731000E72E64E1F62E05EB11E0D8018C91992758 |
| :107320008C9DF0018D9DF00D9C9DF00D1124E35A74 |
| :10733000FA4F9081E80189818E9DE0011124C052AD |
| :10734000DE4F9F3F31F04FEF60E0892F0E94683998 |
| :107350008883FA940E5F1F4FF7FEDFCF5DE5A52E01 |
| :10736000B12C47E2E42E0DE911E035E0F32E26EBD7 |
| :10737000C22E21E0D22EF801808199278A9DF0014A |
| :107380008B9DF00D9A9DF00D1124E35AFA4F9081D8 |
| :10739000D6018C91C2E0D0E0CC0EDD1E8E9DE001C6 |
| :1073A0001124C052DE4F9F3F39F0F80143816281C2 |
| :1073B000892F0E9468398883FA940C5F1F4FF7FE6B |
| :1073C000DACF06E613E0C3E4D0E193E0F92E98812A |
| :1073D000C35ADF4F9F3F39F04FEF60E0892F0E9483 |
| :1073E0006839D8018C93FA94095D1F4FF7FEEFCFEF |
| :1073F00002E014E0C7E8D7E183E0F82E9881C35A91 |
| :10740000DF4F9F3F39F04FEF60E0892F0E946839CE |
| :10741000F8018083FA94095D1F4FF7FEEFCFDF91EB |
| :10742000CF911F910F91FF90EF90DF90CF90BF9081 |
| :10743000AF90089590E08091C800803229F4809147 |
| :10744000CA00863009F491E0892F992708952DE02C |
| :1074500085B18C7F85B9209888EE93E00197F1F78C |
| :1074600083B199278370907081309105A9F08230A3 |
| :107470009105B4F0029791F084B1836084B92A3009 |
| :1074800049F0243139F028982C3010F429980CC098 |
| :10749000299A0AC0289AF8CF2BE0EECF24E1ECCF4E |
| :1074A000892B51F72AE0E8CF822F992708959093EE |
| :1074B0004102809340020895CF93C82FC150CF3F1F |
| :1074C00071F08091680280FD0AC084E690E00E941D |
| :1074D000573A8AEF90E00E941011C15090F7CF9177 |
| :1074E000089580910401909105018F5F9F4F09F0ED |
| :1074F000089588E99AE3909341028093400280E0E6 |
| :107500009CE090930501809304010895809104010B |
| :10751000909105018F5F9F4F09F008958091680257 |
| :1075200080FFFBCF80E197E290934102809340027D |
| :1075300080E890E09093050180930401089580E035 |
| :1075400093E0909305018093040180914002909113 |
| :107550004102892B31F480E797E1909341028093B7 |
| :107560004002089587E090E0909305018093040124 |
| :107570008091400290914102892B31F480E797E19C |
| :10758000909341028093400208950F931F938C01C2 |
| :1075900060914105709142058091430590914405A9 |
| :1075A0000E94261F60914505709146058091470510 |
| :1075B000909148050E94261FC8011F910F910895C0 |
| :1075C0001F93CF930E94D21CC82F0E94D61C182F45 |
| :1075D0000E94CE1C282F8091680280FD04C0C2301A |
| :1075E000F1F0C430C9F1C63021F0C83009F059C0FB |
| :1075F0000CC081E090E090936A02809369028091D0 |
| :1076000068028960809368024CC010926A021092EE |
| :107610006902809168028E7F8093680242C01123C4 |
| :1076200009F03FC08091DF018C3000F1299A2630AB |
| :1076300018F5822F8150853080F10E9414340E9409 |
| :10764000CD330E943C260E94F22C0E94DA1C0E943C |
| :1076500014340E945C3A25C0112319F58091DF0192 |
| :107660008C3030F429980E948316EBCF2998DFCF15 |
| :10767000299A0E948316E5CF809165059927887223 |
| :107680009070892B71F0273061F481E0809399022A |
| :1076900088EE93E00E94573A04C0822F0E94E033A4 |
| :1076A000CCCFCF911F910895089595E090937505E3 |
| :1076B0008AE080938D0581E08093710590938C051D |
| :1076C0008EE480938A0580938B05089584B1806A47 |
| :1076D00084B93D9A82E58CBD1DBC459A85E596E04E |
| :1076E000909376068093750681E080931B068AEA64 |
| :1076F0008093550683E8809356068AE08093570668 |
| :10770000109259061092580610925B0610925A0673 |
| :107710001092E1021092E2021092E30210925106DE |
| :107720008AE48093520684E68093530686E0809331 |
| :1077300054060895F8948091410590914205A091D6 |
| :107740004305B0914405BC01CD0129E830E040E09B |
| :1077500050E00E94403F3093590620935806809194 |
| :10776000450590914605A0914705B0914805BC019B |
| :10777000CD0129E830E040E050E00E94403F3093E6 |
| :107780005B0620935A0620913D0530913E054091BD |
| :107790003F0550914005DA01C90163E0880F991F48 |
| :1077A000AA1FBB1F6A95D1F7820F931FA41FB51F95 |
| :1077B000820F931FA41FB51FBC01CD012FEA34E037 |
| :1077C00040E050E00E94403F309361062093600605 |
| :1077D00080914D0590914E059093630680936206CB |
| :1077E00080914F05909150059093650680936406B3 |
| :1077F00080913B0590913C059093670680936606C7 |
| :1078000080E00E940826DC01CB01BC01CD0129E803 |
| :1078100030E040E050E00E94403F30935D0620930E |
| :107820005C0681E00E940826DC01CB01BC01CD0191 |
| :1078300029E830E040E050E00E94403F30935F068E |
| :1078400020935E061092900210928F021092940282 |
| :107850001092930210929202109291028091570618 |
| :1078600099278C30910509F48CC08D3091050CF06E |
| :107870006CC08A30910509F432C10B9709F4BDC080 |
| :1078800078948091E302882309F49DC02091370603 |
| :1078900030913806C90181509E4F8F5F9340C8F4E4 |
| :1078A0004091390650913A06CA0181509E4F8F5F30 |
| :1078B000934078F48091650585FF0BC03093C20436 |
| :1078C0002093C1045093C0044093BF048AEF809377 |
| :1078D000E20280913D0690913E0621E089369207B2 |
| :1078E0000CF0F2C09C01909374018093730137FDFA |
| :1078F000F1C08091710190917201281B390BC9016F |
| :10790000845E9D4F68E671E00E940B3F845B90406F |
| :1079100090939D0280939C022091410630914206F3 |
| :1079200080914002909141028217930740F4809128 |
| :107930009902882321F430934102209340028091E0 |
| :107940003606873609F448C008958D30910509F44C |
| :10795000E8C00E9709F094CF809151068093680695 |
| :1079600080915206809369068091530680936A063F |
| :107970008091540680936B068091DF0180936C06A2 |
| :107980007FCF8091990280936806853010F0109225 |
| :1079900099028091A6058093690610926B06109259 |
| :1079A0006A0680919C0580936C0680919D0580936A |
| :1079B0006D0680919E0580936E0678948091E30217 |
| :1079C000882309F063CF1092C2041092C104109270 |
| :1079D000C0041092BF04089580914306809304026E |
| :1079E00080914406809306028091450680930502AB |
| :1079F000809146068093E102089580915F05E82F0B |
| :107A0000FF27EE0FFF1FE55FFA4F0190F081E02D99 |
| :107A1000E038F1050CF0B6C02FEFE038F20714F4AF |
| :107A2000E0E8FFEFE093680680916005E82FFF270C |
| :107A3000EE0FFF1FE55FFA4F0190F081E02DE03877 |
| :107A4000F1050CF4B2C0EFE7F0E0E0936906809135 |
| :107A50005E05E82FFF27EE0FFF1FE55FFA4F01904D |
| :107A6000F081E02DE038F1050CF497C0EFE7F0E08D |
| :107A7000E0936A0680915D05E82FFF27EE0FFF1F58 |
| :107A8000E55FFA4F0190F081E02DE038F1050CF44C |
| :107A90007CC0EFE7F0E0E0936B068091D00280932A |
| :107AA0006C068091D20280936D068091D4028093FF |
| :107AB0006E068091D60280936F068091830290912A |
| :107AC000840280937006DCCE2091730130917401A2 |
| :107AD00037FF0FCF10929D0210929C021DCFA8E697 |
| :107AE000B6E0EAEEF1E097E081918D93915097FF37 |
| :107AF000FBCF809168028093700680916802837F3B |
| :107B00008093680280910801909109018093710629 |
| :107B100080917605809372060E94143480937306D8 |
| :107B2000AFCE8091970580936806809198058093E9 |
| :107B300069068091990580936A0680919A058093E1 |
| :107B40006B0680919B0580936C0680919F058093C6 |
| :107B50006D068091A00580936E068091A1058093AB |
| :107B60006F068091A405809370068091A205809392 |
| :107B700071068091A305809372068091A50580937C |
| :107B800073067ECEEFE7F0E04DCF8FEFE038F807D9 |
| :107B90000CF081CFE0E8FFEF7ECF6FEFE038F60723 |
| :107BA0000CF066CFE0E8FFEF63CF4FEFE038F4076B |
| :107BB0000CF04BCFE0E8FFEF48CF80911B06882305 |
| :107BC00009F435C045988091E002E82FFF27EF5E69 |
| :107BD000FD4F982F9F5F808180935706963008F065 |
| :107BE00027C09093E00210921B060E949A3B81E00E |
| :107BF0008093E4020000000000000000000000008C |
| :107C00000000000000000000000000000000000074 |
| :107C10000000000000000000000000000000000064 |
| :107C20000000000080915506809374068EBD089573 |
| :107C30001092E002D8CF20911B06222309F049C000 |
| :107C40000DB407FE46C084E080933F02459A3EB5DE |
| :107C50008091E70299278130910509F43BC0823079 |
| :107C600091050CF451C0029709F439C02091E40247 |
| :107C7000203208F02BC045980000000000000000F2 |
| :107C800000000000000000000000000000000000F4 |
| :107C900000000000000000000000000000000000E4 |
| :107CA0000000000000000000E0917506F0917606EB |
| :107CB000E20FF11D80818EBD809174069081890F45 |
| :107CC000809374062F5F2093E402089581E08093EF |
| :107CD0001B060895353531F12093E702C7CF809117 |
| :107CE000E602E82FFF27E45EF94F30838F5F809331 |
| :107CF000E6028A3100F18091E802381711F12093F1 |
| :107D0000E3021092E702B2CF892B09F0AFCF2093A4 |
| :107D1000E6023093E802313809F0A8CF81E0809381 |
| :107D2000E702A4CF8091E802830F8093E80282E00B |
| :107D30008093E7029BCF8091E802830F8093E80253 |
| :107D400095CFF8948AE1ECE1F6E0A6E3B6E0019085 |
| :107D50000D928A95E1F7789481E08093E302109286 |
| :107D6000E70284CFFB01DC0140FF05C002C00590A3 |
| :107D70000D9205900D9242505040C8F70895FC01B5 |
| :107D80004150504030F001900616D1F73197CF01A5 |
| :107D90000895882799270895629FD001739FF00165 |
| :107DA000829FE00DF11D649FE00DF11D929FF00D8B |
| :107DB000839FF00D749FF00D659FF00D9927729FC2 |
| :107DC000B00DE11DF91F639FB00DE11DF91FBD014D |
| :107DD000CF0111240895991B79E004C0991F9617CB |
| :107DE00008F0961B881F7A95C9F780950895AA1BFD |
| :107DF000BB1B51E107C0AA1FBB1FA617B70710F096 |
| :107E0000A61BB70B881F991F5A95A9F780959095C7 |
| :107E1000BC01CD01089597FB092E07260AD077FDF6 |
| :107E200004D0E5DF06D000201AF4709561957F4FED |
| :107E30000895F6F7909581959F4F0895A1E21A2E27 |
| :107E4000AA1BBB1BFD010DC0AA1FBB1FEE1FFF1FFE |
| :107E5000A217B307E407F50720F0A21BB30BE40B4E |
| :107E6000F50B661F771F881F991F1A9469F7609595 |
| :107E70007095809590959B01AC01BD01CF0108954F |
| :107E800097FB092E05260ED057FD04D0D7DF0AD068 |
| :107E9000001C38F450954095309521953F4F4F4F39 |
| :107EA0005F4F0895F6F790958095709561957F4F97 |
| :107EB0008F4F9F4F0895F999FECFB2BDA1BDF89A9B |
| :107EC000119600B40895F7DF01921A94E1F708952E |
| :107ED000F2DFE02DF0DFF02D0895F999FECFB2BD6D |
| :107EE000A1BD00BC11960FB6F894FA9AF99A0FBE8C |
| :107EF0000895F3DF012CF1DF112408950190EDDFE7 |
| :067F00001A94E1F7089558 |
| :107F0600F40101FFFFFF010164006A0004000400A0 |
| :107F16000200080008000402190048656C6C6F2016 |
| :107F2600576F726C64000000000000000000000043 |
| :107F3600000000000000000000000000000000003B |
| :107F4600000000000000000000000000000000002B |
| :107F5600000000000000000000000000000000001B |
| :107F6600000000000000000000000D64000101F4A4 |
| :107F760001FFFFFFFF000138010153706F727400AB |
| :107F8600004E6F726D616C0000426567696E6E65CA |
| :107F960072000051756164726F000032000A0000C1 |
| :107FA600640C000096160600FF371901FF391A0106 |
| :107FB600FF47210AFF0B010E031504170718051DBD |
| :107FC60009221227132A142B152C1631173A1B3B9C |
| :107FD6001C3C1D3D1E3E1F4220452246234A180DCD |
| :107FE60030FB103A40089696020A00000000000096 |
| :107FF600000064465A414064000000000000000092 |
| :1080060000000000000000000080000001000000E9 |
| :08801600000A0B0D0B0C0E001B |
| :00000001FF |
| /branches/dongfang_FC_rewrite/TODO-list.txt |
|---|
| 1,2 → 1,20 |
| - Check that acc. mode looping is still complete and "safe" to use. |
| - Find that pitch and roll rate limiter missing at least in HH mode. |
| - Find that pitch and roll rate limiter missing at least in HH mode. DONE |
| Reng. i badevaer. DONE |
| Reng. i sovevaer. DONE |
| Rydde op i vasketoej DONE |
| Mail til Phat DONE |
| SMS til Jonggil DONE |
| Skift servo paa fly DONE |
| Velo flicken DONE |
| Rep. CKF kopter DONE aber wieder |
| Montere LED lys paa kopter |
| Skrive GPS software |
| Jakke til rens |
| GPS hack? |
| Rydde op paa bord... |
| LED Flasher Problem beh vers. |
| Krankenkasse Anfrage |
| Still More Streamlined Steam and Early Diesels |
| /branches/dongfang_FC_rewrite/analog.c |
|---|
| 113,14 → 113,25 |
| volatile uint8_t ACC_FILTER; |
| /* |
| * Air pressure measurement. |
| * Air pressure |
| */ |
| #define MIN_RAWPRESSURE 200 |
| #define MAX_RAWPRESSURE (1023-MIN_RAWPRESSURE) |
| volatile uint8_t rangewidth = 53; |
| volatile uint16_t rawAirPressure; |
| volatile uint16_t filteredAirPressure; |
| volatile uint8_t rangewidth = 106; |
| // Direct from sensor, irrespective of range. |
| // volatile uint16_t rawAirPressure; |
| // Value of 2 samples, with range. |
| volatile uint16_t simpleAirPressure; |
| // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
| volatile int32_t filteredAirPressure; |
| // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
| volatile int32_t airPressureSum; |
| // The number of samples summed into airPressureSum so far. |
| volatile uint8_t pressureMeasurementCount; |
| /* |
| * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
| * That is divided by 3 below, for a final 10.34 per volt. |
| 169,23 → 180,23 |
| AD_ACC_PITCH, |
| AD_ACC_ROLL, |
| // AD_AIRPRESSURE, |
| AD_AIRPRESSURE, |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_ACC_Z, // at 7, measure Z acc. |
| AD_ACC_Z, // at 8, measure Z acc. |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, // at 10, finish yaw gyro |
| AD_GYRO_YAW, // at 11, finish yaw gyro |
| AD_ACC_PITCH, // at 11, finish pitch axis acc. |
| AD_ACC_ROLL, // at 12, finish roll axis acc. |
| AD_AIRPRESSURE, // at 13, finish air pressure. |
| AD_ACC_PITCH, // at 12, finish pitch axis acc. |
| AD_ACC_ROLL, // at 13, finish roll axis acc. |
| AD_AIRPRESSURE, // at 14, finish air pressure. |
| AD_GYRO_PITCH, // at 14, finish pitch gyro |
| AD_GYRO_ROLL, // at 15, finish roll gyro |
| AD_UBAT // at 16, measure battery. |
| AD_GYRO_PITCH, // at 15, finish pitch gyro |
| AD_GYRO_ROLL, // at 16, finish roll gyro |
| AD_UBAT // at 17, measure battery. |
| }; |
| // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
| 230,14 → 241,10 |
| } |
| } |
| uint16_t getAbsPressure(int advalue) { |
| uint16_t getSimplePressure(int advalue) { |
| return (uint16_t)OCR0A * (uint16_t)rangewidth + advalue; |
| } |
| uint16_t filterAirPressure(uint16_t rawpressure) { |
| return rawpressure; |
| } |
| /***************************************************** |
| * Interrupt Service Routine for ADC |
| * Runs at 312.5 kHz or 3.2 µs. When all states are |
| 247,9 → 254,10 |
| ISR(ADC_vect) { |
| static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
| static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0}; |
| static uint8_t pressure_wait = 10; |
| static uint16_t pressureAutorangingWait = 25; |
| uint16_t rawAirPressure; |
| uint8_t i, axis; |
| int16_t range; |
| int16_t newrange; |
| // for various filters... |
| int16_t tempOffsetGyro, tempGyro; |
| 261,7 → 269,8 |
| * sensorInputs array first, and all the processing after the last sample. |
| */ |
| switch(state++) { |
| case 7: // Z acc |
| case 8: // Z acc |
| if (ACC_REVERSED[Z]) |
| acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
| else |
| 268,7 → 277,7 |
| acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
| break; |
| case 10: // yaw gyro |
| case 11: // yaw gyro |
| rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
| if (GYRO_REVERSED[YAW]) |
| yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
| 276,7 → 285,7 |
| yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
| break; |
| case 11: // pitch axis acc. |
| case 12: // pitch axis acc. |
| if (ACC_REVERSED[PITCH]) |
| acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH]; |
| else |
| 286,7 → 295,7 |
| measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1); |
| break; |
| case 12: // roll axis acc. |
| case 13: // roll axis acc. |
| if (ACC_REVERSED[ROLL]) |
| acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL]; |
| else |
| 295,38 → 304,71 |
| measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1); |
| break; |
| case 13: // air pressure |
| if (pressure_wait) { |
| // A range switch was done recently. Wait for steadying. |
| pressure_wait--; |
| case 14: // air pressure |
| if (pressureAutorangingWait) { |
| //A range switch was done recently. Wait for steadying. |
| pressureAutorangingWait--; |
| break; |
| } |
| range = OCR0A; |
| rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
| if (rawAirPressure < MIN_RAWPRESSURE) { |
| // value is too low, so decrease voltage on the op amp minus input, making the value higher. |
| range -= (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1; |
| if (range < 0) range = 0; |
| pressure_wait = (OCR0A - range) * 4; |
| OCR0A = range; |
| newrange = OCR0A - (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1; |
| if (newrange > MIN_RANGES_EXTRAPOLATION) { |
| pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; |
| OCR0A = newrange; |
| } else { |
| if (OCR0A) { |
| OCR0A--; |
| pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
| } |
| } |
| } else if (rawAirPressure > MAX_RAWPRESSURE) { |
| // value is too high, so increase voltage on the op amp minus input, making the value lower. |
| range += (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1; |
| if (range > 254) range = 254; |
| pressure_wait = (range - OCR0A) * 4; |
| OCR0A = range; |
| // If near the end, make a limited increase |
| newrange = OCR0A + (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1; |
| if (newrange < MAX_RANGES_EXTRAPOLATION) { |
| pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
| OCR0A = newrange; |
| } else { |
| if (OCR0A<254) { |
| OCR0A++; |
| pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
| } |
| } |
| } |
| // Even if the sample is off-range, use it. |
| simpleAirPressure = getSimplePressure(rawAirPressure); |
| if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
| // Danger: pressure near lower end of range. If the measurement saturates, the |
| // copter may climb uncontrolled... Simulate a drastic reduction in pressure. |
| airPressureSum += (int16_t)MIN_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MIN_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
| } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
| // Danger: pressure near upper end of range. If the measurement saturates, the |
| // copter may fall uncontrolled... Simulate a drastic increase in pressure. |
| airPressureSum += (int16_t)MAX_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MAX_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
| } else { |
| filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure)); |
| // normal case. |
| airPressureSum += simpleAirPressure; |
| } |
| DebugOut.Analog[13] = range; |
| DebugOut.Analog[14] = rawAirPressure; |
| DebugOut.Analog[15] = filteredAirPressure; |
| // 2 samples were added. |
| pressureMeasurementCount += 2; |
| if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR) { |
| filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER-1) + airPressureSum + AIRPRESSURE_FILTER/2) / AIRPRESSURE_FILTER; |
| pressureMeasurementCount = airPressureSum = 0; |
| } |
| // DebugOut.Analog[14] = OCR0A; |
| // DebugOut.Analog[15] = simpleAirPressure; |
| DebugOut.Analog[11] = UBat; |
| DebugOut.Analog[27] = acc[Z]; |
| break; |
| case 14: |
| case 15: // pitch or roll gyro. |
| axis = state - 15; |
| case 15: |
| case 16: // pitch or roll gyro. |
| axis = state - 16; |
| tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
| // DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
| /* |
| 379,8 → 421,9 |
| gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER; |
| break; |
| case 16: |
| // battery |
| case 17: |
| // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
| // This is divided by 3 --> 10.34 counts per volt. |
| UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
| analogDataReady = 1; // mark |
| ADCycleCount++; |
| 407,7 → 450,6 |
| #define GYRO_OFFSET_CYCLES 32 |
| uint8_t i, axis; |
| int32_t deltaOffsets[3] = {0,0,0}; |
| int16_t filteredDelta; |
| // Set the filters... to be removed again, once some good settings are found. |
| GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1; |
| 419,15 → 461,15 |
| // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
| for(i=0; i < GYRO_OFFSET_CYCLES; i++) { |
| Delay_ms_Mess(10); |
| for (axis=0; axis<=YAW; axis++) { |
| deltaOffsets[axis] += rawGyroSum[axis] - gyroOffset[axis]; |
| Delay_ms_Mess(20); |
| for (axis=PITCH; axis<=YAW; axis++) { |
| deltaOffsets[axis] += rawGyroSum[axis]; |
| } |
| } |
| for (axis=0; axis<=YAW; axis++) { |
| filteredDelta = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
| gyroOffset[axis] += filteredDelta; |
| for (axis=PITCH; axis<=YAW; axis++) { |
| gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES; |
| DebugOut.Analog[20+axis] = gyroOffset[axis]; |
| } |
| // Noise is relative to offset. So, reset noise measurements when changing offsets. |
| 437,6 → 479,13 |
| accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
| accOffset[Z] = GetParamWord(PID_ACC_Z); |
| // Rough estimate. Hmm no nothing happens at calibration anyway. |
| // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
| // pressureMeasurementCount = 0; |
| // Experiment! |
| // filteredAirPressureOffset = filteredAirPressure - 1000L; |
| Delay_ms_Mess(100); |
| } |
| 456,12 → 505,12 |
| for(i=0; i < ACC_OFFSET_CYCLES; i++) { |
| Delay_ms_Mess(10); |
| for (axis=0; axis<=YAW; axis++) { |
| deltaOffset[axis] += acc[axis]; |
| } |
| for (axis=PITCH; axis<=YAW; axis++) { |
| deltaOffset[axis] += acc[axis]; |
| } |
| } |
| for (axis=0; axis<=YAW; axis++) { |
| for (axis=PITCH; axis<=YAW; axis++) { |
| filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
| accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
| } |
| 486,7 → 535,7 |
| /* |
| pressureDiff = 0; |
| DebugOut.Analog[16] = rawAirPressure; |
| // DebugOut.Analog[16] = rawAirPressure; |
| #define PRESSURE_CAL_CYCLE_COUNT 2 |
| for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
| 503,7 → 552,7 |
| pressureDiff += (rawAirPressure - savedRawAirPressure); |
| } |
| DebugOut.Analog[16] = rangewidth = |
| (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2); |
| // DebugOut.Analog[16] = |
| rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2); |
| */ |
| } |
| /branches/dongfang_FC_rewrite/analog.h |
|---|
| 4,7 → 4,7 |
| //#include "invenSense.h" |
| //#include "ENC-03_FC1.3.h" |
| #include "ADXRS610_FC2.0.h" |
| //#include "ADXRS610_FC2.0.h" |
| /* |
| * How much low pass filtering to apply for gyro_PID. |
| 181,11 → 181,14 |
| extern volatile int16_t gyro_PID[2]; |
| extern volatile int16_t gyro_ATT[2]; |
| extern volatile int16_t gyroD[2]; |
| extern volatile int16_t yawGyro; |
| extern volatile uint16_t ADCycleCount; |
| extern volatile int16_t UBat; |
| extern volatile int16_t yawGyro; |
| // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
| #define UBAT_AT_5V (int16_t)((5.0 / 3.0) * (1.0/11.0) / (3.0 * 1024)) |
| /* |
| * This is not really for external use - but the ENC-03 gyro modules needs it. |
| */ |
| 198,18 → 201,45 |
| extern volatile int16_t filteredAcc[2]; |
| /* |
| * Diagnostics: Gyro noise level because of motor vibrations. The variables |
| * only really reflect the noise level when the copter stands still but with |
| * its motors running. |
| */ |
| extern volatile uint16_t gyroNoisePeak[2]; |
| extern volatile uint16_t accNoisePeak[2]; |
| /* |
| * Air pressure. |
| * The sensor has a sensitivity of 46 mV/kPa. |
| * An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 1195 * 10^-6 * h |
| * |
| */ |
| #define AIRPRESSURE_SUMMATION_FACTOR 14 |
| #define AIRPRESSURE_FILTER 8 |
| // Minimum A/D value before a range change is performed. |
| #define MIN_RAWPRESSURE (200 * 2) |
| // Maximum A/D value before a range change is performed. |
| #define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE) |
| #define MIN_RANGES_EXTRAPOLATION 10 |
| #define MAX_RANGES_EXTRAPOLATION 250 |
| #define PRESSURE_EXTRAPOLATION_COEFF 25L |
| #define AUTORANGE_WAIT_FACTOR 1 |
| extern volatile uint16_t simpleAirPressure; |
| extern volatile int32_t filteredAirPressure; |
| /* |
| * At saturation, the filteredAirPressure may actually be (simulated) negative. |
| */ |
| extern volatile int32_t filteredAirPressure; |
| /* |
| * Flag: Interrupt handler has done all A/D conversion and processing. |
| */ |
| extern volatile uint8_t analogDataReady; |
| // Diagnostics: Gyro noise level because of motor vibrations. The variables |
| // only really reflect the noise level when the copter stands still but with |
| // its motors running. |
| extern volatile uint16_t gyroNoisePeak[2]; |
| extern volatile uint16_t accNoisePeak[2]; |
| // void SearchAirPressureOffset(void); |
| void analog_init(void); |
| // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 59,10 → 59,14 |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| // For scope debugging only! |
| #include "rc.h" |
| // where our main data flow comes from. |
| #include "analog.h" |
| #include "configuration.h" |
| #include "output.h" |
| // Some calculations are performed depending on some stick related things. |
| #include "controlMixer.h" |
| 84,7 → 88,7 |
| * The variables are overwritten at each attitude calculation invocation - the values |
| * are not preserved or reused. |
| */ |
| int16_t rate[2], yawRate; |
| int16_t rate_ATT[2], yawRate; |
| // With different (less) filtering |
| int16_t rate_PID[2]; |
| 105,20 → 109,19 |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. |
| */ |
| int32_t angle[2], yawAngle; |
| int32_t angle[2], yawAngleDiff; |
| int readingHeight = 0; |
| // compass course |
| int16_t compassHeading = -1; // negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| int16_t compassOffCourse = 0; |
| int16_t compassHeading = -1; // negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| int16_t compassOffCourse = 0; |
| uint16_t updateCompassCourse = 0; |
| uint8_t compassCalState = 0; |
| // uint8_t FunnelCourse = 0; |
| uint8_t compassCalState = 0; |
| uint16_t badCompassHeading = 500; |
| int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t yawGyroDrift; |
| #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| 126,10 → 129,13 |
| int16_t correctionSum[2] = {0,0}; |
| // For NaviCTRL use. |
| int16_t averageAcc[2] = {0,0}, averageAccCount = 0; |
| /* |
| * Experiment: Compensating for dynamic-induced gyro biasing. |
| */ |
| int16_t dynamicOffset[2] = {0,0}, dynamicOffsetYaw = 0; |
| int16_t driftComp[2] = {0,0}, driftCompYaw = 0; |
| // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
| // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
| // int16_t dynamicCalCount; |
| 164,18 → 170,18 |
| // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
| dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
| dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0; |
| driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
| correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| // Calibrate hardware. |
| analog_calibrate(); |
| // reset gyro readings |
| rate[PITCH] = rate[ROLL] = yawRate = 0; |
| // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| yawAngle = 0; |
| yawAngleDiff = 0; |
| // update compass course to current heading |
| compassCourse = compassHeading; |
| 191,19 → 197,20 |
| * TODO: Ultimately, the analog module could do this (instead of dumping |
| * the values into variables). |
| * The rate variable end up in a range of about [-1024, 1023]. |
| * When scaled down by CONTROL_SCALING, the interval is about [-256, 256]. |
| *************************************************************************/ |
| void getAnalogData(void) { |
| uint8_t axis; |
| for (axis=PITCH; axis <=ROLL; axis++) { |
| rate_PID[axis] = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| yawRate = yawGyro + dynamicOffsetYaw; |
| averageAccCount++; |
| yawRate = yawGyro + driftCompYaw; |
| // We are done reading variables from the analog module. |
| // Interrupt-driven sensor reading may restart. |
| analogDataReady = 0; |
| 221,23 → 228,23 |
| int16_t cosroll = int_cos(angle[ROLL]); |
| int16_t sinroll = int_sin(angle[ROLL]); |
| int16_t tanpitch = int_tan(angle[PITCH]); |
| #define ANTIOVF 1024 |
| ACRate[PITCH] = ((int32_t) rate[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate[ROLL] + (((int32_t)rate[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| #define ANTIOVF 512 |
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| } |
| // 480 usec with axis coupling - almost no time without. |
| void integrate(void) { |
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
| // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
| trigAxisCoupling(); |
| trigAxisCoupling(); |
| } else { |
| ACRate[PITCH] = rate[PITCH]; |
| ACRate[ROLL] = rate[ROLL]; |
| ACYawRate = yawRate; |
| ACRate[PITCH] = rate_ATT[PITCH]; |
| ACRate[ROLL] = rate_ATT[ROLL]; |
| ACYawRate = yawRate; |
| } |
| /* |
| 246,9 → 253,7 |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| */ |
| yawGyroHeading += ACYawRate; |
| // Why is yawAngle not wrapped 'round? |
| yawAngle += ACYawRate; |
| yawAngleDiff += yawRate; |
| if(yawGyroHeading >= YAWOVER360) { |
| yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| 283,21 → 288,26 |
| uint8_t axis; |
| int32_t correction; |
| if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] = 1; |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived; |
| if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
| if((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if(abs(controlYaw) > 25) { // reduce further if yaw stick is active |
| if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if (debugFullWeight) |
| DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| else |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| 305,26 → 315,23 |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| // There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
| // change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
| // = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
| // = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 |
| // = permilleAcc * (accDerived - angle[axis]) / 1000 |
| // Experiment: Do not acutally apply the correction. See if drift compensation does that. |
| // angle[axis] += correction / 1000; |
| angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| DebugOut.Analog[16+axis] = angle[axis] - correction; |
| } |
| DebugOut.Digital[1] = debugFullWeight; |
| } else { |
| DebugOut.Digital[0] = 0; |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Analog[9] = 0; |
| DebugOut.Analog[10] = 0; |
| DebugOut.Analog[16] = 0; |
| DebugOut.Analog[17] = 0; |
| // experiment: Kill drift compensation updates when not flying smooth. |
| correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| } |
| } |
| 348,12 → 355,15 |
| timer = DRIFTCORRECTION_TIME; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
| deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[28 + axis] = dynamicOffset[axis]; |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim; |
| DebugOut.Analog[28+axis] = driftComp[axis]; |
| correctionSum[axis] = 0; |
| } |
| } |
| 363,8 → 373,14 |
| * Main procedure. |
| ************************************************************************/ |
| void calculateFlightAttitude(void) { |
| // part1: 550 usec. |
| // part1a: 550 usec. |
| // part1b: 60 usec. |
| getAnalogData(); |
| // end part1b |
| integrate(); |
| // end part1a |
| DebugOut.Analog[6] = ACRate[PITCH]; |
| DebugOut.Analog[7] = ACRate[ROLL]; |
| 378,60 → 394,63 |
| correctIntegralsByAcc0thOrder(); |
| driftCorrection(); |
| #endif |
| // end part1 |
| } |
| /* |
| void updateCompass(void) { |
| void updateCompass(void) { |
| int16_t w, v, r,correction, error; |
| if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| setCompassCalState(); |
| if (controlMixer_testCompassCalState()) { |
| compassCalState++; |
| if(compassCalState < 5) beepNumber(compassCalState); |
| else beep(1000); |
| } |
| } else { |
| // get maximum attitude angle |
| w = abs(pitchAngle / 512); |
| v = abs(rollAngle / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| if(abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| // get maximum attitude angle |
| w = abs(angle[PITCH] / 512); |
| v = abs(angle[ROLL] / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| if(abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| } |
| if(!badCompassHeading && w < 25) { |
| yawGyroDrift += error; |
| if(updateCompassCourse) { |
| beep(200); |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| updateCompassCourse = 0; |
| } |
| } |
| yawGyroHeading += (error * 8) / correction; |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| if(w >= 0) { |
| if(!badCompassHeading) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| // calc course deviation |
| r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| yawAngleDiff += v; |
| } |
| else |
| { // wait a while |
| badCompassHeading--; |
| } |
| } else { // ignore compass at extreme attitudes for a while |
| badCompassHeading = 500; |
| } |
| } |
| if(!badCompassHeading && w < 25) { |
| if(updateCompassCourse) { |
| beep(200); |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| updateCompassCourse = 0; |
| } |
| } |
| yawGyroHeading += (error * 8) / correction; |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| if(w >= 0) { |
| if(!badCompassHeading) { |
| v = 64 + (maxControlPitch + maxControlRoll) / 8; |
| // calc course deviation |
| r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| yawAngle += v; |
| } |
| else |
| { // wait a while |
| badCompassHeading--; |
| } |
| } |
| else { // ignore compass at extreme attitudes for a while |
| badCompassHeading = 500; |
| } |
| } |
| } |
| */ |
| } |
| /* |
| * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
| 457,13 → 476,13 |
| // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
| if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
| // manual mode |
| dynamicOffsetPitch = dynamicParams.UserParam7 - 128; |
| dynamicOffsetRoll = dynamicParams.UserParam8 - 128; |
| driftCompPitch = dynamicParams.UserParam7 - 128; |
| driftCompRoll = dynamicParams.UserParam8 - 128; |
| } else { |
| // use the sampled value (does not seem to work so well....) |
| dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
| dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
| dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount; |
| driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
| driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
| driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
| } |
| // keep resetting these meanwhile, to avoid accumulating errors. |
| /branches/dongfang_FC_rewrite/attitude.h |
|---|
| 81,13 → 81,13 |
| /* |
| * Rotation rates |
| */ |
| extern int16_t rate_PID[2], yawRate; |
| extern int16_t rate_PID[2], rate_ATT[2], yawRate; |
| extern int16_t differential[2]; |
| /* |
| * Attitudes calculated by numerical integration of gyro rates |
| */ |
| extern int32_t angle[2], yawAngle; |
| extern int32_t angle[2], yawAngleDiff; |
| // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| 103,6 → 103,8 |
| extern uint16_t updateCompassCourse; |
| extern uint16_t badCompassHeading; |
| void updateCompass(void); |
| /* |
| * Interval wrap-over values for attitude integrals |
| */ |
| 124,6 → 126,11 |
| extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
| /* |
| * For NaviCtrl use. |
| */ |
| extern int16_t averageAcc[2], averageAccCount; |
| /* |
| * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
| * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
| */ |
| 135,6 → 142,8 |
| // void attitude_startDynamicCalibration(void); |
| // void attitude_continueDynamicCalibration(void); |
| int32_t getAngleEstimateFromAcc(uint8_t axis); |
| /* |
| * Main routine, called from the flight loop. |
| */ |
| /branches/dongfang_FC_rewrite/attitudeControl.c |
|---|
| 0,0 → 1,46 |
| #include <inttypes.h> |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "dongfangMath.h" |
| // For scope debugging only! |
| #include "rc.h" |
| // = cos^2(45 degs). |
| const int32_t MINPROJECTION = (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
| // Takes 380 - 400 usec. Way too slow. |
| // With static MINPROJECTION: 220 usec. |
| uint16_t AC_getThrottle(uint16_t throttle) { |
| int32_t projection; |
| // part1 start: 150 usec |
| // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
| projection = (int32_t)int_cos(angle[PITCH]) * (int32_t)int_cos(angle[ROLL]); |
| // part1 end. |
| uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
| int16_t deltaThrottle; |
| if (projection < MINPROJECTION && projection >= 0) { |
| // projection = MINPROJECTION; |
| deltaThrottle = 0; |
| } else if (projection >- MINPROJECTION && projection<0) { |
| // projection = -MINPROJECITON; |
| deltaThrottle = 0; |
| } else |
| /* |
| * We need delta throttle = constant/projection1 |
| * (constant * MATH_UNIT_FACTOR^2) / projection. |
| */ |
| deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
| // DebugOut.Analog[13] = deltaThrottle; |
| return throttle + deltaThrottle; |
| } |
| /* |
| har: R = e * k/p |
| vil R = e * ( 1 - k/p ) |
| = e - ek/p |
| */ |
| /branches/dongfang_FC_rewrite/attitudeControl.h |
|---|
| 0,0 → 1,5 |
| #ifndef _ATTITUDECONTROL_H |
| #define _ATTITUDECONTROL_H |
| #include <inttypes.h> |
| uint16_t AC_getThrottle(uint16_t throttle); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/ADXRS610_FC2.0.c |
|---|
| 0,0 → 1,18 |
| #include "ADXRS610_FC2.0.h" |
| #include "configuration.h" |
| const uint8_t GYRO_REVERSED[3] = {0,0,0}; |
| const uint8_t ACC_REVERSED[3] = {0,1,0}; |
| void gyro_calibrate(void) {} |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 5; |
| staticParams.DriftComp = 10; |
| staticParams.GyroAccFactor = 1; |
| staticParams.GyroAccTrim = 5; |
| // Not used. |
| staticParams.AngleTurnOverPitch = 78; |
| staticParams.AngleTurnOverRoll = 78; |
| } |
| /branches/dongfang_FC_rewrite/backup/ADXRS610_FC2.0.h |
|---|
| 0,0 → 1,28 |
| /* |
| #ifndef _ADXRS610_H |
| #define _ADXRS610_H |
| #include "sensors.h" |
| / * |
| * Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board. |
| * / |
| #define GYRO_HW_NAME "ADXR" |
| #define GYRO_HW_FACTOR 1.2288f |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 1.0f |
| / * |
| * Same for yaw. |
| * / |
| #define GYRO_YAW_CORRECTION 1.0f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/backup/ENC-03_FC1.3.c |
|---|
| 0,0 → 1,69 |
| //#include "ENC-03_FC1.3.h" |
| #include "printf_P.h" |
| #include "analog.h" |
| #include "twimaster.h" |
| #include "configuration.h" |
| #include "timer0.h" |
| #define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
| #define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
| const uint8_t GYRO_REVERSED[3] = {0,0,1}; |
| const uint8_t ACC_REVERSED[3] = {0,1,0}; |
| // void gyro_init(void) {} |
| void gyro_calibrate(void) { |
| uint8_t i, axis, factor, numberOfAxesInRange = 0; |
| uint16_t timeout; |
| // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
| timeout = SetDelay(2000); |
| for(i = 140; i != 0; i--) { |
| // If all 3 axis are in range, shorten the remaining number of iterations. |
| if(numberOfAxesInRange == 3 && i > 10) i = 9; |
| numberOfAxesInRange = 0; |
| for (axis=PITCH; axis<=YAW; axis++) { |
| if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW; |
| else factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
| if(rawGyroSum[axis] < 510 * factor) DACValues[axis]--; |
| else if(rawGyroSum[axis] > 515 * factor) DACValues[axis]++; |
| else numberOfAxesInRange++; |
| /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
| if(DACValues[axis] < 10) { |
| DACValues[axis] = 10; |
| } else if(DACValues[axis] > 245) { |
| DACValues[axis] = 245; |
| } |
| } |
| I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
| // Wait for I2C to finish transmission. |
| while(twi_state) { |
| // Did it take too long? |
| if(CheckDelay(timeout)) { |
| printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
| break; |
| } |
| } |
| analog_start(); |
| Delay_ms_Mess(i<10 ? 10 : 2); |
| } |
| Delay_ms_Mess(70); |
| } |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 3; |
| staticParams.DriftComp = 100; |
| staticParams.GyroAccFactor = 1; |
| staticParams.GyroAccTrim = 5; |
| // Not used. |
| staticParams.AngleTurnOverPitch = 85; |
| staticParams.AngleTurnOverRoll = 85; |
| } |
| /branches/dongfang_FC_rewrite/backup/ENC-03_FC1.3.h |
|---|
| 0,0 → 1,26 |
| /* |
| #ifndef _ENC03_FC13_H |
| #define _ENC03_FC13_H |
| #include "sensors.h" |
| / * |
| * Configuration for the ENC-03 gyros and oriented and wired on FC 1.3 (with DAC). |
| * / |
| #define GYRO_HW_NAME "ENC" |
| #define GYRO_HW_FACTOR 1.304f |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 1.11f |
| / * |
| * Same for yaw. |
| * / |
| #define GYRO_YAW_CORRECTION 1.28f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/backup/GPSControl.c |
|---|
| 0,0 → 1,8 |
| #include "GPSControl.h" |
| #include<inttypes.h> |
| int16_t GPSStickPitch, GPSStickRoll; |
| void GPS_setNeutral(void) { |
| GPSStickPitch = GPSStickRoll = 0; |
| } |
| /branches/dongfang_FC_rewrite/backup/GPSControl.h |
|---|
| 0,0 → 1,10 |
| #ifndef _GPSCONTROL_H |
| #define _GPSCONTROL_H |
| #include<inttypes.h> |
| extern int16_t GPSStickPitch, GPSStickRoll; |
| void GPS_setNeutral(void); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/H_and_I_AxisCoupling.txt |
|---|
| 0,0 → 1,76 |
| // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
| if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
| { |
| tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
| tmpl3 *= Parameter_AchsKopplung2; //65 |
| tmpl3 /= 4096L; |
| tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
| tmpl4 *= Parameter_AchsKopplung2; //65 |
| tmpl4 /= 4096L; |
| KopplungsteilNickRoll = tmpl3; |
| KopplungsteilRollNick = tmpl4; |
| tmpl4 -= tmpl3; |
| ErsatzKompass += tmpl4; |
| if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
| tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
| tmpl *= Parameter_AchsKopplung1; // 90 |
| tmpl /= 4096L; |
| tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
| tmpl2 *= Parameter_AchsKopplung1; |
| tmpl2 /= 4096L; |
| if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
| //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
| } |
| else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
| TrimRoll = tmpl - tmpl2 / 100L; |
| TrimNick = -tmpl2 + tmpl / 100L; |
| // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
| Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
| Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
| if(Mess_IntegralRoll > Umschlag180Roll) |
| { |
| Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
| Mess_IntegralRoll2 = Mess_IntegralRoll; |
| } |
| if(Mess_IntegralRoll <-Umschlag180Roll) |
| { |
| Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
| Mess_IntegralRoll2 = Mess_IntegralRoll; |
| } |
| // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
| Mess_IntegralNick2 += MesswertNick + TrimNick; |
| Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
| if(Mess_IntegralNick > Umschlag180Nick) |
| { |
| Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
| Mess_IntegralNick2 = Mess_IntegralNick; |
| } |
| if(Mess_IntegralNick <-Umschlag180Nick) |
| { |
| Mess_IntegralNick = (Umschlag180Nick - 25000L); |
| Mess_IntegralNick2 = Mess_IntegralNick; |
| } |
| .. |
| if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
| else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
| if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
| else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
| #define TRIM_MAX 200 |
| if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
| if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
| MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING); |
| MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING); |
| MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING)); |
| messwertNick er nu P-part.... |
| /branches/dongfang_FC_rewrite/backup/License.txt |
|---|
| 0,0 → 1,52 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 nichtkommerziellen 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 sonstigen 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-profit 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, our webpage (http://www.MikroKopter.de) must be |
| // + clearly linked and named 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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /branches/dongfang_FC_rewrite/backup/TODO-list.txt |
|---|
| 0,0 → 1,20 |
| - Check that acc. mode looping is still complete and "safe" to use. |
| - Find that pitch and roll rate limiter missing at least in HH mode. DONE |
| Reng. i badevaer. DONE |
| Reng. i sovevaer. DONE |
| Rydde op i vasketoej DONE |
| Mail til Phat DONE |
| SMS til Jonggil DONE |
| Skift servo paa fly DONE |
| Velo flicken DONE |
| Rep. CKF kopter DONE aber wieder |
| Montere LED lys paa kopter |
| Skrive GPS software |
| Jakke til rens |
| GPS hack? |
| Rydde op paa bord... |
| LED Flasher Problem beh vers. |
| Krankenkasse Anfrage |
| Still More Streamlined Steam and Early Diesels |
| /branches/dongfang_FC_rewrite/backup/analog.c |
|---|
| 0,0 → 1,558 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <avr/pgmspace.h> |
| #include "analog.h" |
| #include "sensors.h" |
| // for Delay functions |
| #include "timer0.h" |
| // For DebugOut |
| #include "uart0.h" |
| // For reading and writing acc. meter offsets. |
| #include "eeprom.h" |
| /* |
| * For each A/D conversion cycle, each analog channel is sampled a number of times |
| * (see array channelsForStates), and the results for each channel are summed. |
| * Here are those for the gyros and the acc. meters. They are not zero-offset. |
| * They are exported in the analog.h file - but please do not use them! The only |
| * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
| * the offsets with the DAC. |
| */ |
| volatile int16_t rawGyroSum[3]; |
| volatile int16_t acc[3]; |
| volatile int16_t filteredAcc[2]={0,0}; |
| /* |
| * These 4 exported variables are zero-offset. The "PID" ones are used |
| * in the attitude control as rotation rates. The "ATT" ones are for |
| * integration to angles. |
| */ |
| volatile int16_t gyro_PID[2]; |
| volatile int16_t gyro_ATT[2]; |
| volatile int16_t gyroD[2]; |
| volatile int16_t yawGyro; |
| /* |
| * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| * standing still. They are used for adjusting the gyro and acc. meter values |
| * to be centered on zero. |
| */ |
| volatile int16_t gyroOffset[3] = { |
| 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, |
| 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, |
| 512 * GYRO_SUMMATION_FACTOR_YAW |
| }; |
| volatile int16_t accOffset[3] = { |
| 512 * ACC_SUMMATION_FACTOR_PITCHROLL, |
| 512 * ACC_SUMMATION_FACTOR_PITCHROLL, |
| 512 * ACC_SUMMATION_FACTOR_Z |
| }; |
| /* |
| * This allows some experimentation with the gyro filters. |
| * Should be replaced by #define's later on... |
| */ |
| volatile uint8_t GYROS_PID_FILTER; |
| volatile uint8_t GYROS_ATT_FILTER; |
| volatile uint8_t GYROS_D_FILTER; |
| volatile uint8_t ACC_FILTER; |
| /* |
| * Air pressure |
| */ |
| volatile uint8_t rangewidth = 106; |
| // Direct from sensor, irrespective of range. |
| // volatile uint16_t rawAirPressure; |
| // Value of 2 samples, with range. |
| volatile uint16_t simpleAirPressure; |
| // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
| volatile int32_t filteredAirPressure; |
| // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
| volatile int32_t airPressureSum; |
| // The number of samples summed into airPressureSum so far. |
| volatile uint8_t pressureMeasurementCount; |
| /* |
| * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
| * That is divided by 3 below, for a final 10.34 per volt. |
| * So the initial value of 100 is for 9.7 volts. |
| */ |
| volatile int16_t UBat = 100; |
| /* |
| * Control and status. |
| */ |
| volatile uint16_t ADCycleCount = 0; |
| volatile uint8_t analogDataReady = 1; |
| /* |
| * Experiment: Measuring vibration-induced sensor noise. |
| */ |
| volatile uint16_t gyroNoisePeak[2]; |
| volatile uint16_t accNoisePeak[2]; |
| // ADC channels |
| #define AD_GYRO_YAW 0 |
| #define AD_GYRO_ROLL 1 |
| #define AD_GYRO_PITCH 2 |
| #define AD_AIRPRESSURE 3 |
| #define AD_UBAT 4 |
| #define AD_ACC_Z 5 |
| #define AD_ACC_ROLL 6 |
| #define AD_ACC_PITCH 7 |
| /* |
| * Table of AD converter inputs for each state. |
| * The number of samples summed for each channel is equal to |
| * the number of times the channel appears in the array. |
| * The max. number of samples that can be taken in 2 ms is: |
| * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
| * loop needs a little time between reading AD values and |
| * re-enabling ADC, the real limit is (how much?) lower. |
| * The acc. sensor is sampled even if not used - or installed |
| * at all. The cost is not significant. |
| */ |
| const uint8_t channelsForStates[] PROGMEM = { |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, |
| AD_ACC_PITCH, |
| AD_ACC_ROLL, |
| AD_AIRPRESSURE, |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_ACC_Z, // at 8, measure Z acc. |
| AD_GYRO_PITCH, |
| AD_GYRO_ROLL, |
| AD_GYRO_YAW, // at 11, finish yaw gyro |
| AD_ACC_PITCH, // at 12, finish pitch axis acc. |
| AD_ACC_ROLL, // at 13, finish roll axis acc. |
| AD_AIRPRESSURE, // at 14, finish air pressure. |
| AD_GYRO_PITCH, // at 15, finish pitch gyro |
| AD_GYRO_ROLL, // at 16, finish roll gyro |
| AD_UBAT // at 17, measure battery. |
| }; |
| // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
| // uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
| void analog_init(void) { |
| uint8_t sreg = SREG; |
| // disable all interrupts before reconfiguration |
| cli(); |
| //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
| DDRA = 0x00; |
| PORTA = 0x00; |
| // Digital Input Disable Register 0 |
| // Disable digital input buffer for analog adc_channel pins |
| DIDR0 = 0xFF; |
| // external reference, adjust data to the right |
| ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR)); |
| // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
| ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH; |
| //Set ADC Control and Status Register A |
| //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
| ADCSRA = (0<<ADEN)|(0<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(0<<ADIE); |
| //Set ADC Control and Status Register B |
| //Trigger Source to Free Running Mode |
| ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
| // Start AD conversion |
| analog_start(); |
| // restore global interrupt flags |
| SREG = sreg; |
| } |
| void measureNoise(const int16_t sensor, volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
| if (sensor > (int16_t)(*noiseMeasurement)) { |
| *noiseMeasurement = sensor; |
| } else if (-sensor > (int16_t)(*noiseMeasurement)) { |
| *noiseMeasurement = -sensor; |
| } else if (*noiseMeasurement > damping) { |
| *noiseMeasurement -= damping; |
| } else { |
| *noiseMeasurement = 0; |
| } |
| } |
| uint16_t getSimplePressure(int advalue) { |
| return (uint16_t)OCR0A * (uint16_t)rangewidth + advalue; |
| } |
| /***************************************************** |
| * Interrupt Service Routine for ADC |
| * Runs at 312.5 kHz or 3.2 µs. When all states are |
| * processed the interrupt is disabled and further |
| * AD conversions are stopped. |
| *****************************************************/ |
| ISR(ADC_vect) { |
| static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
| static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0}; |
| static uint16_t pressureAutorangingWait = 25; |
| uint16_t rawAirPressure; |
| uint8_t i, axis; |
| int16_t newrange; |
| // for various filters... |
| int16_t tempOffsetGyro, tempGyro; |
| sensorInputs[ad_channel] += ADC; |
| /* |
| * Actually we don't need this "switch". We could do all the sampling into the |
| * sensorInputs array first, and all the processing after the last sample. |
| */ |
| switch(state++) { |
| case 8: // Z acc |
| if (ACC_REVERSED[Z]) |
| acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
| else |
| acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
| break; |
| case 11: // yaw gyro |
| rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
| if (GYRO_REVERSED[YAW]) |
| yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
| else |
| yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
| break; |
| case 12: // pitch axis acc. |
| if (ACC_REVERSED[PITCH]) |
| acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH]; |
| else |
| acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH]; |
| filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER; |
| measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1); |
| break; |
| case 13: // roll axis acc. |
| if (ACC_REVERSED[ROLL]) |
| acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL]; |
| else |
| acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL]; |
| filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER; |
| measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1); |
| break; |
| case 14: // air pressure |
| if (pressureAutorangingWait) { |
| //A range switch was done recently. Wait for steadying. |
| pressureAutorangingWait--; |
| break; |
| } |
| rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
| if (rawAirPressure < MIN_RAWPRESSURE) { |
| // value is too low, so decrease voltage on the op amp minus input, making the value higher. |
| newrange = OCR0A - (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1; |
| if (newrange > MIN_RANGES_EXTRAPOLATION) { |
| pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; |
| OCR0A = newrange; |
| } else { |
| if (OCR0A) { |
| OCR0A--; |
| pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
| } |
| } |
| } else if (rawAirPressure > MAX_RAWPRESSURE) { |
| // value is too high, so increase voltage on the op amp minus input, making the value lower. |
| // If near the end, make a limited increase |
| newrange = OCR0A + (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1; |
| if (newrange < MAX_RANGES_EXTRAPOLATION) { |
| pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
| OCR0A = newrange; |
| } else { |
| if (OCR0A<254) { |
| OCR0A++; |
| pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
| } |
| } |
| } |
| // Even if the sample is off-range, use it. |
| simpleAirPressure = getSimplePressure(rawAirPressure); |
| if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
| // Danger: pressure near lower end of range. If the measurement saturates, the |
| // copter may climb uncontrolled... Simulate a drastic reduction in pressure. |
| airPressureSum += (int16_t)MIN_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MIN_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
| } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
| // Danger: pressure near upper end of range. If the measurement saturates, the |
| // copter may fall uncontrolled... Simulate a drastic increase in pressure. |
| airPressureSum += (int16_t)MAX_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MAX_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
| } else { |
| // normal case. |
| airPressureSum += simpleAirPressure; |
| } |
| // 2 samples were added. |
| pressureMeasurementCount += 2; |
| if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR) { |
| filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER-1) + airPressureSum + AIRPRESSURE_FILTER/2) / AIRPRESSURE_FILTER; |
| pressureMeasurementCount = airPressureSum = 0; |
| } |
| // DebugOut.Analog[14] = OCR0A; |
| // DebugOut.Analog[15] = simpleAirPressure; |
| DebugOut.Analog[11] = UBat; |
| break; |
| case 15: |
| case 16: // pitch or roll gyro. |
| axis = state - 16; |
| tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
| // DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
| /* |
| * Process the gyro data for the PID controller. |
| */ |
| // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
| // gyro with a wider range, and helps counter saturation at full control. |
| if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
| if (tempGyro < SENSOR_MIN_PITCHROLL) { |
| tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
| } |
| else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
| tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
| } |
| } |
| // 2) Apply sign and offset, scale before filtering. |
| if (GYRO_REVERSED[axis]) { |
| tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
| } else { |
| tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
| } |
| // 3) Scale and filter. |
| tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER; |
| // 4) Measure noise. |
| measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
| // 5) Differential measurement. |
| gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER; |
| // 6) Done. |
| gyro_PID[axis] = tempOffsetGyro; |
| /* |
| * Now process the data for attitude angles. |
| */ |
| tempGyro = rawGyroSum[axis]; |
| // 1) Apply sign and offset, scale before filtering. |
| if (GYRO_REVERSED[axis]) { |
| tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
| } else { |
| tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
| } |
| // 2) Filter. |
| gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER; |
| break; |
| case 17: |
| // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
| // This is divided by 3 --> 10.34 counts per volt. |
| UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
| analogDataReady = 1; // mark |
| ADCycleCount++; |
| // Stop the sampling. Cycle is over. |
| state = 0; |
| for (i=0; i<8; i++) { |
| sensorInputs[i] = 0; |
| } |
| break; |
| default: {} // do nothing. |
| } |
| // set up for next state. |
| ad_channel = pgm_read_byte(&channelsForStates[state]); |
| // ad_channel = channelsForStates[state]; |
| // set adc muxer to next ad_channel |
| ADMUX = (ADMUX & 0xE0) | ad_channel; |
| // after full cycle stop further interrupts |
| if(state) analog_start(); |
| } |
| void analog_calibrate(void) { |
| #define GYRO_OFFSET_CYCLES 10 |
| uint8_t i, axis; |
| int32_t deltaOffsets[3] = {0,0,0}; |
| // Set the filters... to be removed again, once some good settings are found. |
| GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1; |
| GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1; |
| GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
| ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
| gyro_calibrate(); |
| // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
| for(i=0; i < GYRO_OFFSET_CYCLES; i++) { |
| Delay_ms_Mess(20); |
| for (axis=PITCH; axis<=YAW; axis++) { |
| deltaOffsets[axis] += rawGyroSum[axis]; |
| } |
| } |
| for (axis=PITCH; axis<=YAW; axis++) { |
| gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES; |
| DebugOut.Analog[20+axis] = gyroOffset[axis]; |
| } |
| // Noise is relative to offset. So, reset noise measurements when changing offsets. |
| gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
| accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
| accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
| accOffset[Z] = GetParamWord(PID_ACC_Z); |
| // Rough estimate. Hmm no nothing happens at calibration anyway. |
| // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
| // pressureMeasurementCount = 0; |
| // Experiment! |
| // filteredAirPressureOffset = filteredAirPressure - 1000L; |
| Delay_ms_Mess(100); |
| } |
| /* |
| * Find acc. offsets for a neutral reading, and write them to EEPROM. |
| * Does not (!} update the local variables. This must be done with a |
| * call to analog_calibrate() - this always (?) is done by the caller |
| * anyway. There would be nothing wrong with updating the variables |
| * directly from here, though. |
| */ |
| void analog_calibrateAcc(void) { |
| #define ACC_OFFSET_CYCLES 10 |
| uint8_t i, axis; |
| int32_t deltaOffset[3] = {0,0,0}; |
| int16_t filteredDelta; |
| // int16_t pressureDiff, savedRawAirPressure; |
| for(i=0; i < ACC_OFFSET_CYCLES; i++) { |
| Delay_ms_Mess(10); |
| for (axis=PITCH; axis<=YAW; axis++) { |
| deltaOffset[axis] += acc[axis]; |
| } |
| } |
| for (axis=PITCH; axis<=YAW; axis++) { |
| filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
| accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
| } |
| // Save ACC neutral settings to eeprom |
| SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
| SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
| SetParamWord(PID_ACC_Z, accOffset[Z]); |
| // Noise is relative to offset. So, reset noise measurements when |
| // changing offsets. |
| accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
| // Setting offset values has an influence in the analog.c ISR |
| // Therefore run measurement for 100ms to achive stable readings |
| Delay_ms_Mess(100); |
| // Set the feedback so that air pressure ends up in the middle of the range. |
| // (raw pressure high --> OCR0A also high...) |
| // OCR0A += (rawAirPressure - 512) / rangewidth; |
| // Delay_ms_Mess(500); |
| /* |
| pressureDiff = 0; |
| // DebugOut.Analog[16] = rawAirPressure; |
| #define PRESSURE_CAL_CYCLE_COUNT 2 |
| for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
| savedRawAirPressure = rawAirPressure; |
| OCR0A++; |
| Delay_ms_Mess(200); |
| // raw pressure will decrease. |
| pressureDiff += (savedRawAirPressure - rawAirPressure); |
| savedRawAirPressure = rawAirPressure; |
| OCR0A--; |
| Delay_ms_Mess(200); |
| // raw pressure will increase. |
| pressureDiff += (rawAirPressure - savedRawAirPressure); |
| } |
| // DebugOut.Analog[16] = |
| rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2); |
| */ |
| } |
| /branches/dongfang_FC_rewrite/backup/analog.h |
|---|
| 0,0 → 1,260 |
| #ifndef _ANALOG_H |
| #define _ANALOG_H |
| #include <inttypes.h> |
| //#include "invenSense.h" |
| //#include "ENC-03_FC1.3.h" |
| //#include "ADXRS610_FC2.0.h" |
| /* |
| * How much low pass filtering to apply for gyro_PID. |
| * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
| * Temporarily replaced by userparam-configurable variable. |
| */ |
| // #define GYROS_PID_FILTER 1 |
| /* |
| * How much low pass filtering to apply for gyro_ATT. |
| * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
| * Temporarily replaced by userparam-configurable variable. |
| */ |
| // #define GYROS_ATT_FILTER 1 |
| // Temporarily replaced by userparam-configurable variable. |
| // #define ACC_FILTER 4 |
| /* |
| About setting constants for different gyros: |
| Main parameters are positive directions and voltage/angular speed gain. |
| The "Positive direction" is the rotation direction around an axis where |
| the corresponding gyro outputs a voltage > the no-rotation voltage. |
| A gyro is considered, in this code, to be "forward" if its positive |
| direction is: |
| - Nose down for pitch |
| - Left hand side down for roll |
| - Clockwise seen from above for yaw. |
| Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
| GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
| Setting gyro gain correctly: All sensor measurements in analog.c take |
| place in a cycle, each cycle comprising all sensors. Some sensors are |
| sampled more than ones, and the results added. The pitch and roll gyros |
| are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
| code. |
| In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
| or 4 (other versions), offset to zero, low pass filtered and then assigned |
| to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
| roll. |
| So: |
| gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
| where V is 2 for FC1.0 and 4 for all others. |
| Assuming constant ADCValue, in the H&I code: |
| gyro = I * ADCValue. |
| where I is 8 for FC1.0 and 16 for all others. |
| The relation between rotation rate and ADCValue: |
| ADCValue [units] = |
| rotational speed [deg/s] * |
| gyro sensitivity [V / deg/s] * |
| amplifier gain [units] * |
| 1024 [units] / |
| 3V full range [V] |
| or: H is the number of steps the ADC value changes with, |
| for a 1 deg/s change in rotational velocity: |
| H = ADCValue [units] / rotation rate [deg/s] = |
| gyro sensitivity [V / deg/s] * |
| amplifier gain [units] * |
| 1024 [units] / |
| 3V full range [V] |
| Examples: |
| FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
| H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
| FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
| H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
| My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
| H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
| (only about half as sensitive as V1.3. But it will take about twice the |
| rotation rate!) |
| All together: gyro = I * H * rotation rate [units / (deg/s)]. |
| */ |
| /* |
| * A factor that the raw gyro values are multiplied by, |
| * before being filtered and passed to the attitude module. |
| * A value of 1 would cause a little bit of loss of precision in the |
| * filtering (on the other hand the values are so noisy in flight that |
| * it will not really matter - but when testing on the desk it might be |
| * noticeable). 4 is fine for the default filtering. |
| * Experiment: Set it to 1. |
| */ |
| #define GYRO_FACTOR_PITCHROLL 4 |
| /* |
| * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
| * respectively. This is = the number of occurences of each channel in the |
| * channelsForStates array in analog.c. |
| */ |
| #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
| #define GYRO_SUMMATION_FACTOR_YAW 2 |
| #define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
| #define ACC_SUMMATION_FACTOR_Z 1 |
| /* |
| Integration: |
| The HiResXXXX values are divided by 8 (in H&I firmware) before integration. |
| In the Killagreg rewrite of the H&I firmware, the factor 8 is called |
| HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR, |
| and care has been taken that all other constants (gyro to degree factor, and |
| 180 degree flip-over detection limits) are corrected to it. Because the |
| division by the constant takes place in the flight attitude code, the |
| constant is there. |
| The control loop executes every 2ms, and for each iteration |
| gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
| Assuming a constant rotation rate v and a zero initial gyroIntegral |
| (for this explanation), we get: |
| gyroIntegral = |
| N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
| N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
| where N is the number of summations; N = t/2ms. |
| For one degree of rotation: t*v = 1: |
| gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
| This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
| Examples: |
| FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
| FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
| My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
| */ |
| /* |
| * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
| * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
| */ |
| #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
| #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
| /* |
| * Gyro saturation prevention. |
| */ |
| // How far from the end of its range a gyro is considered near-saturated. |
| #define SENSOR_MIN_PITCHROLL 32 |
| // Other end of the range (calculated) |
| #define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
| // Max. boost to add "virtually" to gyro signal at total saturation. |
| #define EXTRAPOLATION_LIMIT 2500 |
| // Slope of the boost (calculated) |
| #define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
| /* |
| * This value is subtracted from the gyro noise measurement in each iteration, |
| * making it return towards zero. |
| */ |
| #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
| #define PITCH 0 |
| #define ROLL 1 |
| #define YAW 2 |
| #define Z 2 |
| /* |
| * The values that this module outputs |
| * These first 2 exported arrays are zero-offset. The "PID" ones are used |
| * in the attitude control as rotation rates. The "ATT" ones are for |
| * integration to angles. For the same axis, the PID and ATT variables |
| * generally have about the same values. There are just some differences |
| * in filtering, and when a gyro becomes near saturated. |
| * Maybe this distinction is not really necessary. |
| */ |
| extern volatile int16_t gyro_PID[2]; |
| extern volatile int16_t gyro_ATT[2]; |
| extern volatile int16_t gyroD[2]; |
| extern volatile int16_t yawGyro; |
| extern volatile uint16_t ADCycleCount; |
| extern volatile int16_t UBat; |
| // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
| #define UBAT_AT_5V (int16_t)((5.0 / 3.0) * (1.0/11.0) / (3.0 * 1024)) |
| /* |
| * This is not really for external use - but the ENC-03 gyro modules needs it. |
| */ |
| extern volatile int16_t rawGyroSum[3]; |
| /* |
| * The acceleration values that this module outputs. They are zero based. |
| */ |
| extern volatile int16_t acc[3]; |
| extern volatile int16_t filteredAcc[2]; |
| /* |
| * Diagnostics: Gyro noise level because of motor vibrations. The variables |
| * only really reflect the noise level when the copter stands still but with |
| * its motors running. |
| */ |
| extern volatile uint16_t gyroNoisePeak[2]; |
| extern volatile uint16_t accNoisePeak[2]; |
| /* |
| * Air pressure. |
| * The sensor has a sensitivity of 46 mV/kPa. |
| * An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 1195 * 10^-6 * h |
| * |
| */ |
| #define AIRPRESSURE_SUMMATION_FACTOR 14 |
| #define AIRPRESSURE_FILTER 8 |
| // Minimum A/D value before a range change is performed. |
| #define MIN_RAWPRESSURE (200 * 2) |
| // Maximum A/D value before a range change is performed. |
| #define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE) |
| #define MIN_RANGES_EXTRAPOLATION 10 |
| #define MAX_RANGES_EXTRAPOLATION 250 |
| #define PRESSURE_EXTRAPOLATION_COEFF 25L |
| #define AUTORANGE_WAIT_FACTOR 1 |
| extern volatile uint16_t simpleAirPressure; |
| extern volatile int32_t filteredAirPressure; |
| /* |
| * At saturation, the filteredAirPressure may actually be (simulated) negative. |
| */ |
| extern volatile int32_t filteredAirPressure; |
| /* |
| * Flag: Interrupt handler has done all A/D conversion and processing. |
| */ |
| extern volatile uint8_t analogDataReady; |
| void analog_init(void); |
| // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
| #define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
| // set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
| #define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
| /* |
| * "Warm" calibration: Zero-offset gyros. |
| */ |
| void analog_calibrate(void); |
| /* |
| * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
| */ |
| void analog_calibrateAcc(void); |
| #endif //_ANALOG_H |
| /branches/dongfang_FC_rewrite/backup/attitude.c |
|---|
| 0,0 → 1,483 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt 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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /************************************************************************/ |
| /* Flight Attitude */ |
| /************************************************************************/ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| // For scope debugging only! |
| #include "rc.h" |
| // where our main data flow comes from. |
| #include "analog.h" |
| #include "configuration.h" |
| #include "output.h" |
| // Some calculations are performed depending on some stick related things. |
| #include "controlMixer.h" |
| // For Servo_On / Off |
| // #include "timer2.h" |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #include "gps.h" |
| #endif |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| /* |
| * Gyro readings, as read from the analog module. It would have been nice to flow |
| * them around between the different calculations as a struct or array (doing |
| * things functionally without side effects) but this is shorter and probably |
| * faster too. |
| * The variables are overwritten at each attitude calculation invocation - the values |
| * are not preserved or reused. |
| */ |
| int16_t rate_ATT[2], yawRate; |
| // With different (less) filtering |
| int16_t rate_PID[2]; |
| int16_t differential[2]; |
| /* |
| * Gyro readings, after performing "axis coupling" - that is, the transfomation |
| * of rotation rates from the airframe-local coordinate system to a ground-fixed |
| * coordinate system. If axis copling is disabled, the gyro readings will be |
| * copied into these directly. |
| * These are global for the same pragmatic reason as with the gyro readings. |
| * The variables are overwritten at each attitude calculation invocation - the values |
| * are not preserved or reused. |
| */ |
| int16_t ACRate[2], ACYawRate; |
| /* |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. |
| */ |
| int32_t angle[2], yawAngleDiff; |
| int readingHeight = 0; |
| // compass course |
| int16_t compassHeading = -1; // negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| int16_t compassOffCourse = 0; |
| uint16_t updateCompassCourse = 0; |
| uint8_t compassCalState = 0; |
| uint16_t badCompassHeading = 500; |
| int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t yawGyroDrift; |
| #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| int16_t correctionSum[2] = {0,0}; |
| // For NaviCTRL use. |
| int16_t averageAcc[2] = {0,0}, averageAccCount = 0; |
| /* |
| * Experiment: Compensating for dynamic-induced gyro biasing. |
| */ |
| int16_t driftComp[2] = {0,0}, driftCompYaw = 0; |
| // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
| // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
| // int16_t dynamicCalCount; |
| /************************************************************************ |
| * Set inclination angles from the acc. sensor data. |
| * If acc. sensors are not used, set to zero. |
| * TODO: One could use inverse sine to calculate the angles more |
| * accurately, but since: 1) the angles are rather small at times when |
| * it makes sense to set the integrals (standing on ground, or flying at |
| * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| * it is hardly worth the trouble. |
| ************************************************************************/ |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
| } |
| void setStaticAttitudeAngles(void) { |
| #ifdef ATTITUDE_USE_ACC_SENSORS |
| angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
| angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
| #else |
| angle[PITCH] = angle[ROLL] = 0; |
| #endif |
| } |
| /************************************************************************ |
| * Neutral Readings |
| ************************************************************************/ |
| void attitude_setNeutral(void) { |
| // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
| dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
| driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
| correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| // Calibrate hardware. |
| analog_calibrate(); |
| // reset gyro readings |
| // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| yawAngleDiff = 0; |
| // update compass course to current heading |
| compassCourse = compassHeading; |
| // Inititialize YawGyroIntegral value with current compass heading |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| // Servo_On(); //enable servo output |
| } |
| /************************************************************************ |
| * Get sensor data from the analog module, and release the ADC |
| * TODO: Ultimately, the analog module could do this (instead of dumping |
| * the values into variables). |
| * The rate variable end up in a range of about [-1024, 1023]. |
| *************************************************************************/ |
| void getAnalogData(void) { |
| uint8_t axis; |
| for (axis=PITCH; axis <=ROLL; axis++) { |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| averageAccCount++; |
| yawRate = yawGyro + driftCompYaw; |
| // We are done reading variables from the analog module. |
| // Interrupt-driven sensor reading may restart. |
| analogDataReady = 0; |
| analog_start(); |
| } |
| /* |
| * This is the standard flight-style coordinate system transformation |
| * (from airframe-local axes to a ground-based system). For some reason |
| * the MK uses a left-hand coordinate system. The tranformation has been |
| * changed accordingly. |
| */ |
| void trigAxisCoupling(void) { |
| int16_t cospitch = int_cos(angle[PITCH]); |
| int16_t cosroll = int_cos(angle[ROLL]); |
| int16_t sinroll = int_sin(angle[ROLL]); |
| int16_t tanpitch = int_tan(angle[PITCH]); |
| #define ANTIOVF 512 |
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| } |
| // 480 usec with axis coupling - almost no time without. |
| void integrate(void) { |
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
| // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
| trigAxisCoupling(); |
| } else { |
| ACRate[PITCH] = rate_ATT[PITCH]; |
| ACRate[ROLL] = rate_ATT[ROLL]; |
| ACYawRate = yawRate; |
| } |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| */ |
| yawGyroHeading += ACYawRate; |
| yawAngleDiff += yawRate; |
| if(yawGyroHeading >= YAWOVER360) { |
| yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| } else if(yawGyroHeading < 0) { |
| yawGyroHeading += YAWOVER360; |
| } |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| angle[axis] += ACRate[axis]; |
| if(angle[axis] > PITCHROLLOVER180) { |
| angle[axis] -= PITCHROLLOVER360; |
| } else if (angle[axis] <= -PITCHROLLOVER180) { |
| angle[axis] += PITCHROLLOVER360; |
| } |
| } |
| } |
| /************************************************************************ |
| * A kind of 0'th order integral correction, that corrects the integrals |
| * directly. This is the "gyroAccFactor" stuff in the original code. |
| * There is (there) also a drift compensation |
| * - it corrects the differential of the integral = the gyro offsets. |
| * That should only be necessary with drifty gyros like ENC-03. |
| ************************************************************************/ |
| void correctIntegralsByAcc0thOrder(void) { |
| // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| // are less than ....., or reintroduce Kalman. |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t correction; |
| if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived; |
| if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if(abs(controlYaw) > 25) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| DebugOut.Analog[18 + axis] = getAngleEstimateFromAcc(axis); |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| DebugOut.Analog[14+axis] = permilleAcc; |
| DebugOut.Analog[16+axis] = angle[axis] - correction; |
| } |
| // DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| } |
| } |
| /************************************************************************ |
| * This is an attempt to correct not the error in the angle integrals |
| * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
| * cause of it: Gyro drift, vibration and rounding errors. |
| * All the corrections made in correctIntegralsByAcc0thOrder over |
| * DRIFTCORRECTION_TIME cycles are summed up. This number is |
| * then divided by DRIFTCORRECTION_TIME to get the approx. |
| * correction that should have been applied to each iteration to fix |
| * the error. This is then added to the dynamic offsets. |
| ************************************************************************/ |
| // 2 times / sec. = 488/2 |
| #define DRIFTCORRECTION_TIME 256L |
| void driftCorrection(void) { |
| static int16_t timer = DRIFTCORRECTION_TIME; |
| int16_t deltaCorrection; |
| uint8_t axis; |
| if (! --timer) { |
| timer = DRIFTCORRECTION_TIME; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[28 + axis] = driftComp[axis]; |
| correctionSum[axis] = 0; |
| } |
| } |
| } |
| /************************************************************************ |
| * Main procedure. |
| ************************************************************************/ |
| void calculateFlightAttitude(void) { |
| // part1: 550 usec. |
| // part1a: 550 usec. |
| // part1b: 60 usec. |
| getAnalogData(); |
| // end part1b |
| integrate(); |
| // end part1a |
| DebugOut.Analog[6] = ACRate[PITCH]; |
| DebugOut.Analog[7] = ACRate[ROLL]; |
| DebugOut.Analog[8] = ACYawRate; |
| DebugOut.Analog[3] = rate_PID[PITCH]; |
| DebugOut.Analog[4] = rate_PID[ROLL]; |
| DebugOut.Analog[5] = yawRate; |
| #ifdef ATTITUDE_USE_ACC_SENSORS |
| correctIntegralsByAcc0thOrder(); |
| driftCorrection(); |
| #endif |
| // end part1 |
| } |
| void updateCompass(void) { |
| int16_t w, v, r,correction, error; |
| if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (controlMixer_testCompassCalState()) { |
| compassCalState++; |
| if(compassCalState < 5) beepNumber(compassCalState); |
| else beep(1000); |
| } |
| } else { |
| // get maximum attitude angle |
| w = abs(angle[PITCH] / 512); |
| v = abs(angle[ROLL] / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| if(abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| } |
| if(!badCompassHeading && w < 25) { |
| yawGyroDrift += error; |
| if(updateCompassCourse) { |
| beep(200); |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| updateCompassCourse = 0; |
| } |
| } |
| yawGyroHeading += (error * 8) / correction; |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| if(w >= 0) { |
| if(!badCompassHeading) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| // calc course deviation |
| r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| yawAngleDiff += v; |
| } |
| else |
| { // wait a while |
| badCompassHeading--; |
| } |
| } else { // ignore compass at extreme attitudes for a while |
| badCompassHeading = 500; |
| } |
| } |
| } |
| /* |
| * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
| * and to compensate them away. It brings about some improvement, but no miracles. |
| * As long as the left stick is kept in the start-motors position, the dynamic compensation |
| * will measure the effect of vibration, to use for later compensation. So, one should keep |
| * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
| * speed unfortunately... must find a better way) |
| */ |
| /* |
| void attitude_startDynamicCalibration(void) { |
| dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
| savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
| } |
| void attitude_continueDynamicCalibration(void) { |
| // measure dynamic offset now... |
| dynamicCalPitch += hiResPitchGyro; |
| dynamicCalRoll += hiResRollGyro; |
| dynamicCalYaw += rawYawGyroSum; |
| dynamicCalCount++; |
| // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
| if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
| // manual mode |
| driftCompPitch = dynamicParams.UserParam7 - 128; |
| driftCompRoll = dynamicParams.UserParam8 - 128; |
| } else { |
| // use the sampled value (does not seem to work so well....) |
| driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
| driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
| driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
| } |
| // keep resetting these meanwhile, to avoid accumulating errors. |
| setStaticAttitudeIntegrals(); |
| yawAngle = 0; |
| } |
| */ |
| /branches/dongfang_FC_rewrite/backup/attitude.h |
|---|
| 0,0 → 1,149 |
| /*********************************************************************************/ |
| /* Attitude sense system (processing of gyro, accelerometer and altimeter data) */ |
| /*********************************************************************************/ |
| #ifndef _ATTITUDE_H |
| #define _ATTITUDE_H |
| #include <inttypes.h> |
| #include "analog.h" |
| // For debugging only. |
| #include "uart0.h" |
| /* |
| * If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
| * acc. sensor to be ignored at attitude calibration. |
| */ |
| #define ATTITUDE_USE_ACC_SENSORS yes |
| /* |
| * Default hysteresis to use for the -180 degree to 180 degree wrap. |
| */ |
| #define PITCHOVER_HYSTERESIS 0L |
| #define ROLLOVER_HYSTERESIS 0L |
| /* |
| * The frequency at which numerical integration takes place. 488 in original code. |
| */ |
| #define INTEGRATION_FREQUENCY 488 |
| /* |
| * Gyro readings are divided by this before being used in attitude control. This will scale them |
| * to match the scale of the stick control etc. variables. This is just a rough non-precision |
| * scaling - the below definitions make precise conversion factors. |
| * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
| * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
| */ |
| #define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
| /* |
| * Constant for deriving an attitude angle from acceleration measurement. |
| * |
| * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
| * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
| * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
| * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
| * In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
| * The sine of v is the far side / the hypothenusis: |
| * sin v = acc / sqrt(acc^2 + acc_z^2) |
| * Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
| * sin v ~= acc / acc_z |
| * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
| * and sin v ~= v (small angles, in radians): |
| * sin v ~= acc / 410 |
| * v / 57.3 ~= acc / 410 |
| * v ~= acc * 57.3 / 410 |
| * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
| * |
| * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
| */ |
| #define DEG_ACC_FACTOR 7 |
| /* |
| * Growth of the integral per degree: |
| * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
| * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
| * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
| */ |
| #define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
| #define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
| /* |
| * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
| * = the factor an acc. sensor should be multiplied by to get the gyro integral |
| * value for the same (small) angle. |
| * The value is about 200. |
| */ |
| #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| /* |
| * Rotation rates |
| */ |
| extern int16_t rate_PID[2], rate_ATT[2], yawRate; |
| extern int16_t differential[2]; |
| /* |
| * Attitudes calculated by numerical integration of gyro rates |
| */ |
| extern int32_t angle[2], yawAngleDiff; |
| // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| /* |
| * Compass navigation |
| */ |
| extern int16_t compassHeading; |
| extern int16_t compassCourse; |
| extern int16_t compassOffCourse; |
| extern uint8_t compassCalState; |
| extern int32_t yawGyroHeading; |
| extern int16_t yawGyroHeadingInDeg; |
| extern uint16_t updateCompassCourse; |
| extern uint16_t badCompassHeading; |
| /* |
| * Interval wrap-over values for attitude integrals |
| */ |
| extern long turnOver180Pitch, turnOver180Roll; |
| // No longer used. |
| // extern uint8_t FunnelCourse; |
| /* |
| * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
| * to help cancelling out drift and vibration noise effects. The dynamic offsets themselves |
| * can be updated in flight by different ways, for example: |
| * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
| * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
| * integration, and then adding the sum / n to the dynamic offset |
| * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
| * - Invent your own... |
| */ |
| extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
| /* |
| * For NaviCtrl use. |
| */ |
| extern int16_t averageAcc[2], averageAccCount; |
| /* |
| * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
| * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
| */ |
| void attitude_setNeutral(void); |
| /* |
| * Experiment. |
| */ |
| // void attitude_startDynamicCalibration(void); |
| // void attitude_continueDynamicCalibration(void); |
| int32_t getAngleEstimateFromAcc(uint8_t axis); |
| /* |
| * Main routine, called from the flight loop. |
| */ |
| void calculateFlightAttitude(void); |
| #endif //_ATTITUDE_H |
| /branches/dongfang_FC_rewrite/backup/attitudeControl.c |
|---|
| 0,0 → 1,46 |
| #include <inttypes.h> |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "dongfangMath.h" |
| // For scope debugging only! |
| #include "rc.h" |
| // = cos^2(45 degs). |
| const int32_t MINPROJECTION = (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
| // Takes 380 - 400 usec. Way too slow. |
| // With static MINPROJECTION: 220 usec. |
| uint16_t AC_getThrottle(uint16_t throttle) { |
| int32_t projection; |
| // part1 start: 150 usec |
| // It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
| projection = (int32_t)int_cos(angle[PITCH]) * (int32_t)int_cos(angle[ROLL]); |
| // part1 end. |
| uint8_t effect = dynamicParams.UserParams[2]; |
| int16_t deltaThrottle; |
| if (projection < MINPROJECTION && projection >= 0) { |
| // projection = MINPROJECTION; |
| deltaThrottle = 0; |
| } else if (projection >- MINPROJECTION && projection<0) { |
| // projection = -MINPROJECITON; |
| deltaThrottle = 0; |
| } else |
| /* |
| * We need delta throttle = constant/projection1 |
| * (constant * MATH_UNIT_FACTOR^2) / projection. |
| */ |
| deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
| // DebugOut.Analog[13] = deltaThrottle; |
| return throttle; |
| } |
| /* |
| har: R = e * k/p |
| vil R = e * ( 1 - k/p ) |
| = e - ek/p |
| */ |
| /branches/dongfang_FC_rewrite/backup/attitudeControl.h |
|---|
| 0,0 → 1,5 |
| #ifndef _ATTITUDECONTROL_H |
| #define _ATTITUDECONTROL_H |
| #include <inttypes.h> |
| uint16_t AC_getThrottle(uint16_t throttle); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/bugs.txt |
|---|
| 0,0 → 1,9 |
| tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
| Wer sieht hier Fehler? |
| 1) Nick- und Rollwinkeln werden mit Gierkonstant berechnet (1160 od. 1291) |
| 2) Pythagoras wird auf Winklen (statt Seitenlängen) verwendet. |
| /branches/dongfang_FC_rewrite/backup/commands.c |
|---|
| 0,0 → 1,125 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include "commands.h" |
| #include "controlMixer.h" |
| #include "flight.h" |
| #include "eeprom.h" |
| #include "attitude.h" |
| void commands_handleCommands(void) { |
| /* |
| * Get the current command (start/stop motors, calibrate), if any. |
| */ |
| uint8_t command = controlMixer_getCommand(); |
| uint8_t repeated = controlMixer_isCommandRepeated(); |
| uint8_t argument = controlMixer_getArgument(); |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (command == COMMAND_GYROCAL && !repeated) { |
| // Run gyro calibration but do not repeat it. |
| GRN_OFF; |
| // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
| // isFlying = 0; |
| // check roll/pitch stick position |
| // if pitch stick is top or roll stick is left or right --> change parameter setting |
| // according to roll/pitch stick position |
| if (argument < 6) { |
| // Gyro calinbration, with or without selecting a new parameter-set. |
| if(argument > 0 && argument < 6) { |
| // A valid parameter-set (1..5) was chosen - use it. |
| setActiveParamSet(argument); |
| } |
| ParamSet_ReadFromEEProm(getActiveParamSet()); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| controlMixer_setNeutral(); |
| beepNumber(getActiveParamSet()); |
| } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
| // If right stick is centered and down |
| // TODO: Out of here! State machine instead. |
| compassCalState = 1; |
| beep(1000); |
| } |
| } |
| // save the ACC neutral setting to eeprom |
| else { |
| if(command == COMMAND_ACCCAL && !repeated) { |
| // Run gyro and acc. meter calibration but do not repeat it. |
| GRN_OFF; |
| analog_calibrateAcc(); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| controlMixer_setNeutral(); |
| beepNumber(getActiveParamSet()); |
| } |
| } |
| } // end !MOTOR_RUN condition. |
| if (command == COMMAND_START) { |
| isFlying = 1; // TODO: Really???? |
| // if (!controlMixer_isCommandRepeated()) { |
| // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors. |
| MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all??? |
| // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors. |
| // attitude_continueDynamicCalibration(); |
| // setPointYaw = 0; |
| // IPartPitch = 0; |
| // IPartRoll = 0; |
| // } |
| } else if (command == COMMAND_STOP) { |
| isFlying = 0; |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/commands.h |
|---|
| 0,0 → 1,17 |
| #ifndef _COMMANDS_H |
| #define _COMMANDS_H |
| #include <inttypes.h> |
| void commands_handleCommands(void); |
| /* |
| * An enumeration over the start motors, stop motors, calibrate gyros |
| * and calibreate acc. meters commands. |
| */ |
| #define COMMAND_NONE 0 |
| #define COMMAND_START 6 |
| #define COMMAND_STOP 8 |
| #define COMMAND_GYROCAL 2 |
| #define COMMAND_ACCCAL 4 |
| #endif |
| /branches/dongfang_FC_rewrite/backup/commands.txt |
|---|
| 0,0 → 1,8 |
| These are the control commands: |
| - startMotors |
| - stopMotors |
| - calibrateGyros |
| - calibrateAcc (+height?) |
| - calibrateCompass |
| - captureHeightSetpoint |
| /branches/dongfang_FC_rewrite/backup/configuration.c |
|---|
| 0,0 → 1,292 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <util/delay.h> |
| #include <avr/eeprom.h> |
| #include <stddef.h> |
| #include "configuration.h" |
| #include "eeprom.h" |
| #include "timer0.h" |
| int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
| dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
| uint8_t CPUType = ATMEGA644; |
| uint8_t BoardRelease = 13; |
| /************************************************************************ |
| * Map the parameter to pot values |
| * Replacing this code by the code below saved almost 1 kbyte. |
| ************************************************************************/ |
| void configuration_applyVariablesToParams(void) { |
| uint8_t i; |
| #define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;} |
| #define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}} |
| SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight); |
| SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100); |
| SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100); |
| SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect); |
| SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect); |
| SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,10,255); |
| SET_POT(dynamicParams.GyroI,staticParams.GyroI); |
| SET_POT(dynamicParams.GyroD,staticParams.GyroD); |
| SET_POT(dynamicParams.IFactor,staticParams.IFactor); |
| for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
| SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]); |
| } |
| for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
| SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]); |
| } |
| SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl); |
| SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit); |
| SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1); |
| SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2); |
| SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection); |
| SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability); |
| SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255); |
| SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255); |
| #if defined (USE_MK3MAG) |
| SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl); |
| SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain); |
| SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP); |
| SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI); |
| SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD); |
| SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC); |
| SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255); |
| SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection); |
| SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation); |
| #endif |
| SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl); |
| } |
| const XLATION XLATIONS[] = { |
| {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)}, |
| {offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)}, |
| {offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)}, |
| {offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)}, |
| {offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)}, |
| {offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)}, |
| {offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)}, |
| {offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)}, |
| {offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)}, |
| {offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)}, |
| {offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)}, |
| {offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)}, |
| {offsetof(paramset_t, NaviGpsModeControl), |
| offsetof(dynamicParam_t, NaviGpsModeControl)}, |
| {offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)}, |
| {offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)}, |
| {offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)}, |
| {offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)}, |
| {offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)}, |
| {offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)}, |
| {offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)}, |
| {offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)} |
| }; |
| const MMXLATION MMXLATIONS[] = { |
| {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
| {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightD),0,150}, |
| {offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255}, |
| {offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255}, |
| {offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255}, |
| {offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255} |
| }; |
| uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
| uint8_t result; |
| if (src>=251) result = variables[src-251]; |
| else result = src; |
| if (result < min) result = min; |
| else if (result > max) result = max; |
| return result; |
| } |
| void configuration_applyVariablesToParams_dead(void) { |
| uint8_t i, src; |
| uint8_t* pointerToTgt; |
| for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) { |
| src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) { |
| src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max); |
| } |
| } |
| for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
| src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
| src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| } |
| uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values |
| uint8_t CPUType = ATMEGA644; |
| if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset |
| return CPUType; |
| } |
| /* |
| * Automatic detection of hardware components is not supported in this development-oriented |
| * FC firmware. It would go against the point of it: To enable alternative hardware |
| * configurations with otherwise unsupported components. Instead, one should write |
| * custom code + adjust constants for the new hardware, and include the relevant code |
| * from the makefile. |
| * However - we still do detect the board release. Reason: Otherwise it would be too |
| * tedious to have to modify the code for how to turn on and off LEDs when deploying |
| * on different HW version.... |
| */ |
| uint8_t getBoardRelease(void) { |
| uint8_t BoardRelease = 13; |
| // the board release is coded via the pull up or down the 2 status LED |
| PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
| DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input |
| _delay_loop_2(1000); // make some delay |
| switch( PINB & ((1<<PINB1)|(1<<PINB0))) { |
| case 0x00: |
| BoardRelease = 10; // 1.0 |
| break; |
| case 0x01: |
| BoardRelease = 11; // 1.1 or 1.2 |
| break; |
| case 0x02: |
| BoardRelease = 20; // 2.0 |
| break; |
| case 0x03: |
| BoardRelease = 13; // 1.3 |
| break; |
| default: |
| break; |
| } |
| // set LED ports as output |
| DDRB |= (1<<DDB1)|(1<<DDB0); |
| RED_ON; |
| GRN_OFF; |
| return BoardRelease; |
| } |
| void beep(uint16_t millis) { |
| BeepTime = millis; |
| } |
| /* |
| * Make [numbeeps] beeps. |
| */ |
| void beepNumber(uint8_t numbeeps) { |
| while(numbeeps--) { |
| if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren! |
| beep(100); // 0.1 second |
| Delay_ms(250); // blocks 250 ms as pause to next beep, |
| // this will block the flight control loop, |
| // therefore do not use this function if motors are running |
| } |
| } |
| /* |
| * Beep the R/C alarm signal |
| */ |
| void beepRCAlarm(void) { |
| if(BeepModulation == 0xFFFF) { // If not already beeping an alarm signal (?) |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0x0C00; |
| } |
| } |
| /* |
| * Beep the I2C bus error signal |
| */ |
| void beepI2CAlarm(void) { |
| if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| BeepTime = 10000; // 1 second |
| BeepModulation = 0x0080; |
| } |
| } |
| /* |
| * Beep the battery low alarm signal |
| */ |
| void beepBatteryAlarm(void) { |
| BeepModulation = 0x0300; |
| if(!BeepTime) { |
| BeepTime = 6000; // 0.6 seconds |
| } |
| } |
| /* |
| * Beep the EEPROM checksum alarm |
| */ |
| void beepEEPROMAlarm(void) { |
| BeepModulation = 0x0007; |
| if(!BeepTime) { |
| BeepTime = 6000; // 0.6 seconds |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/configuration.h |
|---|
| 0,0 → 1,205 |
| #ifndef _CONFIGURATION_H |
| #define _CONFIGURATION_H |
| #include <inttypes.h> |
| #include <avr/io.h> |
| typedef struct { |
| /*PMM*/ uint8_t HeightD; |
| /* P */ uint8_t MaxHeight; |
| /*PMM*/ uint8_t HeightP; |
| /* P */ uint8_t Height_ACC_Effect; |
| /* P */ uint8_t CompassYawEffect; |
| /* P */ uint8_t GyroD; |
| /*PMM*/ uint8_t GyroP; |
| /* P */ uint8_t GyroI; |
| /* Never used */ uint8_t StickYawP; |
| /* P */ uint8_t IFactor; |
| /* P */ uint8_t UserParams[8]; |
| /* P */ uint8_t ServoPitchControl; |
| /* P */ uint8_t LoopGasLimit; |
| /* P */ uint8_t AxisCoupling1; |
| /* P */ uint8_t AxisCoupling2; |
| /* P */ uint8_t AxisCouplingYawCorrection; |
| /* P */ uint8_t DynamicStability; |
| /* P */ uint8_t ExternalControl; |
| /*PMM*/ uint8_t J16Timing; |
| /*PMM*/ uint8_t J17Timing; |
| /* P */ uint8_t NaviGpsModeControl; |
| /* P */ uint8_t NaviGpsGain; |
| /* P */ uint8_t NaviGpsP; |
| /* P */ uint8_t NaviGpsI; |
| /* P */ uint8_t NaviGpsD; |
| /* P */ uint8_t NaviGpsACC; |
| /*PMM*/ uint8_t NaviOperatingRadius; |
| /* P */ uint8_t NaviWindCorrection; |
| /* P */ uint8_t NaviSpeedCompensation; |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| } dynamicParam_t; |
| extern dynamicParam_t dynamicParams; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| uint8_t min, max; |
| } MMXLATION; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| } XLATION; |
| // values above 250 representing poti1 to poti4 |
| typedef struct { |
| uint8_t ChannelAssignment[8]; // see upper defines for details |
| uint8_t GlobalConfig; // see upper defines for bitcoding |
| uint8_t HeightMinGas; // Value : 0-100 |
| uint8_t HeightD; // Value : 0-250 |
| uint8_t MaxHeight; // Value : 0-32 |
| uint8_t HeightP; // Value : 0-32 |
| uint8_t Height_Gain; // Value : 0-50 |
| uint8_t Height_ACC_Effect; // Value : 0-250 |
| uint8_t StickP; // Value : 1-6 |
| uint8_t StickD; // Value : 0-64 |
| uint8_t StickYawP; // Value : 1-20 |
| uint8_t MinThrottle; // Value : 0-32 |
| uint8_t MaxThrottle; // Value : 33-250 |
| uint8_t GyroAccFactor; // Value : 1-64 |
| uint8_t CompassYawEffect; // Value : 0-32 |
| uint8_t GyroP; // Value : 10-250 |
| uint8_t GyroI; // Value : 0-250 |
| uint8_t GyroD; // Value : 0-250 |
| uint8_t LowVoltageWarning; // Value : 0-250 |
| uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Emp�ngsverlust |
| uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
| uint8_t Unused0; // |
| uint8_t IFactor; // Value : 0-250 |
| uint8_t UserParams1[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam2; // Value : 0-250 |
| uint8_t UserParam3; // Value : 0-250 |
| uint8_t UserParam4; // Value : 0-250 |
| */ |
| uint8_t ServoPitchControl; // Value : 0-250 // Stellung des Servos |
| uint8_t ServoPitchComp; // Value : 0-250 // Einfluss Gyro/Servo |
| uint8_t ServoPitchMin; // Value : 0-250 // Anschlag |
| uint8_t ServoPitchMax; // Value : 0-250 // Anschlag |
| uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output |
| uint8_t LoopGasLimit; // Value: 0-250 max. Gas w�hrend Looping |
| uint8_t LoopThreshold; // Value: 0-250 Schwelle f�r Stickausschlag |
| uint8_t LoopHysteresis; // Value: 0-250 Hysterese f�r Stickausschlag |
| uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
| uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AngleTurnOverPitch; // Value: 0-250 180�-Punkt |
| uint8_t AngleTurnOverRoll; // Value: 0-250 180�-Punkt |
| uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t DriftComp; // limit for gyrodrift compensation |
| uint8_t DynamicStability; // PID limit for Attitude controller |
| uint8_t UserParams2[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam6; // Value : 0-250 |
| uint8_t UserParam7; // Value : 0-250 |
| uint8_t UserParam8; // Value : 0-250 |
| */ |
| uint8_t J16Bitmask; // for the J16 Output |
| uint8_t J16Timing; // for the J16 Output |
| uint8_t J17Bitmask; // for the J17 Output |
| uint8_t J17Timing; // for the J17 Output |
| uint8_t NaviGpsModeControl; // Parameters for the Naviboard |
| uint8_t NaviGpsGain; // overall gain for GPS-PID controller |
| uint8_t NaviGpsP; // P gain for GPS-PID controller |
| uint8_t NaviGpsI; // I gain for GPS-PID controller |
| uint8_t NaviGpsD; // D gain for GPS-PID controller |
| uint8_t NaviGpsPLimit; // P limit for GPS-PID controller |
| uint8_t NaviGpsILimit; // I limit for GPS-PID controller |
| uint8_t NaviGpsDLimit; // D limit for GPS-PID controller |
| uint8_t NaviGpsACC; // ACC gain for GPS-PID controller |
| uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions |
| uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements |
| uint8_t NaviWindCorrection; // streng of wind course correction |
| uint8_t NaviSpeedCompensation; // D gain fefore position hold login |
| uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights |
| uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm |
| uint8_t NaviPHLoginTime; // position hold logintimeout |
| uint8_t ExternalControl; // for serial Control |
| uint8_t BitConfig; // see upper defines for bitcoding |
| uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
| uint8_t Reserved[4]; |
| int8_t Name[12]; |
| } paramset_t; |
| #define PARAMSET_STRUCT_LEN sizeof(paramset_t) |
| extern paramset_t staticParams; |
| typedef struct { |
| uint8_t Revision; |
| int8_t Name[12]; |
| int8_t Motor[16][4]; |
| } __attribute__((packed)) MixerTable_t; |
| extern MixerTable_t Mixer; |
| // MKFlags |
| #define MKFLAG_MOTOR_RUN (1<<0) |
| #define MKFLAG_FLY (1<<1) |
| #define MKFLAG_CALIBRATE (1<<2) |
| #define MKFLAG_START (1<<3) |
| #define MKFLAG_EMERGENCY_LANDING (1<<4) |
| #define MKFLAG_RESERVE1 (1<<5) |
| #define MKFLAG_RESERVE2 (1<<6) |
| #define MKFLAG_RESERVE3 (1<<7) |
| // bit mask for staticParams.GlobalConfig |
| #define CFG_HEIGHT_CONTROL (1<<0) |
| #define CFG_HEIGHT_SWITCH (1<<1) |
| #define CFG_HEADING_HOLD (1<<2) |
| #define CFG_COMPASS_ACTIVE (1<<3) |
| #define CFG_COMPASS_FIX (1<<4) |
| #define CFG_GPS_ACTIVE (1<<5) |
| #define CFG_AXIS_COUPLING_ACTIVE (1<<6) |
| #define CFG_ROTARY_RATE_LIMITER (1<<7) |
| // bit mask for staticParams.BitConfig |
| #define CFG_LOOP_UP (1<<0) |
| #define CFG_LOOP_DOWN (1<<1) |
| #define CFG_LOOP_LEFT (1<<2) |
| #define CFG_LOOP_RIGHT (1<<3) |
| #define CFG_HEIGHT_3SWITCH (1<<4) |
| #define ATMEGA644 0 |
| #define ATMEGA644P 1 |
| #define SYSCLK F_CPU |
| // Not really a part of configuration, but heck, LEDs and beepers now have a home. |
| #define RED_OFF {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
| #define RED_ON {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);} |
| #define RED_FLASH PORTB ^= (1<<PORTB0) |
| #define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);} |
| #define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);} |
| #define GRN_FLASH PORTB ^= (1<<PORTB1) |
| // Mixer table |
| #define MIX_THROTTLE 0 |
| #define MIX_PITCH 1 |
| #define MIX_ROLL 2 |
| #define MIX_YAW 3 |
| extern volatile uint8_t MKFlags; |
| extern int16_t variables[8]; |
| extern uint8_t BoardRelease; |
| extern uint8_t CPUType; |
| void configuration_applyVariablesToParams(void); |
| uint8_t getCPUType(void); |
| uint8_t getBoardRelease(void); |
| // Not really a part of configuration, but heck, LEDs and beepers now have a home. |
| void beep(uint16_t millis); |
| void beepNumber(uint8_t numbeeps); |
| void beepRCAlarm(void); |
| void beepI2CAlarm(void); |
| void beepBatteryAlarm(void); |
| #endif // _CONFIGURATION_H |
| /branches/dongfang_FC_rewrite/backup/control.c |
|---|
| 0,0 → 1,376 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt 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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /* |
| OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler? |
| #include <stdlib.h> |
| #include "control.h" |
| #include "rc.h" |
| #include "configuration.h" |
| #include "attitude.h" |
| #include "eeprom.h" |
| #include "flight.h" |
| #define RCChannel(dimension) (PPM_in[staticParams.ChannelAssignment[dimension]]) |
| uint16_t maxStickPitch = 0, maxStickRoll = 0; |
| int16_t stickPitch = 0, stickRoll = 0, stickYaw = 0, stickThrottle = 0; |
| int16_t GPSStickPitch = 0, GPSStickRoll = 0; |
| int16_t externalStickPitch = 0, externalStickRoll = 0, externalStickYaw = 0, externalHeightValue = -20; |
| // dongfang's own experiment: Cablibrated sticks. |
| int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
| // Looping-or-not flags. |
| uint8_t loopingPitch = 0, loopingRoll = 0; |
| uint8_t loopingLeft = 0, loopingRight = 0, loopingDown = 0, loopingTop = 0; |
| // Internal variables for reading commands made with an R/S stick. |
| uint8_t lastStickCommand = STICK_COMMAND_UNDEF; |
| uint8_t stickCommandTimer = 0; |
| ExternalControl_t externalControl; |
| / * |
| * Stick diagram: |
| * 2--3--4 |
| * | | + |
| * 1 9 5 ^ 0 |
| * | | | |
| * 8--7--6 |
| * |
| * + <-- |
| * 0 |
| * |
| * Not in any of these positions: 0 |
| * / |
| / * |
| * The stick most be further from center than this to indicate a settings number (1-5). |
| * / |
| #define STICK_SETTINGSELECTION_THRESHOLD 70 |
| uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) { |
| if(RCChannel(CH_THROTTLE) > thresholdThrottle) { |
| // throttle is up |
| if(RCChannel(CH_YAW) > thresholdYaw) |
| return STICK_COMMAND_GYROCAL; |
| if(RCChannel(CH_YAW) < -thresholdYaw) |
| return STICK_COMMAND_ACCCAL; |
| return STICK_COMMAND_UNDEF; |
| } else if(RCChannel(CH_THROTTLE) < -thresholdThrottle) { |
| // pitch is down |
| if(RCChannel(CH_YAW) > thresholdYaw) |
| return STICK_COMMAND_STOP; |
| if(RCChannel(CH_YAW) < -thresholdYaw) |
| return STICK_COMMAND_START; |
| return STICK_COMMAND_UNDEF; |
| } else { |
| // pitch is around center |
| return STICK_COMMAND_UNDEF; |
| } |
| } |
| uint8_t control_getRightRCStickIndex(void) { |
| if(RCChannel(CH_PITCH) > STICK_SETTINGSELECTION_THRESHOLD) { |
| // pitch is up |
| if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
| return 2; |
| if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
| return 4; |
| return 3; |
| } else if(RCChannel(CH_PITCH) < -STICK_SETTINGSELECTION_THRESHOLD) { |
| // pitch is down |
| if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
| return 8; |
| if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
| return 6; |
| return 7; |
| } else { |
| // pitch is around center |
| if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD) |
| return 1; |
| if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD) |
| return 5; |
| return 9; |
| } |
| } |
| / * |
| * This could be expanded to take calibrate / start / stop commands from ohter sources |
| * than the R/C (read: Custom MK R/C project) |
| * / |
| void control_senseStickCommands(void) { |
| uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85); |
| if (stickCommandNow != lastStickCommand) { |
| lastStickCommand = stickCommandNow; |
| stickCommandTimer = 0; |
| } else { |
| if (stickCommandTimer < 201) |
| stickCommandTimer++; |
| } |
| } |
| / * |
| * This could be expanded to take calibrate / start / stop commands from ohter sources |
| * than the R/C (read: Custom MK R/C project) |
| * / |
| uint8_t control_getStickCommand(void) { |
| // If the same command was made 200 times, it's stable. |
| if (stickCommandTimer >= 200) { |
| return lastStickCommand; |
| } |
| return STICK_COMMAND_UNDEF; |
| } |
| uint8_t control_isStickCommandRepeated(void) { |
| return stickCommandTimer > 200 ? 1 : 0; |
| } |
| / * |
| * To be fired only when the right stick is in the center position. |
| * This will cause the value of pitch and roll stick to be adjusted |
| * to zero (not just to near zero, as per the assumption in rc.c |
| * about the rc signal. I had values about 50..70 with a Futaba |
| * R617 receiver.) This calibration is not strictly necessary, but |
| * for control logic that depends on the exact (non)center position |
| * of a stick, it may be useful. |
| * / |
| void control_setNeutral(void) { |
| stickOffsetPitch += stickPitch; |
| stickOffsetRoll += stickRoll; |
| } |
| / * |
| * Set the potientiometer values to the values of the respective R/C channel |
| * right now. No slew rate limit. |
| * / |
| void control_initPots(void) { |
| uint8_t i; |
| for (i=0; i<4; i++) { |
| pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET; |
| } |
| for (i=4; i<8; i++) { |
| pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET; |
| } |
| } |
| / * |
| * Update potentiometer values with slow slew rate. Could be made faster if desired. |
| * / |
| void control_updatePots(void) { |
| uint8_t i; |
| uint16_t targetvalue; |
| for (i=0; i<8; i++) { |
| if (i<4) // configured pots |
| targetvalue = RCChannel(CH_POTS + i) + POT_OFFSET; |
| else // PPM24-Extension |
| targetvalue = PPM_in[9 + i] + POT_OFFSET; |
| if (targetvalue < 0) targetvalue = 0; |
| if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--; |
| } |
| } |
| / * |
| * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| * / |
| void control_update(void) { |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| stickPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
| // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4; |
| stickPitch += PPM_diff[staticParams.ChannelAssignment[CH_PITCH]] * staticParams.StickD; |
| stickPitch = stickPitch - stickOffsetPitch - GPSStickPitch; |
| stickRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
| // stick_roll = (stick_roll * 3 + RCChannel(CH_ROLL) * staticParams.StickP) / 4; |
| stickRoll += PPM_diff[staticParams.ChannelAssignment[CH_ROLL]] * staticParams.StickD; |
| stickRoll = stickRoll - stickOffsetRoll - GPSStickRoll; |
| // mapping of yaw |
| stickYaw = -RCChannel(CH_YAW); |
| // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| if (stickYaw > 2) stickYaw-= 2; |
| else if (stickYaw< -2) stickYaw += 2; |
| else stickYaw = 0; |
| } |
| // mapping of gas |
| stickThrottle = RCChannel(CH_THROTTLE) + 120;// shift to positive numbers |
| if(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128) { |
| stickPitch += (int16_t) externalControl.pitch * (int16_t) staticParams.StickP; |
| stickRoll += (int16_t) externalControl.roll * (int16_t) staticParams.StickP; |
| stickYaw += externalControl.yaw; |
| // ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)staticParams.Height_Gain; |
| // Dubious: Lowest throttle setting has precedence. |
| if(externalControl.throttle < stickThrottle) stickThrottle = externalControl.throttle; |
| } |
| if(stickThrottle < 0) stickThrottle = 0; |
| if(abs(stickPitch / CONTROL_SCALING) > maxStickPitch) { |
| maxStickPitch = abs(stickPitch) / CONTROL_SCALING; |
| if(maxStickPitch > 100) maxStickPitch = 100; |
| } |
| else if (maxStickPitch) maxStickPitch--; |
| if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) { |
| maxStickRoll = abs(stickRoll) / CONTROL_SCALING; |
| if(maxStickRoll > 100) maxStickRoll = 100; |
| } |
| else if (maxStickRoll) maxStickRoll--; |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Looping? Do not consider external or GPS input for this :) |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if((RCChannel(CH_ROLL) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_LEFT) loopingLeft = 1; |
| else { |
| if(loopingLeft) { // Hysteresis |
| if((RCChannel(CH_ROLL) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingLeft = 0; |
| } |
| } |
| if((RCChannel(CH_ROLL) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_RIGHT) loopingRight = 1; |
| else { |
| if(loopingRight) { // Hysteresis |
| if(RCChannel(CH_ROLL) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingRight = 0; |
| } |
| } |
| if((RCChannel(CH_PITCH) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_UP) loopingTop = 1; |
| else { |
| if(loopingTop) { // Hysteresis |
| if((RCChannel(CH_PITCH) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingTop = 0; |
| } |
| } |
| if((RCChannel(CH_PITCH) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_DOWN) loopingDown = 1; |
| else { |
| if(loopingDown) { // Hysteresis |
| if(RCChannel(CH_PITCH) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingDown = 0; |
| } |
| } |
| if(loopingLeft || loopingRight) loopingRoll = 1; else loopingRoll = 0; |
| if(loopingTop || loopingDown) { loopingPitch = 1; loopingRoll = 0; loopingLeft = 0; loopingRight = 0;} else loopingPitch = 0; |
| } |
| void setCompassCalState(void) { |
| static uint8_t stick = 1; |
| // if pitch is centered or top set stick to zero |
| if(RCChannel(CH_PITCH) > -20) stick = 0; |
| // if pitch is down trigger to next cal state |
| if((RCChannel(CH_PITCH) < -70) && !stick) { |
| stick = 1; |
| compassCalState++; |
| if(compassCalState < 5) beepNumber(compassCalState); |
| else beep(1000); |
| } |
| } |
| / * |
| * |
| * / |
| uint8_t control_hasNewRCData(void) { |
| // return !NewPpmData--; |
| return (NewPpmData-- == 0) ? 1 : 0; |
| } |
| void control_performPilotCalibrationCommands(uint8_t stickCommand) { |
| if (stickCommand == STICK_COMMAND_GYROCAL && !control_isStickCommandRepeated()) { |
| // Run gyro calibration but do not repeat it. |
| GRN_OFF; |
| // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
| // isFlying = 0; |
| // check roll/pitch stick position |
| // if pitch stick is top or roll stick is left or right --> change parameter setting |
| // according to roll/pitch stick position |
| uint8_t setting = control_getRightRCStickIndex(); |
| if ((setting > 0 && setting < 6) || setting == 9) { |
| // Gyro calinbration, with or without selecting a new parameter-set. |
| if(setting > 0 && setting < 6) { |
| // A valid parameter-set (1..5) was chosen - use it. |
| setActiveParamSet(setting); |
| } |
| ParamSet_ReadFromEEProm(getActiveParamSet()); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| if (setting == 9) { // Right stick is centered; calibrate it to zero (hmm strictly does not belong here). |
| control_setNeutral(); // Calibrate right stick neutral position. |
| } |
| beepNumber(getActiveParamSet()); |
| } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) { |
| // If right stick is centered and down |
| compassCalState = 1; |
| beep(1000); |
| } |
| } |
| // save the ACC neutral setting to eeprom |
| else { |
| if(stickCommand == STICK_COMMAND_ACCCAL && !control_isStickCommandRepeated()) { |
| // Run gyro and acc. meter calibration but do not repeat it. |
| GRN_OFF; |
| analog_calibrateAcc(); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| control_setNeutral(); // Calibrate right stick neutral position. |
| beepNumber(getActiveParamSet()); |
| } |
| } |
| } // end !MOTOR_RUN condition. |
| /* |
| /branches/dongfang_FC_rewrite/backup/control.h |
|---|
| 0,0 → 1,76 |
| /*********************************************************************************/ |
| /* Stick control interface */ |
| /*********************************************************************************/ |
| /* |
| OBSOLETED BY controlMixer.h. But this is how it looked - maybe somebody will find it simpler? |
| #ifndef _CONTROL_H |
| #define _CONTROL_H |
| #include <inttypes.h> |
| #define CONTROL_SCALING 4 |
| // defines for lookup staticParams.ChannelAssignment |
| #define CH_PITCH 0 |
| #define CH_ROLL 1 |
| #define CH_THROTTLE 2 |
| #define CH_YAW 3 |
| #define CH_POTS 4 |
| #define POT_OFFSET 110 |
| extern int16_t stickPitch, stickRoll, stickYaw, stickThrottle; |
| extern int16_t stickOffsetNick, stickOffsetRoll; |
| extern uint16_t maxStickPitch, maxStickRoll; |
| extern uint8_t loopingPitch, loopingRoll; |
| // external control |
| extern int16_t externalStickPitch, externalStickRoll, externalStickYaw; |
| // current GPS-stick values |
| extern int16_t GPSStickPitch, GPSStickRoll; |
| typedef struct { |
| uint8_t digital[2]; |
| uint8_t eemoteButtons; |
| int8_t pitch; |
| int8_t roll; |
| int8_t yaw; |
| uint8_t throttle; |
| int8_t height; |
| uint8_t free; |
| uint8_t frame; |
| uint8_t config; |
| } __attribute__((packed)) ExternalControl_t; |
| extern ExternalControl_t externalControl; |
| //uint8_t control_getLeftStickCalibrateIndex(void); |
| //uint8_t control_getLeftStickMotorStartIndex(void); |
| //uint8_t control_getRightRCStickIndex(void); |
| void control_initPots(void); |
| void control_updatePots(void); |
| void setCompassCalState(void); |
| void updateCompass(void); |
| void control_setNeutral(void); |
| void control_update(void); |
| #define STICK_COMMAND_UNDEF 0 |
| #define STICK_COMMAND_START 6 |
| #define STICK_COMMAND_STOP 8 |
| #define STICK_COMMAND_GYROCAL 2 |
| #define STICK_COMMAND_ACCCAL 4 |
| void control_senseStickCommands(void); |
| uint8_t control_getStickCommand(void); |
| uint8_t control_isStickCommandRepeated(void); |
| void control_performPilotCalibrationCommands(uint8_t stickCommand); |
| extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200) |
| extern uint8_t control_hasNewRCData(void); |
| #endif //_CONTROL_H |
| */ |
| /branches/dongfang_FC_rewrite/backup/controlMixer.c |
|---|
| 0,0 → 1,215 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include "controlMixer.h" |
| #include "rc.h" |
| #include "heightControl.h" |
| #include "attitudeControl.h" |
| #include "externalControl.h" |
| #include "configuration.h" |
| #include "attitude.h" |
| #include "commands.h" |
| //#include "eeprom.h" |
| //#include "flight.h" |
| uint16_t maxControl[2] = {0,0}; |
| int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0; |
| uint8_t looping = 0; |
| // Internal variables for reading commands made with an R/C stick. |
| uint8_t lastCommand = COMMAND_NONE; |
| uint8_t lastArgument; |
| uint8_t isCommandRepeated = 0; |
| // MK flags. TODO: Replace by enum. State machine. |
| uint16_t isFlying = 0; |
| volatile uint8_t MKFlags = 0; |
| /* |
| * This could be expanded to take arguments from ohter sources than the RC |
| * (read: Custom MK RC project) |
| */ |
| uint8_t controlMixer_getArgument(void) { |
| return lastArgument; |
| } |
| /* |
| * This could be expanded to take calibrate / start / stop commands from ohter sources |
| * than the R/C (read: Custom MK R/C project) |
| */ |
| uint8_t controlMixer_getCommand(void) { |
| return lastCommand; |
| } |
| uint8_t controlMixer_isCommandRepeated(void) { |
| return isCommandRepeated; |
| } |
| void controlMixer_setNeutral() { |
| EC_setNeutral(); |
| HC_setGround(); |
| } |
| /* |
| * Set the potientiometer values to the momentary values of the respective R/C channels. |
| * No slew rate limitation. |
| */ |
| void controlMixer_initVariables(void) { |
| uint8_t i; |
| for (i=0; i<8; i++) { |
| variables[i] = RC_getVariable(i); |
| } |
| } |
| /* |
| * Update potentiometer values with limited slew rate. Could be made faster if desired. |
| * TODO: It assumes R/C as source. Not necessarily true. |
| */ |
| void controlMixer_updateVariables(void) { |
| uint8_t i; |
| int16_t targetvalue; |
| for (i=0; i<8; i++) { |
| targetvalue = RC_getVariable(i); |
| if (targetvalue < 0) targetvalue = 0; |
| if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--; |
| } |
| } |
| uint8_t controlMixer_getSignalQuality(void) { |
| uint8_t rcQ = RC_getSignalQuality(); |
| uint8_t ecQ = EC_getSignalQuality(); |
| // This needs not be the only correct solution... |
| return rcQ > ecQ ? rcQ : ecQ; |
| } |
| /* |
| * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| */ |
| void controlMixer_update(void) { |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| // TODO: If no signal --> zero. |
| uint8_t axis; |
| // takes almost no time... |
| RC_update(); |
| // takes almost no time... |
| EC_update(); |
| // takes about 80 usec. |
| HC_update(); |
| int16_t* RC_PRTY = RC_getPRTY(); |
| int16_t* EC_PRTY = EC_getPRTY(); |
| control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
| control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
| // This can be a CPU time killer if the function implementations are inefficient. |
| controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE])); |
| controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
| if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| controlMixer_updateVariables(); |
| configuration_applyVariablesToParams(); |
| looping = RC_getLooping(looping); |
| } else { // Signal is not OK |
| // Could handle switch to emergency flight here. |
| // throttle is handled elsewhere. |
| looping = 0; |
| } |
| // part1a end. |
| /* This is not really necessary with the pull-towards-zero of all sticks (see rc.c) |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| if (controlYaw > 2) controlYaw-= 2; |
| else if (controlYaw< -2) controlYaw += 2; |
| else controlYaw = 0; |
| } |
| */ |
| /* |
| * Record maxima |
| */ |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
| maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
| if(maxControl[axis] > 100) maxControl[axis] = 100; |
| } else if (maxControl[axis]) maxControl[axis]--; |
| } |
| uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
| uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
| if (rcCommand != COMMAND_NONE) { |
| isCommandRepeated = (lastCommand == rcCommand); |
| lastCommand = rcCommand; |
| lastArgument = RC_getArgument(); |
| } else if (ecCommand != COMMAND_NONE) { |
| isCommandRepeated = (lastCommand == ecCommand); |
| lastCommand = ecCommand; |
| lastArgument = EC_getArgument(); |
| } else { |
| // Both sources have no command, or one or both are out. |
| // Just set to false. There is no reason to check if the none-command was repeated anyway. |
| isCommandRepeated = 0; |
| lastCommand = COMMAND_NONE; |
| } |
| // part1 end. |
| } |
| // TODO: Integrate into command system. |
| uint8_t controlMixer_testCompassCalState(void) { |
| return RC_testCompassCalState(); |
| } |
| /branches/dongfang_FC_rewrite/backup/controlMixer.h |
|---|
| 0,0 → 1,127 |
| #include <inttypes.h> |
| /* |
| * An attempt to define a generic control. That could be an R/C receiver, an external control |
| * (serial over Bluetooth, Wi232, XBee, whatever) or the NaviCtrl. |
| * This resembles somewhat an object-oriented class definition (except that there are no virtuals). |
| * The idea is that the combination of different control inputs, of the way they superimpose upon |
| * each other, the priorities between them and the behavior in case that one fails is simplified, |
| * and all in one place. |
| */ |
| /* |
| * Signal qualities, used to determine the availability of a control. |
| * NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost. |
| * SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage |
| * or disengage emergency landing. |
| * SIGNAL_OK means the signal is usable for flight. |
| * SIGNAL_GOOD means the signal can also be used for setting parameters. |
| */ |
| #define NO_SIGNAL 0 |
| #define SIGNAL_LOST 1 |
| #define SIGNAL_BAD 2 |
| #define SIGNAL_OK 3 |
| #define SIGNAL_GOOD 4 |
| /* |
| * The PRTY arrays |
| */ |
| #define CONTROL_PITCH 0 |
| #define CONTROL_ROLL 1 |
| #define CONTROL_THROTTLE 2 |
| #define CONTROL_YAW 3 |
| /* |
| * Looping flags. |
| * LOOPING_UP || LOOPING_DOWN <=> LOOPING_PITCH_AXIS |
| * LOOPING_LEFT || LOOPING_RIGHT <=> LOOPING_ROLL_AXIS |
| */ |
| #define LOOPING_UP (1<<0) |
| #define LOOPING_DOWN (1<<1) |
| #define LOOPING_LEFT (1<<2) |
| #define LOOPING_RIGHT (1<<3) |
| #define LOOPING_PITCH_AXIS (1<<4) |
| #define LOOPING_ROLL_AXIS (1<<5) |
| /* |
| * This is only relevant for "abstract controls" ie. all control sources have the |
| * same interface. This struct of code pointers is used like an abstract class |
| * definition from object-oriented languages, and all control input implementations |
| * will declare an instance of the stuct (=implementation of the abstract class). |
| */ |
| typedef struct { |
| /* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
| int16_t(*getPitch)(void); |
| /* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
| int16_t(*getRoll)(void); |
| /* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
| int16_t(*getYaw)(void); |
| /* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */ |
| uint16_t(*getThrottle)(void); |
| /* Signal quality, by the above SIGNAL_... definitions. */ |
| uint8_t (*getSignalQuality)(void); |
| /* Calibrate sticks to their center positions (only relevant for R/C, really) */ |
| void (*calibrate)(void); |
| } t_control; |
| /* |
| * Our output. |
| */ |
| extern int16_t control[2], controlYaw, controlThrottle; |
| extern uint16_t maxControl[2]; |
| extern uint8_t looping; |
| extern volatile uint8_t MKFlags; |
| extern uint16_t isFlying; |
| void controlMixer_initVariables(void); |
| void controlMixer_updateVariables(void); |
| void controlMixer_setNeutral(void); |
| /* |
| * Update the exported variables. Called at every flight control cycle. |
| */ |
| void controlMixer_update(void); |
| /* |
| * Get the current command. See the COMMAND_.... define's |
| */ |
| uint8_t controlMixer_getCommand(void); |
| void controlMixer_performCalibrationCommands(uint8_t command); |
| uint8_t controlMixer_getSignalQuality(void); |
| /* |
| * The controls operate in a [-150 * CONTROL_SCALING, 150 * CONTROL_SCALING] interval |
| * Throttle is [0..300 * CONTROL_SCALING]. |
| * (just about. No precision needed). |
| */ |
| #define CONTROL_SCALING (1024/256) |
| /* |
| * Gets the argument for the current command (a number). |
| * |
| * Stick position to argument values (for stick control): |
| * 2--3--4 |
| * | | + |
| * 1 9 5 ^ 0 |
| * | | | |
| * 8--7--6 |
| * |
| * + <-- |
| * 0 |
| * |
| * Not in any of these positions: 0 |
| */ |
| // void controlMixer_handleCommands(void); |
| uint8_t controlMixer_getArgument(void); |
| uint8_t controlMixer_isCommandRepeated(void); |
| // TODO: Abstract away if possible. |
| uint8_t controlMixer_testCompassCalState(void); |
| // void controlMixer_updateCompass(void); |
| /branches/dongfang_FC_rewrite/backup/displays.txt |
|---|
| 0,0 → 1,10 |
| Timerstyret eller Anforderung0 : 'H' format. Timerkonstant = interval fra request * 10 |
| H format response: H, address, buffer(80) |
| h format request: keys(invers!), interval |
| keys: key1(2): baglaens, key2(4): forlaens, begge: #0, ingen: Bliv paa samme side. |
| andre keys: key4(8): reset(???) flyvetimer. |
| Kun h request setter keys. |
| DisplayAnfordering1: 'L' format. |
| L format response: L, address, menupunkt, maxmenu, buffer(80) |
| l format request: l, address, menupunkt |
| /branches/dongfang_FC_rewrite/backup/dongfangMath.c |
|---|
| 0,0 → 1,231 |
| #include "dongfangMath.h" |
| #include <inttypes.h> |
| #include <avr/pgmspace.h> |
| const int16_t SIN_TABLE[] PROGMEM = { |
| (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.0 * MATH_UNIT_FACTOR) }; |
| const int16_t TAN_TABLE[] PROGMEM = { |
| (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
| (int16_t) (32767) |
| }; |
| int16_t int_sin(int32_t arg) { |
| int8_t sign; |
| int16_t result; |
| arg /= MATH_DRG_FACTOR; |
| arg %= 360; |
| if (arg < 0) { |
| arg = -arg; |
| sign = -1; |
| } else { |
| sign = 1; |
| } |
| if (arg >= 90) { |
| arg = 180 - arg; |
| } |
| result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]); |
| return (sign == 1) ? result : -result; |
| } |
| int16_t int_cos(int32_t arg) { |
| if (arg > 90L * MATH_DRG_FACTOR) |
| return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
| return int_sin(arg + 90L * MATH_DRG_FACTOR); |
| } |
| int16_t int_tan(int32_t arg) { |
| int8_t sign = 1; |
| int16_t result; |
| arg /= MATH_DRG_FACTOR; |
| if (arg >= 90) { |
| arg = 180 - arg; |
| sign = -1; |
| } else if (arg < -90) { |
| arg += 180; |
| } else if (arg < 0) { |
| arg =- arg; |
| sign = -1; |
| } |
| result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
| return (sign == 1) ? result : -result; |
| } |
| /branches/dongfang_FC_rewrite/backup/dongfangMath.h |
|---|
| 0,0 → 1,21 |
| #include<inttypes.h> |
| #include "attitude.h" |
| /* |
| * Angular unit scaling: Number of units per degree |
| */ |
| #define MATH_DRG_FACTOR GYRO_DEG_FACTOR_PITCHROLL |
| /* |
| * Fix-point decimal scaling: Number of units for 1 (so if sin(something) |
| * returns UNIT_FACTOR * 0.8, the result is to be understood as 0.8) |
| * a * sin(b) = (a * int_sin(b * DRG_FACTOR)) / UNIT_FACTOR |
| */ |
| //#define MATH_UNIT_FACTOR 8192 |
| // Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
| // 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
| #define MATH_UNIT_FACTOR 4096 |
| int16_t int_sin(int32_t arg); |
| int16_t int_cos(int32_t arg); |
| int16_t int_tan(int32_t arg); |
| /branches/dongfang_FC_rewrite/backup/dsl.c |
|---|
| 0,0 → 1,227 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // This code has been derived from the implementation of Stefan Engelke. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /* |
| Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu> |
| Permission is hereby granted, free of charge, to any person |
| obtaining a copy of this software and associated documentation |
| files (the "Software"), to deal in the Software without |
| restriction, including without limitation the rights to use, copy, |
| modify, merge, publish, distribute, sublicense, and/or sell copies |
| of the Software, and to permit persons to whom the Software is |
| furnished to do so, subject to the following conditions: |
| The above copyright notice and this permission notice shall be |
| included in all copies or substantial portions of the Software. |
| THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
| NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
| HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
| WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER |
| DEALINGS IN THE SOFTWARE. |
| $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $ |
| RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the |
| communication protocol used by DSL receivers of Act Europe. |
| The DSL receivers have a serial communication port to connect |
| two receivers in diversity mode. Each receiver is sending the |
| received servo signals periodically over this port. This fact |
| can be used to connect the receiver to the control unit of the |
| model via UART instead of evaluating the PPM signal. |
| If you have any questions, fell free to send me an e-mail. |
| */ |
| /* |
| Connection of DSL to SV1 of FC: |
| ( DSL Pin1 is on side of channel 4 ) |
| 1. GND <--> pin 7 (GND) |
| 2. TXD <--> pin 3 (RXD1 Atmega644p) |
| 3. RXD <--> pin 4 (TXD1 Atmega644p) optional |
| 4. 5V <--> pin 2 (5V) |
| Do not connect the receiver via PPM-Sumsignal output the same time. |
| Data are send at every 20 ms @ 38400 Baud 8-N-1 |
| Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc |
| FREQALLOC = 35, 40, 72 |
| RSSI = 0.. 255 // Received signal strength indicator |
| VBAT = 0...255 // supply voltage (0.0V.. 7.8V) |
| Servo Pair: |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC| |
| X is channel index of 1 servo value |
| D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms) |
| there are 8 channels submitted, i.e 4 servo pairs |
| Frame examples with signel received |
| FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE |
| FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE |
| FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD |
| FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE |
| FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD |
| FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF |
| FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF |
| Frame examples with no signal received |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| FFFF 1F23F000A30426 |
| */ |
| #include <stdlib.h> |
| #include "dsl.h" |
| #include "rc.h" |
| #include "uart0.h" |
| uint8_t dsl_RSSI = 0; |
| uint8_t dsl_Battery = 0; |
| uint8_t dsl_Allocation = 0; |
| uint8_t PacketBuffer[6]; |
| //uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c |
| typedef union |
| { |
| int16_t Servo[2]; |
| uint8_t byte[4]; |
| } ChannelPair_t; |
| ChannelPair_t ChannelPair; |
| // This function is called, when a new servo signal is properly received. |
| // Parameters: servo - servo number (0-9) |
| // signal - servo signal between 7373 (1ms) and 14745 (2ms) |
| void dsl_new_signal(uint8_t channel, int16_t signal) |
| { |
| int16_t tmp; |
| uint8_t index = channel + 1; // mk channels start with 1 |
| //RC_Quality = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level |
| //if(RC_Quality > 255) RC_Quality = 255; |
| // signal from DSL-receiver is between 7373 (1ms) und 14745 (2ms). |
| signal-= 11059; // shift to neutral |
| signal/= 24; // scale to mk rc resolution |
| if(abs(signal-PPM_in[index]) < 6) |
| { |
| if(RC_Quality < 200) RC_Quality +=10; |
| else RC_Quality = 200; |
| } |
| // calculate exponential history for signal |
| tmp = (3 * (PPM_in[index]) + signal) / 4; |
| if(tmp > signal+1) tmp--; else |
| if(tmp < signal-1) tmp++; |
| // calculate signal difference on good signal level |
| if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; // update channel value |
| if(index == 4) |
| { |
| NewPpmData = 0; |
| } |
| } |
| // This function is called within dsl_parser(), when a complete |
| // data packet with valid checksum has been received. |
| void dsl_decode_packet(void) |
| { |
| uint8_t i; |
| // check for header condition |
| if((PacketBuffer[0] & 0xF0) == 0x10) |
| { |
| if(PacketBuffer[0] == 0x1F) // separate status frame |
| { |
| dsl_Allocation = PacketBuffer[1]; // Get frequency allocation |
| // ?? = PacketBuffer[2]; |
| dsl_RSSI = PacketBuffer[3]; // Get signal quality |
| dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply |
| // ?? = PacketBuffer[5]; |
| if(dsl_RSSI == 0) |
| { |
| RC_Quality = 0; |
| for (i = 0; i<5; i++) |
| { |
| PPM_diff[i] = 0; |
| PPM_in[i] = 0; |
| } |
| } |
| } |
| else // probably a channel pair |
| { |
| i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair |
| if(i < 10)// maximum 12 channels |
| { |
| // big to little endian |
| ChannelPair.byte[1] = PacketBuffer[1]; |
| ChannelPair.byte[0] = PacketBuffer[2]; |
| ChannelPair.byte[3] = PacketBuffer[3]; |
| ChannelPair.byte[2] = PacketBuffer[4]; |
| dsl_new_signal(i, ChannelPair.Servo[0]); |
| dsl_new_signal(i+1,ChannelPair.Servo[1]); |
| } |
| } |
| } // EOF header condition |
| } |
| // this function should be called within the UART RX ISR |
| void dsl_parser(uint8_t c) |
| { |
| static uint8_t last_c = 0; |
| static uint8_t crc = 0; |
| static uint8_t cnt = 0; |
| static uint8_t packet_len = 0; |
| // check for sync condition |
| if ((c==0xFF) && (last_c==0xFF)) |
| { |
| cnt = 0; // reset byte counter |
| crc = 0; // reset checksum |
| return; |
| } |
| if(cnt == 0) // begin of a packet |
| { |
| if(c == 0x1F) packet_len = 5; // a status packet has 5 bytes + crc |
| else packet_len = 4; // a channel pair packet has 4 bytes + crc |
| } |
| if(cnt > packet_len) // packet complete, crc byte received |
| { |
| // calculate checksum |
| crc = ~crc; |
| if (crc == 0xFF) crc = 0xFE; |
| // if crc matches decode the packet |
| if (c == crc) dsl_decode_packet(); |
| // handle next packet |
| cnt = 0; |
| crc = 0; |
| } |
| else // collect channel data bytes |
| { |
| PacketBuffer[cnt++] = c; |
| crc += c; |
| } |
| // store last byte for sync check |
| last_c = c; |
| } |
| /branches/dongfang_FC_rewrite/backup/dsl.h |
|---|
| 0,0 → 1,15 |
| #ifndef _DSL_H |
| #define _DSL_H |
| #include <inttypes.h> |
| extern uint8_t dsl_RSSI; // Received signal strength indicator |
| extern uint8_t dsl_Battery; // Battery voltage (0-255 [0V - 8.2V]) |
| extern uint8_t dsl_Allocation; // Frequency allocation (35,40,72) |
| #define USART1_BAUD 38400 |
| // this function should be called within the UART RX ISR |
| extern void dsl_parser(uint8_t c); |
| #endif //_DSL_H |
| /branches/dongfang_FC_rewrite/backup/dynamic_calibration_scrap.txt |
|---|
| 0,0 → 1,38 |
| /* |
| * Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always |
| * pushing of pulling on the pitch or roll stick. |
| */ |
| /* |
| if(ADCycleCount >= dynamicParams.UserParam2 * 10) { |
| // This algo works OK on the desk but it is a little sluggish and it oscillates. |
| // It does not very effectively cancel drift because of dynamics. |
| minStickForAutoCal = dynamicParams.UserParam3 * 10; |
| maxStickForAutoCal = dynamicParams.UserParam4 * 10; |
| // If not already corrected to the limit, and dynamic calibration is enabled: |
| if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
| // The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset. |
| if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) { |
| dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...) |
| pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
| } else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) { |
| dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...) |
| pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
| } |
| } |
| // If not already corrected to the limit, and dynamic calibration is enabled: |
| if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
| if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) { |
| dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128); |
| rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
| } else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) { |
| dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128); |
| rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
| } |
| } |
| ADCycleCount = 0; |
| } |
| */ |
| /branches/dongfang_FC_rewrite/backup/eeprom.c |
|---|
| 0,0 → 1,394 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Contant Values |
| // + 0-250 -> normale Values |
| // + 251 -> Poti1 |
| // + 252 -> Poti2 |
| // + 253 -> Poti3 |
| // + 254 -> Poti4 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #ifndef EEMEM |
| #define EEMEM __attribute__ ((section (".eeprom"))) |
| #endif |
| #include <avr/eeprom.h> |
| #include <string.h> |
| #include "eeprom.h" |
| #include "printf_P.h" |
| #include "output.h" |
| // TODO: Get rid of these. They have nothing to do with eeprom. |
| #include "flight.h" |
| #include "rc.h" |
| #include "sensors.h" |
| // byte array in eeprom |
| uint8_t EEPromArray[E2END+1] EEMEM; |
| paramset_t staticParams; |
| MixerTable_t Mixer; |
| /* |
| * Default for your own experiments here, so you don't have to reset them |
| * from MK-Tool all the time. |
| */ |
| void setDefaultUserParams(void) { |
| uint8_t i; |
| for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
| staticParams.UserParams1[i] = 0; |
| } |
| for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
| staticParams.UserParams2[i] = 0; |
| } |
| /* |
| * While we are still using userparams for flight parameters, do set |
| * some safe & meaningful default values. |
| */ |
| staticParams.UserParams1[3] = 10; // Throttle stick D=10 |
| staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4 |
| staticParams.UserParams2[1] = 2; // H&I motor smoothing. |
| staticParams.UserParams2[2] = 120; // Yaw I factor |
| staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles. |
| } |
| void setOtherDefaults(void) { |
| /* Channel assignments were changed to the normal: |
| * Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4 |
| */ |
| staticParams.ChannelAssignment[CH_PITCH] = 2; |
| staticParams.ChannelAssignment[CH_ROLL] = 1; |
| staticParams.ChannelAssignment[CH_THROTTLE] = 3; |
| staticParams.ChannelAssignment[CH_YAW] = 4; |
| staticParams.ChannelAssignment[CH_POTS+0] = 5; |
| staticParams.ChannelAssignment[CH_POTS+1] = 6; |
| staticParams.ChannelAssignment[CH_POTS+2] = 7; |
| staticParams.ChannelAssignment[CH_POTS+3] = 8; |
| staticParams.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_HEADING_HOLD; // CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
| staticParams.HeightMinGas = 30; |
| staticParams.MaxHeight = 251; |
| staticParams.HeightP = 10; |
| staticParams.HeightD = 30; |
| staticParams.Height_ACC_Effect = 30; |
| staticParams.Height_Gain = 4; |
| staticParams.StickP = 12; |
| staticParams.StickD = 16; |
| staticParams.StickYawP = 12; |
| staticParams.MinThrottle = 8; |
| staticParams.MaxThrottle = 230; |
| staticParams.CompassYawEffect = 128; |
| staticParams.GyroP = 80; |
| staticParams.GyroI = 100; |
| staticParams.LowVoltageWarning = 95; |
| staticParams.EmergencyGas = 35; |
| staticParams.EmergencyGasDuration = 30; |
| staticParams.Unused0 = 0; |
| staticParams.IFactor = 32; |
| staticParams.ServoPitchControl = 100; |
| staticParams.ServoPitchComp = 40; |
| staticParams.ServoPitchCompInvert = 0; |
| staticParams.ServoPitchMin = 50; |
| staticParams.ServoPitchMax = 150; |
| staticParams.ServoRefresh = 5; |
| staticParams.LoopGasLimit = 50; |
| staticParams.LoopThreshold = 90; |
| staticParams.LoopHysteresis = 50; |
| staticParams.BitConfig = 0; |
| staticParams.AxisCoupling1 = 90; |
| staticParams.AxisCoupling2 = 67; |
| staticParams.AxisCouplingYawCorrection = 0; |
| staticParams.DynamicStability = 50; |
| staticParams.J16Bitmask = 95; |
| staticParams.J17Bitmask = 243; |
| staticParams.J16Timing = 15; |
| staticParams.J17Timing = 15; |
| staticParams.NaviGpsModeControl = 253; |
| staticParams.NaviGpsGain = 100; |
| staticParams.NaviGpsP = 90; |
| staticParams.NaviGpsI = 90; |
| staticParams.NaviGpsD = 90; |
| staticParams.NaviGpsPLimit = 75; |
| staticParams.NaviGpsILimit = 75; |
| staticParams.NaviGpsDLimit = 75; |
| staticParams.NaviGpsACC = 0; |
| staticParams.NaviGpsMinSat = 6; |
| staticParams.NaviStickThreshold = 8; |
| staticParams.NaviWindCorrection = 90; |
| staticParams.NaviSpeedCompensation = 30; |
| staticParams.NaviOperatingRadius = 100; |
| staticParams.NaviAngleLimitation = 100; |
| staticParams.NaviPHLoginTime = 4; |
| } |
| /***************************************************/ |
| /* Default Values for parameter set 1 */ |
| /***************************************************/ |
| void ParamSet_DefaultSet1(void) { // sport |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| memcpy(staticParams.Name, "Sport\0",6); |
| } |
| /***************************************************/ |
| /* Default Values for parameter set 2 */ |
| /***************************************************/ |
| void ParamSet_DefaultSet2(void) { // normal |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| staticParams.Height_Gain = 3; |
| staticParams.J16Timing = 20; |
| staticParams.J17Timing = 20; |
| memcpy(staticParams.Name, "Normal\0", 7); |
| } |
| /***************************************************/ |
| /* Default Values for parameter set 3 */ |
| /***************************************************/ |
| void ParamSet_DefaultSet3(void) { // beginner |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| staticParams.Height_Gain = 3; |
| staticParams.EmergencyGasDuration = 20; |
| staticParams.AxisCouplingYawCorrection = 70; |
| staticParams.J16Timing = 30; |
| staticParams.J17Timing = 30; |
| memcpy(staticParams.Name, "Beginner\0", 9); |
| } |
| /***************************************************/ |
| /* Read Parameter from EEPROM as byte */ |
| /***************************************************/ |
| uint8_t GetParamByte(uint16_t param_id) { |
| return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
| } |
| /***************************************************/ |
| /* Write Parameter to EEPROM as byte */ |
| /***************************************************/ |
| void SetParamByte(uint16_t param_id, uint8_t value) { |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
| } |
| /***************************************************/ |
| /* Read Parameter from EEPROM as word */ |
| /***************************************************/ |
| uint16_t GetParamWord(uint16_t param_id) { |
| return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
| } |
| /***************************************************/ |
| /* Write Parameter to EEPROM as word */ |
| /***************************************************/ |
| void SetParamWord(uint16_t param_id, uint16_t value) { |
| eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
| } |
| /***************************************************/ |
| /* Read Parameter Set from EEPROM */ |
| /***************************************************/ |
| // number [1..5] |
| void ParamSet_ReadFromEEProm(uint8_t setnumber) { |
| if((1 > setnumber) || (setnumber > 5)) setnumber = 3; |
| eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber - 1)], PARAMSET_STRUCT_LEN); |
| output_init(); |
| } |
| /***************************************************/ |
| /* Write Parameter Set to EEPROM */ |
| /***************************************************/ |
| // number [1..5] |
| void ParamSet_WriteToEEProm(uint8_t setnumber) { |
| if(setnumber > 5) setnumber = 5; |
| if(setnumber < 1) return; |
| eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber - 1)], PARAMSET_STRUCT_LEN); |
| eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH], PARAMSET_STRUCT_LEN); |
| eeprom_write_block( &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping |
| // set this parameter set to active set |
| setActiveParamSet(setnumber); |
| output_init(); |
| } |
| /***************************************************/ |
| /* Get active parameter set */ |
| /***************************************************/ |
| uint8_t getActiveParamSet(void) { |
| uint8_t setnumber; |
| setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]); |
| if(setnumber > 5) { |
| setnumber = 3; |
| eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
| } |
| return(setnumber); |
| } |
| /***************************************************/ |
| /* Set active parameter set */ |
| /***************************************************/ |
| void setActiveParamSet(uint8_t setnumber) { |
| if(setnumber > 5) setnumber = 5; |
| if(setnumber < 1) setnumber = 1; |
| eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
| } |
| /***************************************************/ |
| /* Read MixerTable from EEPROM */ |
| /***************************************************/ |
| uint8_t MixerTable_ReadFromEEProm(void) { |
| if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == EEMIXER_REVISION) { |
| eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
| return 1; |
| } |
| else return 0; |
| } |
| /***************************************************/ |
| /* Write Mixer Table to EEPROM */ |
| /***************************************************/ |
| uint8_t MixerTable_WriteToEEProm(void) { |
| if(Mixer.Revision == EEMIXER_REVISION) { |
| eeprom_write_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
| return 1; |
| } |
| else return 0; |
| } |
| /***************************************************/ |
| /* Default Values for Mixer Table */ |
| /***************************************************/ |
| void MixerTable_Default(void) { // Quadro |
| uint8_t i; |
| Mixer.Revision = EEMIXER_REVISION; |
| // clear mixer table (but preset throttle) |
| for(i = 0; i < 16; i++) { |
| Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
| Mixer.Motor[i][MIX_PITCH] = 0; |
| Mixer.Motor[i][MIX_ROLL] = 0; |
| Mixer.Motor[i][MIX_YAW] = 0; |
| } |
| // default = Quadro |
| Mixer.Motor[0][MIX_PITCH] = +64; Mixer.Motor[0][MIX_YAW] = +64; |
| Mixer.Motor[1][MIX_PITCH] = -64; Mixer.Motor[1][MIX_YAW] = +64; |
| Mixer.Motor[2][MIX_ROLL] = -64; Mixer.Motor[2][MIX_YAW] = -64; |
| Mixer.Motor[3][MIX_ROLL] = +64; Mixer.Motor[3][MIX_YAW] = -64; |
| memcpy(Mixer.Name, "Quadro\0", 7); |
| } |
| /***************************************************/ |
| /* Initialize EEPROM Parameter Sets */ |
| /***************************************************/ |
| void ParamSet_Init(void) { |
| uint8_t Channel_Backup=1, i, j; |
| // parameter version check |
| if(eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) { |
| // if version check faild |
| printf("\n\rInit Parameter in EEPROM"); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table |
| // check if channel mapping backup is valid |
| for (j=0; j<4 && Channel_Backup; j++) { |
| if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) >= 12) |
| Channel_Backup = 0; |
| } |
| // fill all 5 parameter settings |
| for (i=1; i<6; i++) { |
| switch(i) { |
| case 1: |
| ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport) |
| break; |
| case 2: |
| ParamSet_DefaultSet2(); // Kamera |
| break; |
| case 3: |
| ParamSet_DefaultSet3(); // Beginner |
| break; |
| default: |
| ParamSet_DefaultSet2(); // Kamera |
| break; |
| } |
| if(Channel_Backup) { // if we have a rc channel mapping backup in eeprom |
| // restore it |
| for (j=0; j<8; j++) { |
| staticParams.ChannelAssignment[j] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+j]); |
| } |
| } |
| ParamSet_WriteToEEProm(i); |
| } |
| // default-Setting is parameter set 3 |
| setActiveParamSet(1); |
| // update version info |
| SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
| } |
| // read active parameter set to staticParams stucture |
| ParamSet_ReadFromEEProm(getActiveParamSet()); |
| printf("\n\rUsing Parameter Set %d", getActiveParamSet()); |
| // load mixer table |
| if(!MixerTable_ReadFromEEProm()) { |
| printf("\n\rGenerating default Mixer Table"); |
| MixerTable_Default(); // Quadro |
| MixerTable_WriteToEEProm(); |
| } |
| // determine motornumber |
| RequiredMotors = 0; |
| for(i = 0; i < 16; i++) { |
| if(Mixer.Motor[i][MIX_THROTTLE] > 0) RequiredMotors++; |
| } |
| printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors); |
| printf("\n\r=============================="); |
| } |
| /branches/dongfang_FC_rewrite/backup/eeprom.h |
|---|
| 0,0 → 1,48 |
| #ifndef _EEPROM_H |
| #define _EEPROM_H |
| #include <inttypes.h> |
| #include "configuration.h" |
| #define EEPROM_ADR_PARAM_BEGIN 0 |
| #define PID_PARAM_REVISION 1 // byte |
| #define PID_ACTIVE_SET 2 // byte |
| #define PID_PRESSURE_OFFSET 3 // byte |
| #define PID_ACC_PITCH 4 // word |
| #define PID_ACC_ROLL 6 // word |
| #define PID_ACC_Z 8 // word |
| #ifdef USE_KILLAGREG |
| #define PID_MM3_X_OFF 11 // byte |
| #define PID_MM3_Y_OFF 12 // byte |
| #define PID_MM3_Z_OFF 13 // byte |
| #define PID_MM3_X_RANGE 14 // word |
| #define PID_MM3_Y_RANGE 16 // word |
| #define PID_MM3_Z_RANGE 18 // word |
| #endif |
| #define EEPROM_ADR_CHANNELS 80 // 8 bytes |
| #define EEPROM_ADR_PARAMSET_LENGTH 98 // word |
| #define EEPROM_ADR_PARAMSET_BEGIN 100 |
| #define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076 |
| #define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility) |
| #define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility) |
| extern void ParamSet_Init(void); |
| extern void ParamSet_ReadFromEEProm(uint8_t setnumber); |
| extern void ParamSet_WriteToEEProm(uint8_t setnumber); |
| extern uint8_t MixerTable_ReadFromEEProm(void); |
| extern uint8_t MixerTable_WriteToEEProm(void); |
| extern uint8_t GetParamByte(uint16_t param_id); |
| extern void SetParamByte(uint16_t param_id, uint8_t value); |
| extern uint16_t GetParamWord(uint16_t param_id); |
| extern void SetParamWord(uint16_t param_id, uint16_t value); |
| extern uint8_t getActiveParamSet(void); |
| extern void setActiveParamSet(uint8_t setnumber); |
| #endif //_EEPROM_H |
| /branches/dongfang_FC_rewrite/backup/encoding-tester.txt |
|---|
| 0,0 → 1,3 |
| UTF-8: äöü |
| ISO-8859-1: äöü |
| /branches/dongfang_FC_rewrite/backup/externalControl.c |
|---|
| 0,0 → 1,67 |
| #include "externalcontrol.h" |
| #include "configuration.h" |
| #include "controlMixer.h" |
| ExternalControl_t externalControl; |
| uint8_t externalControlActive; |
| int16_t EC_PRTY[4]; |
| // TODO: Who is going to call this |
| void EC_setNeutral(void) { |
| // if necessary. From main.c. |
| externalControl.config = 0; |
| externalControl.pitch = 0; |
| externalControl.roll = 0; |
| externalControl.yaw = 0; |
| externalControl.throttle = 0; |
| // From main.c. What does it do?? |
| externalControl.digital[0] = 0x55; |
| } |
| int16_t* EC_getPRTY(void) { |
| return EC_PRTY; |
| } |
| uint8_t EC_getArgument(void) { |
| return externalControl.config; |
| } |
| uint8_t EC_getCommand(void) { |
| return externalControl.free; |
| } |
| // not implemented. |
| int16_t EC_getVariable(uint8_t varNum) { |
| return 0; |
| } |
| void EC_update() { |
| if (externalControlActive) { |
| externalControlActive--; |
| EC_PRTY[CONTROL_PITCH] = (int16_t) externalControl.pitch * (int16_t) staticParams.StickP; |
| EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.StickP; |
| EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle; |
| EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar?????? |
| } else { |
| EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE] = EC_PRTY[CONTROL_YAW] = 0; |
| } |
| } |
| uint8_t EC_getSignalQuality(void) { |
| if (externalControlActive > 80) |
| // Configured and heard from recently |
| return SIGNAL_GOOD; |
| if (externalControlActive) |
| // Configured and heard from |
| return SIGNAL_OK; |
| if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128)) |
| // External control is not even configured. |
| return NO_SIGNAL; |
| // Configured but expired. |
| return SIGNAL_LOST; |
| } |
| /branches/dongfang_FC_rewrite/backup/externalControl.h |
|---|
| 0,0 → 1,32 |
| // Or does this simply belong in uart0.h?? |
| #ifndef _EXTERNALCONTROL_H |
| #define _EXTERNALCONTROL_H |
| #include<inttypes.h> |
| typedef struct { |
| uint8_t digital[2]; |
| uint8_t remoteButtons; |
| int8_t pitch; |
| int8_t roll; |
| int8_t yaw; |
| uint8_t throttle; |
| int8_t height; |
| uint8_t free; // Let's use that for commands now. |
| uint8_t frame; |
| uint8_t config; // Let's use that for arguemnts. |
| } __attribute__((packed)) ExternalControl_t; |
| extern ExternalControl_t externalControl; |
| extern uint8_t externalControlActive; |
| void EC_update(void); |
| int16_t* EC_getPRTY(void); |
| uint8_t EC_getArgument(void); |
| uint8_t EC_getCommand(void); |
| int16_t EC_getVariable(uint8_t varNum); |
| void EC_calibrate(void); |
| uint8_t EC_getSignalQuality (void); |
| void EC_setNeutral(void); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/fc-0.74_bugs.txt |
|---|
| 0,0 → 1,6 |
| NaviAcc. variables - overflow |
| A/D values are read after AD cycle restart |
| Axis coupling bug (can be demonstrated by flying some fast maneuvers in H H mode with |
| axis coupling on, and watching the copter lose control. Once axis coupling is turned |
| back off, it is OK again). |
| /branches/dongfang_FC_rewrite/backup/flexcontrol.h |
|---|
| 0,0 → 1,8 |
| typedef struct { |
| int16_t(*getPitch)(void); |
| int16_t(*getRoll)(void); |
| int16_t(*getYaw)(void); |
| uint16_t(*getThrottle)(void); |
| uint8_t isSignalGood(void); |
| uint8_t isSignalUnreadable(void); |
| } t_control; |
| /branches/dongfang_FC_rewrite/backup/flight.c |
|---|
| 0,0 → 1,463 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include "eeprom.h" |
| #include "flight.h" |
| // Only for debug. Remove. |
| //#include "analog.h" |
| //#include "rc.h" |
| // Necessary for external control and motor test |
| #include "uart0.h" |
| // for scope debugging |
| #include "rc.h" |
| #include "twimaster.h" |
| #include "attitude.h" |
| #include "controlMixer.h" |
| #ifdef USE_MK3MAG |
| #include "gps.h" |
| #endif |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| /* |
| * These are no longer maintained, just left at 0. The original implementation just summed the acc. |
| * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
| */ |
| // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
| // Some integral weight constant... |
| uint16_t Ki = 10300 / 33; |
| uint8_t RequiredMotors = 0; |
| // No support for altitude control right now. |
| // int16_t SetPointHeight = 0; |
| /************************************************************************/ |
| /* Filter for motor value smoothing (necessary???) */ |
| /************************************************************************/ |
| int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
| switch(dynamicParams.UserParams[5]) { |
| case 0: |
| return newvalue; |
| case 1: |
| return (oldvalue + newvalue) / 2; |
| case 2: |
| if(newvalue > oldvalue) |
| return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
| else |
| return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
| case 3: |
| if(newvalue < oldvalue) |
| return (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
| else |
| return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
| default: return newvalue; |
| } |
| } |
| /************************************************************************/ |
| /* Neutral Readings */ |
| /************************************************************************/ |
| void flight_setNeutral() { |
| MKFlags |= MKFLAG_CALIBRATE; |
| // not really used here any more. |
| dynamicParams.KalmanK = -1; |
| dynamicParams.KalmanMaxDrift = 0; |
| dynamicParams.KalmanMaxFusion = 32; |
| controlMixer_initVariables(); |
| } |
| void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| Ki = 10300 / _Ki; |
| gyroPFactor = _gyroPFactor; |
| gyroIFactor = _gyroIFactor; |
| yawPFactor = _yawPFactor; |
| yawIFactor = _yawIFactor; |
| } |
| void setNormalFlightParameters(void) { |
| setFlightParameters(dynamicParams.IFactor + 1, |
| dynamicParams.GyroP + 10, |
| staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
| dynamicParams.GyroP + 10, |
| dynamicParams.UserParams[6] |
| ); |
| } |
| void setStableFlightParameters(void) { |
| setFlightParameters(33, 90, 120, 90, 120); |
| } |
| /************************************************************************/ |
| /* Main Flight Control */ |
| /************************************************************************/ |
| void flight_control(void) { |
| int16_t tmp_int; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t yawTerm, throttleTerm, term[2]; |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw, PPart[2]; |
| static int32_t IPart[2] = {0,0}; |
| // static int32_t yawControlRate = 0; |
| // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
| // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
| // static int32_t CorrectionPitch, CorrectionRoll; |
| static uint16_t emergencyFlightTime; |
| static int8_t debugDataTimer = 1; |
| // High resolution motor values for smoothing of PID motor outputs |
| static int16_t motorFilters[MAX_MOTORS]; |
| uint8_t i, axis; |
| // Fire the main flight attitude calculation, including integration of angles. |
| calculateFlightAttitude(); |
| /* |
| * TODO: update should: Set the stick variables if good signal, set them to zero if bad. |
| * Set variables also. |
| */ |
| // start part 1: 750-800 usec. |
| // start part 1a: 750-800 usec. |
| // start part1b: 700 usec |
| // start part1c: 700 usec!!!!!!!!! WAY too slow. |
| controlMixer_update(); |
| // end part1c |
| throttleTerm = controlThrottle; |
| if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
| // end part1b: 700 usec. |
| /************************************************************************/ |
| /* RC-signal is bad */ |
| /* This part could be abstracted, as having yet another control input */ |
| /* to the control mixer: An emergency autopilot control. */ |
| /************************************************************************/ |
| if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| RED_ON; |
| beepRCAlarm(); |
| if(emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| if(isFlying > 256) { |
| // We're probably still flying. Descend slowly. |
| throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
| MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
| setStableFlightParameters(); |
| } else { |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
| } |
| } else { |
| // end emergency flight (just cut the motors???) |
| MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
| } |
| } else { |
| // signal is acceptable |
| if(controlMixer_getSignalQuality() > SIGNAL_BAD) { |
| // Reset emergency landing control variables. |
| MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
| // The time is in whole seconds. |
| emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488; |
| } |
| // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
| if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| // increment flight-time counter until overflow. |
| if(isFlying != 0xFFFF) isFlying++; |
| } else |
| /* |
| * When standing on the ground, do not apply I controls and zero the yaw stick. |
| * Probably to avoid integration effects that will cause the copter to spin |
| * or flip when taking off. |
| */ |
| if(isFlying < 256) { |
| IPart[PITCH] = IPart[ROLL] = 0; |
| // TODO: Don't stomp on other modules' variables!!! |
| controlYaw = 0; |
| if(isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| yawAngleDiff = 0; |
| } |
| } else { |
| // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
| // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
| MKFlags |= (MKFLAG_FLY); |
| } |
| commands_handleCommands(); |
| // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| setNormalFlightParameters(); |
| // } |
| } // end else (not bad signal case) |
| // end part1a: 750-800 usec. |
| /* |
| * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
| * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
| * This is the throttle part. |
| */ |
| if(looping) { |
| if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
| } |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| badCompassHeading = 1000; |
| if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| } |
| } |
| // yawControlRate = controlYaw; |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| #if defined (USE_MK3MAG) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } |
| else { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| } |
| #endif |
| // end part 1: 750-800 usec. |
| // start part 3: 350 - 400 usec. |
| #define SENSOR_LIMIT (4096 * 4) |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| if(looping & ((1<<4)<<axis)) { |
| PPart[axis] = 0; |
| } else { // TODO: Where do the 44000 come from??? |
| PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| } |
| /* |
| * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
| * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
| * where pfactor is in [0..1]. |
| */ |
| PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
| + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
| CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| } |
| PDPartYaw = |
| (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
| + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| // limit control feedback |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| /* |
| * Compose throttle term. |
| * If a Bl-Ctrl is missing, prevent takeoff. |
| */ |
| if(missingMotor) { |
| // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0)) |
| isFlying = 1; // keep within lift off condition |
| throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
| } |
| throttleTerm *= CONTROL_SCALING; |
| /* |
| * Compose yaw term. |
| * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
| * However, at low throttle the yaw term is limited to a fixed value, |
| * and at high throttle it is limited by the throttle reserve (the difference |
| * between current throttle and maximum throttle). |
| */ |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| // limit yawTerm |
| if(throttleTerm > MIN_YAWGAS) { |
| CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
| } else { |
| CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
| CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
| tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| /* |
| * Compose pitch and roll terms. This is finally where the sticks come into play. |
| */ |
| if(gyroIFactor) { |
| // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
| // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
| // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
| IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| } else { |
| // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
| // To keep up with a full stick PDPart should be about 156... |
| IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| } |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
| * (max. pitch or roll term is the throttle value). |
| * TODO: Why a growing function of yaw? |
| */ |
| CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
| } |
| // end part 3: 350 - 400 usec. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |
| // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| for(i = 0; i < MAX_MOTORS; i++) { |
| int16_t tmp; |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
| tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
| tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
| tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| tmp = motorFilters[i] / CONTROL_SCALING; |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
| DebugOut.Analog[22+i] = tmp; |
| CHECK_MIN_MAX(tmp, 1, 255); |
| Motor[i].SetPoint = tmp; |
| } |
| else if (motorTestActive) { |
| Motor[i].SetPoint = motorTest[i]; |
| } else { |
| Motor[i].SetPoint = 0; |
| } |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!(--debugDataTimer)) { |
| debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| /* |
| DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
| DebugOut.Analog[24] = controlYaw; |
| DebugOut.Analog[25] = yawAngleDiff / 100L; |
| */ |
| DebugOut.Analog[26] = accNoisePeak[PITCH]; |
| DebugOut.Analog[27] = accNoisePeak[ROLL]; |
| DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/flight.h |
|---|
| 0,0 → 1,43 |
| /*********************************************************************************/ |
| /* Flight Control */ |
| /*********************************************************************************/ |
| #ifndef _FLIGHT_H |
| #define _FLIGHT_H |
| #include <inttypes.h> |
| #include "analog.h" |
| #include "configuration.h" |
| #define PITCH 0 |
| #define ROLL 1 |
| /* |
| * The output BLC throttles can be set in an [0..256[ interval. Some staticParams limits |
| * (min, throttle, max throttle etc) are in a [0..256[ interval. |
| * The calculation of motor throttle values from sensor and control information (basically |
| * flight.c) take place in an [0..1024[ interval for better precision. |
| * This is the conversion factor. |
| * --> Replaced back again by CONTROL_SCALING. Even though the 2 things are not quite the |
| * same, they are unseperable anyway. |
| */ |
| extern uint8_t RequiredMotors; |
| // looping params |
| // extern long TurnOver180Nick, TurnOver180Roll; |
| // external control |
| // extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
| // extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
| // TODO: Rip em up, replace by a flight-state machine. |
| // OK first step: Move to control mixer, where the state machine will be located anyway. |
| // extern volatile uint8_t MKFlags; |
| // extern uint16_t isFlying; |
| void flight_control(void); |
| void transmitMotorThrottleData(void); |
| void flight_setNeutral(void); |
| #endif //_FLIGHT_H |
| /branches/dongfang_FC_rewrite/backup/gps.c |
|---|
| 0,0 → 1,433 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <inttypes.h> |
| #include <stdlib.h> |
| #include "ubx.h" |
| //#include "mymath.h" |
| #include "timer0.h" |
| #include "uart0.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| typedef enum |
| { |
| GPS_FLIGHT_MODE_UNDEF, |
| GPS_FLIGHT_MODE_FREE, |
| GPS_FLIGHT_MODE_AID, |
| GPS_FLIGHT_MODE_HOME, |
| } FlightMode_t; |
| #define GPS_POSINTEGRAL_LIMIT 32000 |
| #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_P_LIMIT 25 |
| typedef struct |
| { |
| int32_t Longitude; |
| int32_t Latitude; |
| int32_t Altitude; |
| Status_t Status; |
| } GPS_Pos_t; |
| // GPS coordinates for hold position |
| GPS_Pos_t HoldPosition = {0,0,0,INVALID}; |
| // GPS coordinates for home position |
| GPS_Pos_t HomePosition = {0,0,0,INVALID}; |
| // the current flight mode |
| FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF; |
| // --------------------------------------------------------------------------------- |
| void GPS_UpdateParameter(void) { |
| static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF; |
| if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) { |
| FlightMode = GPS_FLIGHT_MODE_FREE; |
| } else { |
| if (dynamicParams.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
| else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE; |
| else FlightMode = GPS_FLIGHT_MODE_HOME; |
| } |
| if (FlightMode != FlightModeOld) { |
| BeepTime = 100; |
| } |
| FlightModeOld = FlightMode; |
| } |
| // --------------------------------------------------------------------------------- |
| // This function defines a good GPS signal condition |
| uint8_t GPS_IsSignalOK(void) { |
| static uint8_t GPSFix = 0; |
| if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) { |
| GPSFix = 1; |
| return 1; |
| } |
| else return (0); |
| } |
| // --------------------------------------------------------------------------------- |
| // rescale xy-vector length to limit |
| uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) { |
| uint8_t retval = 0; |
| int32_t len; |
| len = (int32_t)c_sqrt(*x * *x + *y * *y); |
| if (len > limit) { |
| // normalize control vector components to the limit |
| *x = (*x * limit) / len; |
| *y = (*y * limit) / len; |
| retval = 1; |
| } |
| return(retval); |
| } |
| // checks nick and roll sticks for manual control |
| uint8_t GPS_IsManualControlled(void) { |
| if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0; |
| else return 1; |
| } |
| // set given position to current gps position |
| uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) { |
| uint8_t retval = 0; |
| if(pGPSPos == NULL) return(retval); // bad pointer |
| if(GPS_IsSignalOK()) { // is GPS signal condition is fine |
| pGPSPos->Longitude = GPSInfo.longitude; |
| pGPSPos->Latitude = GPSInfo.latitude; |
| pGPSPos->Altitude = GPSInfo.altitude; |
| pGPSPos->Status = NEWDATA; |
| retval = 1; |
| } else { // bad GPS signal condition |
| pGPSPos->Status = INVALID; |
| retval = 0; |
| } |
| return(retval); |
| } |
| // clear position |
| uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) { |
| uint8_t retval = 0; |
| if(pGPSPos == NULL) |
| return retval; // bad pointer |
| else { |
| pGPSPos->Longitude = 0; |
| pGPSPos->Latitude = 0; |
| pGPSPos->Altitude = 0; |
| pGPSPos->Status = INVALID; |
| retval = 1; |
| } |
| return (retval); |
| } |
| // disable GPS control sticks |
| void GPS_Neutral(void) { |
| GPSStickNick = 0; |
| GPSStickRoll = 0; |
| } |
| // calculates the GPS control stick values from the deviation to target position |
| // if the pointer to the target positin is NULL or is the target position invalid |
| // then the P part of the controller is deactivated. |
| void GPS_PIDController(GPS_Pos_t *pTargetPos) { |
| static int32_t PID_Nick, PID_Roll; |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
| int32_t PID_North = 0, PID_East = 0; |
| static int32_t cos_target_latitude = 1; |
| static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
| static GPS_Pos_t *pLastTargetPos = 0; |
| // if GPS data and Compass are ok |
| if( GPS_IsSignalOK() && (CompassHeading >= 0)) { |
| if(pTargetPos != NULL) // if there is a target position |
| { |
| if(pTargetPos->Status != INVALID) // and the position data are valid |
| { |
| // if the target data are updated or the target pointer has changed |
| if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) ) |
| { |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| // recalculate latitude projection |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
| // remember last target pointer |
| pLastTargetPos = pTargetPos; |
| // mark data as processed |
| pTargetPos->Status = PROCESSED; |
| } |
| // calculate position deviation from latitude and longitude differences |
| GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| } |
| else // no valid target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| } |
| else // no target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| //Calculate PID-components of the controller |
| // D-Part |
| D_North = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.velnorth)/512; |
| D_East = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.veleast)/512; |
| // P-Part |
| P_North = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_North)/2048; |
| P_East = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_East)/2048; |
| // I-Part |
| I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| I_East = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| // combine P & I |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
| } |
| // combine PI- and D-Part |
| PID_North += D_North; |
| PID_East += D_East; |
| // scale combination with gain. |
| PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| PID_East = (PID_East * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| // GPS to nick and roll settings |
| // A positive nick angle moves head downwards (flying forward). |
| // A positive roll angle tilts left side downwards (flying left). |
| // If compass heading is 0 the head of the copter is in north direction. |
| // A positive nick angle will fly to north and a positive roll angle will fly to west. |
| // In case of a positive north deviation/velocity the |
| // copter should fly to south (negative nick). |
| // In case of a positive east position deviation and a positive east velocity the |
| // copter should fly to west (positive roll). |
| // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
| // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| // limit resulting GPS control vector |
| GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| GPSStickNick = (int16_t)PID_Nick; |
| GPSStickRoll = (int16_t)PID_Roll; |
| } |
| else // invalid GPS data or bad compass reading |
| { |
| GPS_Neutral(); // do nothing |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| } |
| void GPS_Main(void) { |
| static uint8_t GPS_P_Delay = 0; |
| static uint16_t beep_rythm = 0; |
| GPS_UpdateParameter(); |
| // store home position if start of flight flag is set |
| if(MKFlags & MKFLAG_CALIBRATE) { |
| if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |
| } |
| switch(GPSInfo.status) { |
| case INVALID: // invalid gps data |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) { |
| BeepTime = 100; // beep if signal is neccesary |
| } |
| break; |
| case PROCESSED: // if gps data are already processed do nothing |
| // downcount timeout |
| if(GPSTimeout) GPSTimeout--; |
| // if no new data arrived within timeout set current data invalid |
| // and therefore disable GPS |
| else |
| { |
| GPS_Neutral(); |
| GPSInfo.status = INVALID; |
| } |
| break; |
| case NEWDATA: // new valid data from gps device |
| // if the gps data quality is good |
| beep_rythm++; |
| if (GPS_IsSignalOK()) |
| { |
| switch(FlightMode) // check what's to do |
| { |
| case GPS_FLIGHT_MODE_FREE: |
| // update hold position to current gps position |
| GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad |
| // disable gps control |
| GPS_Neutral(); |
| break; |
| case GPS_FLIGHT_MODE_AID: |
| if(HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| // update hold point to current gps position |
| GPS_SetCurrPosition(&HoldPosition); |
| // disable gps control |
| GPS_Neutral(); |
| GPS_P_Delay = 0; |
| } |
| else // GPS control active |
| { |
| if(GPS_P_Delay < 7) |
| { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position |
| GPS_PIDController(NULL); // activates only the D-Part |
| } |
| else GPS_PIDController(&HoldPosition);// activates the P&D-Part |
| } |
| } |
| else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| break; |
| case GPS_FLIGHT_MODE_HOME: |
| if(HomePosition.Status != INVALID) |
| { |
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| GPS_SetCurrPosition(&HoldPosition); |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HomePosition); |
| } |
| } |
| else // bad home position |
| { |
| BeepTime = 50; // signal invalid home position |
| // try to hold at least the position as a fallback option |
| if (HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HoldPosition); |
| } |
| } |
| else |
| { // try to catch a valid hold position |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| } |
| break; // eof TSK_HOME |
| default: // unhandled task |
| GPS_Neutral(); |
| break; // eof default |
| } // eof switch GPS_Task |
| } // eof gps data quality is good |
| else // gps data quality is bad |
| { // disable gps control |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| } |
| } |
| // set current data as processed to avoid further calculations on the same gps data |
| GPSInfo.status = PROCESSED; |
| break; |
| } // eof GPSInfo.status |
| } |
| /branches/dongfang_FC_rewrite/backup/gps.h |
|---|
| 0,0 → 1,9 |
| #ifndef _GPS_H |
| #define _GPS_H |
| #include <inttypes.h> |
| extern void GPS_Main(void); |
| #endif //_GPS_H |
| /branches/dongfang_FC_rewrite/backup/gyro.h |
|---|
| 0,0 → 1,10 |
| /* |
| * Common procedures for all gyro types: One to initialize them and one to calibrate them. |
| * Calibration would be something like: |
| * FC 1.3 hardware: Searching the DAC values that return neutral readings. |
| * FC 2.0 hardware: Nothing to do. |
| * InvenSense hardware: Output a pulse on the AUTO_ZERO line. |
| */ |
| void gyro_init(void); |
| void gyro_calibrate(); |
| /branches/dongfang_FC_rewrite/backup/heightControl.c |
|---|
| 0,0 → 1,168 |
| #include <inttypes.h> |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "controlMixer.h" |
| // for digital / LED debug. |
| #include "output.h" |
| // For scope debugging only! |
| #include "rc.h" |
| #define INTEGRAL_LIMIT 100000 |
| /* |
| #define DEBUGINTEGRAL 0 |
| #define DEBUGDIFFERENTIAL 0 |
| #define DEBUGHOVERTHROTTLE 0 |
| #define DEBUGHEIGHTSWITCH 0 |
| */ |
| #define LATCH_TIME 40 |
| int32_t groundPressure; |
| int32_t targetHeight; |
| int32_t rampedTargetHeight; |
| #define DEFAULT_HOVERTHROTTLE 50 |
| int32_t stronglyFilteredHeightDiff = 0; |
| uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
| uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
| #define HOVERTHROTTLEFILTER 25 |
| uint8_t heightRampingTimer = 0; |
| int32_t maxHeight; |
| int32_t iHeight; |
| /* |
| These parameters are free to take: |
| uint8_t HeightMinGas; // Value : 0-100 |
| uint8_t HeightD; // Value : 0-250 |
| uint8_t MaxHeight; // Value : 0-32 |
| uint8_t HeightP; // Value : 0-32 |
| uint8_t Height_Gain; // Value : 0-50 |
| uint8_t Height_ACC_Effect; // Value : 0-250 |
| */ |
| int32_t getHeight(void) { |
| return groundPressure - filteredAirPressure; |
| } |
| void HC_setGround(void) { |
| groundPressure = filteredAirPressure; |
| // This should also happen when height control is enabled in-flight. |
| rampedTargetHeight = getHeight(); |
| maxHeight = 0; |
| iHeight = 0; |
| } |
| void HC_update(void) { |
| int32_t height = getHeight(); |
| static uint8_t setHeightLatch = 0; |
| if (height > maxHeight) |
| maxHeight = height; |
| if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
| DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
| if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
| if (setHeightLatch <= LATCH_TIME) { |
| if (setHeightLatch == LATCH_TIME) { |
| targetHeight = height; |
| DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
| } |
| setHeightLatch++; |
| } |
| } else { |
| setHeightLatch = 0; |
| DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
| } |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
| targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
| } |
| if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) { |
| heightRampingTimer = 0; |
| if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
| rampedTargetHeight += staticParams.Height_Gain; |
| } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
| rampedTargetHeight -= staticParams.Height_Gain; |
| } |
| } |
| //DebugOut.Analog[16] = (int16_t)(height / 10); |
| //DebugOut.Analog[17] = (int16_t)(rampedTargetHeight / 10); |
| } |
| // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| // ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH |
| // ParamSet.BitConfig & CFG_HEIGHT_3SWITCH |
| // takes 180-200 usec (with integral term). That is too heavy!!! |
| // takes 100 usec without integral term. |
| uint16_t HC_getThrottle(uint16_t throttle) { |
| int32_t height = getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| static int32_t lastHeight; |
| int16_t dHeight = height - lastHeight; |
| lastHeight = height; |
| // DebugOut.Analog[20] = dHeight; |
| // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
| // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| // iHeight += heightError; |
| if (dHeight > 0) { |
| DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF; |
| DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| } else if (dHeight < 0) { |
| DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF; |
| DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF; |
| } |
| /* |
| if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
| else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
| else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
| else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
| */ |
| int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD; |
| // the "minGas" is now a limit for how much up / down the throttle can be varied |
| if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas; |
| else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas; |
| //DebugOut.Analog[18] = dThrottle; |
| //DebugOut.Analog[19] = iHeight / 10000L; |
| // TODO: Eliminate repitition. |
| if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
| // If switch in use, we only apply height control when switch is also on. |
| if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
| throttle += dThrottle; |
| } |
| } else { |
| // Switch not in use - just apply height control. |
| throttle += dThrottle; |
| } |
| } |
| /* Experiment: Find hover-throttle */ |
| stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
| stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER; |
| if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) { |
| hoverThrottle = stronglyFilteredThrottle; |
| DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
| DebugOut.Analog[18] = hoverThrottle; |
| } else |
| DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
| return throttle; |
| } |
| /branches/dongfang_FC_rewrite/backup/heightControl.h |
|---|
| 0,0 → 1,4 |
| void HC_setGround(void); |
| void HC_update(void); |
| uint16_t HC_getThrottle(uint16_t throttle); |
| /branches/dongfang_FC_rewrite/backup/invenSense.c |
|---|
| 0,0 → 1,42 |
| #include "invenSense.h" |
| #include "timer0.h" |
| #include "configuration.h" |
| #include <avr/io.h> |
| /* |
| * Configuration for my prototype board with InvenSense gyros. |
| * The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
| */ |
| const uint8_t GYRO_REVERSED[3] = {0,0,0}; |
| const uint8_t ACC_REVERSED[3] = {0,0,1}; |
| #define AUTOZERO_PORT PORTD |
| #define AUTOZERO_DDR DDRD |
| #define AUTOZERO_BIT 5 |
| void gyro_calibrate() { |
| // If port not already set to output and high, do it. |
| if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) { |
| AUTOZERO_PORT |= (1<<AUTOZERO_BIT); |
| AUTOZERO_DDR |= (1<<AUTOZERO_BIT); |
| Delay_ms(100); |
| } |
| // Make a pulse on the auto-zero output line. |
| AUTOZERO_PORT &= ~(1<<AUTOZERO_BIT); |
| Delay_ms(1); |
| AUTOZERO_PORT |= (1<<AUTOZERO_BIT); |
| // Delay_ms(10); |
| Delay_ms_Mess(100); |
| } |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 3; |
| staticParams.GyroAccFactor = 1; |
| staticParams.DriftComp = 10; |
| // Not used. |
| staticParams.AngleTurnOverPitch = 85; |
| staticParams.AngleTurnOverRoll = 85; |
| } |
| /branches/dongfang_FC_rewrite/backup/invenSense.h |
|---|
| 0,0 → 1,30 |
| /* |
| #ifndef _INVENSENSE_H |
| #define _INVENSENSE_H |
| #include "sensors.h" |
| #define GYRO_HW_NAME "ISens" |
| / * |
| * The InvenSense gyros have a lower sensitivity than for example the ADXRS610s on the FC 2.0 ME, |
| * but they have a wider range too. |
| * 2mV/deg/s gyros and no amplifiers: |
| * H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
| * / |
| #define GYRO_HW_FACTOR 0.6827f |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 0.93f |
| / * |
| * Same for yaw. |
| * / |
| #define GYRO_YAW_CORRECTION 0.97f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/backup/main.c |
|---|
| 0,0 → 1,305 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/boot.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <util/delay.h> |
| #include "timer0.h" |
| #include "timer2.h" |
| #include "uart0.h" |
| #include "uart1.h" |
| #include "output.h" |
| #include "menu.h" |
| #include "attitude.h" |
| #include "flight.h" |
| #include "controlMixer.h" |
| #include "rc.h" |
| #include "analog.h" |
| #include "configuration.h" |
| #include "printf_P.h" |
| #include "twimaster.h" |
| #ifdef USE_NAVICTRL |
| #include "spi.h" |
| #endif |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| #include "eeprom.h" |
| int16_t main (void) { |
| uint16_t timer; |
| // disable interrupts global |
| cli(); |
| // analyze hardware environment |
| CPUType = getCPUType(); |
| BoardRelease = getBoardRelease(); |
| // disable watchdog |
| MCUSR &=~(1<<WDRF); |
| WDTCSR |= (1<<WDCE)|(1<<WDE); |
| WDTCSR = 0; |
| // PPM_in[CH_THROTTLE] = 0; |
| // Why??? They are already initialized to 0. |
| // stickPitch = stickRoll = stickYaw = 0; |
| RED_OFF; |
| // initalize modules |
| output_init(); |
| timer0_init(); |
| timer2_init(); |
| usart0_Init(); |
| if(CPUType == ATMEGA644P) usart1_Init(); |
| RC_Init(); |
| analog_init(); |
| I2C_init(); |
| #ifdef USE_NAVICTRL |
| SPI_MasterInit(); |
| #endif |
| #ifdef USE_MK3MAG |
| MK3MAG_Init(); |
| #endif |
| // enable interrupts global |
| sei(); |
| printf("\n\r==================================="); |
| printf("\n\rFlightControl"); |
| printf("\n\rHardware: Custom"); |
| printf("\r\n CPU: Atmega644"); |
| if(CPUType == ATMEGA644P) |
| printf("p"); |
| printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
| printf("\n\r==================================="); |
| // Parameter Set handling |
| ParamSet_Init(); |
| // Wait for a short time (otherwise the RC channel check won't work below) |
| // timer = SetDelay(500); |
| // while(!CheckDelay(timer)); |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = SetDelay(200); |
| OUTPUT_ON(0); |
| GRN_OFF; |
| RED_ON; |
| while(!CheckDelay(timer)); |
| timer = SetDelay(200); |
| OUTPUT_OFF(0); |
| OUTPUT_ON(1); |
| RED_OFF; |
| GRN_ON; |
| while(!CheckDelay(timer)); |
| timer = SetDelay(200); |
| while(!CheckDelay(timer)); |
| OUTPUT_OFF(1); |
| twi_diagnostics(); |
| printf("\n\r==================================="); |
| /* |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
| { |
| printf("\n\rCalibrating air pressure sensor.."); |
| timer = SetDelay(1000); |
| SearchAirPressureOffset(); |
| while (!CheckDelay(timer)); |
| printf("OK\n\r"); |
| } |
| */ |
| #ifdef USE_NAVICTRL |
| printf("\n\rSupport for NaviCtrl"); |
| #ifdef USE_RC_DSL |
| printf("\r\nSupport for DSL RC at 2nd UART"); |
| #endif |
| #ifdef USE_RC_SPECTRUM |
| printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
| #endif |
| #endif |
| #ifdef USE_MK3MAG |
| printf("\n\rSupport for MK3MAG Compass"); |
| #endif |
| #if (defined (USE_MK3MAG)) |
| if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
| else printf("\n\rSupport for GPS at 1st UART"); |
| #endif |
| controlMixer_setNeutral(); |
| // Cal. attitude sensors and reset integrals. |
| attitude_setNeutral(); |
| Servo_On(); |
| // Init flight parameters |
| flight_setNeutral(); |
| // RED_OFF; |
| beep(2000); |
| printf("\n\rControl: "); |
| if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
| else printf("Neutral (ACC-Mode)"); |
| printf("\n\n\r"); |
| LCD_Clear(); |
| I2CTimeout = 5000; |
| while (1) { |
| if(runFlightControl && analogDataReady) { // control interval |
| runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
| J4HIGH; |
| flight_control(); |
| J4LOW; |
| /* |
| * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
| * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
| * the test throttle vector. If no testing, stop all motors. |
| */ |
| // Obsoleted. |
| // transmitMotorThrottleData(); |
| RED_OFF; |
| /* |
| Does not belong here. Instead, external control should be ignored in |
| controlMixer if there was no new data from there for some time. |
| if(externalControlActive) externalControlActive--; |
| else { |
| externalControl.config = 0; |
| externalStickPitch = 0; |
| externalStickRoll = 0; |
| externalStickYaw = 0; |
| } |
| */ |
| /* |
| Does not belong here. |
| if(RC_Quality) RC_Quality--; |
| */ |
| /* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
| #ifdef USE_NAVICTRL |
| if(NCDataOkay) { |
| if(--NCDataOkay == 0) // no data from NC |
| { // set gps control sticks neutral |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| NCSerialDataOkay = 0; |
| } |
| } |
| #endif |
| */ |
| if(!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
| RED_ON; |
| if(!I2CTimeout) { |
| I2C_Reset(); |
| I2CTimeout = 5; |
| } |
| } else { |
| RED_OFF; |
| } |
| // Allow Serial Data Transmit if motors must not updated or motors are not running |
| if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| usart0_TransmitTxData(); |
| } |
| usart0_ProcessRxData(); |
| if(CheckDelay(timer)) { |
| if (UBat <= UBAT_AT_5V) { |
| // Do nothing. The voltage on the input side of the regulator is <5V; |
| // we must be running off USB power. Keep it quiet. |
| } else if(UBat < staticParams.LowVoltageWarning) { |
| beepBatteryAlarm(); |
| } |
| #ifdef USE_NAVICTRL |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| #endif |
| timer = SetDelay(20); // every 20 ms |
| } |
| output_update(); |
| } |
| #ifdef USE_NAVICTRL |
| if(!SendSPI) { |
| // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
| // within the SPI_TransmitByte() routine the value is set to 4. |
| // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
| // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
| SPI_TransmitByte(); |
| } |
| #endif |
| } |
| return (1); |
| } |
| /branches/dongfang_FC_rewrite/backup/main.h |
|---|
| 0,0 → 1,3 |
| #ifndef _MAIN_H |
| #define _MAIN_H |
| #endif //_MAIN_H |
| /branches/dongfang_FC_rewrite/backup/makefile |
|---|
| 0,0 → 1,524 |
| #-------------------------------------------------------------------- |
| # MCU name |
| MCU = atmega644p |
| F_CPU = 20000000 |
| #------------------------------------------------------------------- |
| VERSION_MAJOR = 0 |
| VERSION_MINOR = 74 |
| VERSION_PATCH = 100 |
| VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
| VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
| NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
| #------------------------------------------------------------------- |
| #OPTIONS |
| # Use one of the extensions for a gps solution |
| EXT = NAVICTRL |
| #EXT = MK3MAG |
| # Use optional one the RCs if EXT = NAVICTRL has been used |
| #RC = DSL |
| #RC = SPECTRUM |
| GYRO=ENC-03_FC1.3 |
| GYRO_HW_NAME=ENC |
| GYRO_HW_FACTOR=1.304f |
| GYRO_PITCHROLL_CORRECTION=1.11f |
| GYRO_YAW_CORRECTION=1.28f |
| #GYRO=ADXRS610_FC2.0 |
| #GYRO_HW_NAME=ADXR |
| #GYRO_HW_FACTOR=1.2288f |
| #GYRO_PITCHROLL_CORRECTION=1.0f |
| #GYRO_YAW_CORRECTION=1.0f |
| #GYRO=invenSense |
| #GYRO_HW_NAME=Isense |
| #GYRO_HW_FACTOR=0.6827f |
| #GYRO_PITCHROLL_CORRECTION=0.93f |
| #GYRO_YAW_CORRECTION=0.97f |
| #GYRO= |
| #GYRO=invenSense |
| #------------------------------------------------------------------- |
| # get SVN revision |
| REV=0001 |
| #$(shell sh -c "cat .svn/entries | sed -n '4p'") |
| ifeq ($(MCU), atmega644) |
| FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
| HEX_NAME = MEGA644_$(EXT)_$(RC)_$(GYRO) |
| endif |
| ifeq ($(MCU), atmega644p) |
| FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
| HEX_NAME = MEGA644p_$(EXT)_$(RC)_$(GYRO) |
| endif |
| ifeq ($(F_CPU), 16000000) |
| QUARZ = 16MHZ |
| endif |
| ifeq ($(F_CPU), 20000000) |
| QUARZ = 20MHZ |
| endif |
| # Output format. (can be srec, ihex, binary) |
| FORMAT = ihex |
| # Target file name (without extension). |
| ifeq ($(VERSION_PATCH), 0) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)a_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 1) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)b_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 2) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 3) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 4) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 5) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 6) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 7) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 8) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 9) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 10) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 11) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)l_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 12) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 13) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 14) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)o_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 15) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)p_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 16) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)q_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 17) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 100) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)df_SVN$(REV) |
| endif |
| # Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
| # (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
| OPT = 2 |
| #OPT = s |
| ########################################################################################################## |
| # List C source files here. (C dependencies are automatically generated.) |
| SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c |
| SRC += externalControl.c GPSControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
| SRC += eeprom.c uart1.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
| ifeq ($(EXT), MK3MAG) |
| SRC += mk3mag.c gps.c ubx.c |
| #mymath.c |
| endif |
| ifeq ($(EXT), NAVICTRL) |
| SRC += spi.c |
| ifeq ($(RC), DSL) |
| SRC += dsl.c |
| endif |
| ifeq ($(RC), SPECTRUM) |
| SRC += spectrum.c |
| endif |
| endif |
| ########################################################################################################## |
| # List Assembler source files here. |
| # Make them always end in a capital .S. Files ending in a lowercase .s |
| # will not be considered source files but generated files (assembler |
| # output from the compiler), and will be deleted upon "make clean"! |
| # Even though the DOS/Win* filesystem matches both .s and .S the same, |
| # it will preserve the spelling of the filenames, and gcc itself does |
| # care about how the name is spelled on its command-line. |
| ASRC = |
| # List any extra directories to look for include files here. |
| # Each directory must be seperated by a space. |
| EXTRAINCDIRS = |
| # Optional compiler flags. |
| # -g: generate debugging information (for GDB, or for COFF conversion) |
| # -O*: optimization level |
| # -f...: tuning, see gcc manual and avr-libc documentation |
| # -Wall...: warning level |
| # -Wa,...: tell GCC to pass this to the assembler. |
| # -ahlms: create assembler listing |
| CFLAGS = -O$(OPT) \ |
| -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \ |
| -Wall -Wstrict-prototypes \ |
| -DGYRO=$(GYRO) \ |
| -Wa,-adhlns=$(<:.c=.lst) \ |
| $(patsubst %,-I%,$(EXTRAINCDIRS)) |
| # Set a "language standard" compiler flag. |
| # Unremark just one line below to set the language standard to use. |
| # gnu99 = C99 + GNU extensions. See GCC manual for more information. |
| #CFLAGS += -std=c89 |
| #CFLAGS += -std=gnu89 |
| #CFLAGS += -std=c99 |
| CFLAGS += -std=gnu99 |
| CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION} |
| ifeq ($(EXT), MK3MAG) |
| CFLAGS += -DUSE_MK3MAG |
| endif |
| ifeq ($(EXT), NAVICTRL) |
| CFLAGS += -DUSE_NAVICTRL |
| ifeq ($(RC), DSL) |
| CFLAGS += -DUSE_RC_DSL |
| endif |
| ifeq ($(RC), SPECTRUM) |
| CFLAGS += -DUSE_RC_SPECTRUM |
| endif |
| endif |
| ifeq ($(SETUP), QUADRO) |
| CFLAGS += -DUSE_QUADRO |
| endif |
| ifeq ($(SETUP), OCTO) |
| CFLAGS += -DUSE_OCTO |
| endif |
| ifeq ($(SETUP), OCTO2) |
| CFLAGS += -DUSE_OCTO2 |
| endif |
| ifeq ($(SETUP), OCTO3) |
| CFLAGS += -DUSE_OCTO3 |
| endif |
| # Optional assembler flags. |
| # -Wa,...: tell GCC to pass this to the assembler. |
| # -ahlms: create listing |
| # -gstabs: have the assembler create line number information; note that |
| # for use in COFF files, additional information about filenames |
| # and function names needs to be present in the assembler source |
| # files -- see avr-libc docs [FIXME: not yet described there] |
| ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs |
| # Optional linker flags. |
| # -Wl,...: tell GCC to pass this to linker. |
| # -Map: create map file |
| # --cref: add cross reference to map file |
| LDFLAGS = -Wl,-Map=$(TARGET).map,--cref |
| # Additional libraries |
| # Minimalistic printf version |
| #LDFLAGS += -Wl,-u,vfprintf -lprintf_min |
| # Floating point printf version (requires -lm below) |
| #LDFLAGS += -Wl,-u,vfprintf -lprintf_flt |
| # -lm = math library |
| LDFLAGS += -lm |
| ##LDFLAGS += -T./linkerfile/avr5.x |
| # Programming support using avrdude. Settings and variables. |
| # Programming hardware: alf avr910 avrisp bascom bsd |
| # dt006 pavr picoweb pony-stk200 sp12 stk200 stk500 |
| # |
| # Type: avrdude -c ? |
| # to get a full listing. |
| # |
| #AVRDUDE_PROGRAMMER = dt006 |
| #AVRDUDE_PROGRAMMER = stk200 |
| #AVRDUDE_PROGRAMMER = ponyser |
| AVRDUDE_PROGRAMMER = avrispv2 |
| #falls Ponyser ausgewählt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden |
| #AVRDUDE_PORT = com1 # programmer connected to serial device |
| #AVRDUDE_PORT = lpt1 # programmer connected to parallel port |
| AVRDUDE_PORT = usb # programmer connected to USB |
| #AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex |
| AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS) |
| #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep |
| #avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex |
| AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) |
| # Uncomment the following if you want avrdude's erase cycle counter. |
| # Note that this counter needs to be initialized first using -Yn, |
| # see avrdude manual. |
| #AVRDUDE_ERASE += -y |
| # Uncomment the following if you do /not/ wish a verification to be |
| # performed after programming the device. |
| AVRDUDE_FLAGS += -V |
| # Increase verbosity level. Please use this when submitting bug |
| # reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> |
| # to submit bug reports. |
| #AVRDUDE_FLAGS += -v -v |
| # --------------------------------------------------------------------------- |
| # Define directories, if needed. |
| DIRAVR = c:/winavr |
| DIRAVRBIN = $(DIRAVR)/bin |
| DIRAVRUTILS = $(DIRAVR)/utils/bin |
| DIRINC = . |
| DIRLIB = $(DIRAVR)/avr/lib |
| # Define programs and commands. |
| SHELL = sh |
| CC = avr-gcc |
| OBJCOPY = avr-objcopy |
| OBJDUMP = avr-objdump |
| SIZE = avr-size |
| # Programming support using avrdude. |
| AVRDUDE = avrdude |
| REMOVE = rm -f |
| COPY = cp |
| HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex |
| ELFSIZE = $(SIZE) -A $(TARGET).elf |
| # Define Messages |
| # English |
| MSG_ERRORS_NONE = Errors: none |
| MSG_BEGIN = -------- begin -------- |
| MSG_END = -------- end -------- |
| MSG_SIZE_BEFORE = Size before: |
| MSG_SIZE_AFTER = Size after: |
| MSG_COFF = Converting to AVR COFF: |
| MSG_EXTENDED_COFF = Converting to AVR Extended COFF: |
| MSG_FLASH = Creating load file for Flash: |
| MSG_EEPROM = Creating load file for EEPROM: |
| MSG_EXTENDED_LISTING = Creating Extended Listing: |
| MSG_SYMBOL_TABLE = Creating Symbol Table: |
| MSG_LINKING = Linking: |
| MSG_COMPILING = Compiling: |
| MSG_ASSEMBLING = Assembling: |
| MSG_CLEANING = Cleaning project: |
| # Define all object files. |
| OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) |
| # Define all listing files. |
| LST = $(ASRC:.S=.lst) $(SRC:.c=.lst) |
| # Combine all necessary flags and optional flags. |
| # Add target processor to flags. |
| #ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS) |
| ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) |
| ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) |
| # Default target. |
| all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \ |
| $(TARGET).lss $(TARGET).sym sizeafter finished end |
| # Eye candy. |
| # AVR Studio 3.x does not check make's exit code but relies on |
| # the following magic strings to be generated by the compile job. |
| begin: |
| @echo |
| @echo $(MSG_BEGIN) |
| finished: |
| @echo $(MSG_ERRORS_NONE) |
| end: |
| @echo $(MSG_END) |
| @echo |
| # Display size of file. |
| sizebefore: |
| @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi |
| sizeafter: |
| @if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi |
| # Display compiler version information. |
| gccversion : |
| @$(CC) --version |
| # Convert ELF to COFF for use in debugging / simulating in |
| # AVR Studio or VMLAB. |
| COFFCONVERT=$(OBJCOPY) --debugging \ |
| --change-section-address .data-0x800000 \ |
| --change-section-address .bss-0x800000 \ |
| --change-section-address .noinit-0x800000 \ |
| --change-section-address .eeprom-0x810000 |
| coff: $(TARGET).elf |
| @echo |
| @echo $(MSG_COFF) $(TARGET).cof |
| $(COFFCONVERT) -O coff-avr $< $(TARGET).cof |
| extcoff: $(TARGET).elf |
| @echo |
| @echo $(MSG_EXTENDED_COFF) $(TARGET).cof |
| $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof |
| # Program the device. |
| program: $(TARGET).hex $(TARGET).eep |
| $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) |
| # Create final output files (.hex, .eep) from ELF output file. |
| %.hex: %.elf |
| @echo |
| @echo $(MSG_FLASH) $@ |
| $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ |
| %.eep: %.elf |
| @echo |
| @echo $(MSG_EEPROM) $@ |
| -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ |
| --change-section-lma .eeprom=0 -O $(FORMAT) $< $@ |
| # Create extended listing file from ELF output file. |
| %.lss: %.elf |
| @echo |
| @echo $(MSG_EXTENDED_LISTING) $@ |
| $(OBJDUMP) -h -S $< > $@ |
| # Create a symbol table from ELF output file. |
| %.sym: %.elf |
| @echo |
| @echo $(MSG_SYMBOL_TABLE) $@ |
| avr-nm -n $< > $@ |
| # Link: create ELF output file from object files. |
| .SECONDARY : $(TARGET).elf |
| .PRECIOUS : $(OBJ) |
| %.elf: $(OBJ) |
| @echo |
| @echo $(MSG_LINKING) $@ |
| $(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS) |
| # Compile: create object files from C source files. |
| %.o : %.c |
| @echo |
| @echo $(MSG_COMPILING) $< |
| $(CC) -c $(ALL_CFLAGS) $< -o $@ |
| # Compile: create assembler files from C source files. |
| %.s : %.c |
| $(CC) -S $(ALL_CFLAGS) $< -o $@ |
| # Assemble: create object files from assembler source files. |
| %.o : %.S |
| @echo |
| @echo $(MSG_ASSEMBLING) $< |
| $(CC) -c $(ALL_ASFLAGS) $< -o $@ |
| # Target: clean project. |
| clean: begin clean_list finished end |
| clean_list : |
| @echo |
| @echo $(MSG_CLEANING) |
| # $(REMOVE) $(TARGET).hex |
| $(REMOVE) $(TARGET).eep |
| $(REMOVE) $(TARGET).obj |
| $(REMOVE) $(TARGET).cof |
| $(REMOVE) $(TARGET).elf |
| $(REMOVE) $(TARGET).map |
| $(REMOVE) $(TARGET).obj |
| $(REMOVE) $(TARGET).a90 |
| $(REMOVE) $(TARGET).sym |
| $(REMOVE) $(TARGET).lnk |
| $(REMOVE) $(TARGET).lss |
| $(REMOVE) $(OBJ) |
| $(REMOVE) $(LST) |
| $(REMOVE) $(SRC:.c=.s) |
| $(REMOVE) $(SRC:.c=.d) |
| # Automatically generate C source code dependencies. |
| # (Code originally taken from the GNU make user manual and modified |
| # (See README.txt Credits).) |
| # |
| # Note that this will work with sh (bash) and sed that is shipped with WinAVR |
| # (see the SHELL variable defined above). |
| # This may not work with other shells or other seds. |
| # |
| %.d: %.c |
| set -e; $(CC) -MM $(ALL_CFLAGS) $< \ |
| | sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \ |
| [ -s $@ ] || rm -f $@ |
| # Remove the '-' if you want to see the dependency files generated. |
| -include $(SRC:.c=.d) |
| # Listing of phony targets. |
| .PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \ |
| clean clean_list program |
| /branches/dongfang_FC_rewrite/backup/menu.c |
|---|
| 0,0 → 1,291 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include <inttypes.h> |
| #include "eeprom.h" |
| #include "timer2.h" |
| #include "rc.h" |
| #include "externalControl.h" |
| #include "uart0.h" |
| #include "printf_P.h" |
| #include "analog.h" |
| #include "twimaster.h" |
| #include "attitude.h" |
| #if (defined (USE_MK3MAG)) |
| #include "ubx.h" |
| #endif |
| #if (!defined (USE_MK3MAG)) |
| uint8_t MaxMenuItem = 13; |
| #else |
| #ifdef USE_MK3MAG |
| uint8_t MaxMenuItem = 14; |
| #endif |
| #endif |
| uint8_t MenuItem = 0; |
| uint8_t RemoteKeys = 0; |
| #define KEY1 0x01 |
| #define KEY2 0x02 |
| #define KEY3 0x04 |
| #define KEY4 0x08 |
| #define KEY5 0x10 |
| #define DISPLAYBUFFSIZE 80 |
| int8_t DisplayBuff[DISPLAYBUFFSIZE] = "Hello World"; |
| uint8_t DispPtr = 0; |
| /************************************/ |
| /* Clear LCD Buffer */ |
| /************************************/ |
| void LCD_Clear(void) |
| { |
| uint8_t i; |
| for( i = 0; i < DISPLAYBUFFSIZE; i++) DisplayBuff[i] = ' '; |
| } |
| /************************************/ |
| /* Update Menu on LCD */ |
| /************************************/ |
| // Display with 20 characters in 4 lines |
| void LCD_PrintMenu(void) { |
| if(RemoteKeys & KEY1) { |
| if(MenuItem) MenuItem--; |
| else MenuItem = MaxMenuItem; |
| } |
| if(RemoteKeys & KEY2) { |
| if(MenuItem == MaxMenuItem) MenuItem = 0; |
| else MenuItem++; |
| } |
| if((RemoteKeys & KEY1) && (RemoteKeys & KEY2)) MenuItem = 0; |
| LCD_Clear(); |
| if(MenuItem > MaxMenuItem) MenuItem = MaxMenuItem; |
| // print menu item number in the upper right corner |
| if(MenuItem < 10) { |
| LCD_printfxy(17,0,"[%i]",MenuItem); |
| } else { |
| LCD_printfxy(16,0,"[%i]",MenuItem); |
| } |
| switch(MenuItem) { |
| case 0:// Version Info Menu Item |
| LCD_printfxy(0,0,"+ MikroKopter +"); |
| LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
| LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), Mixer.Name); |
| if(I2CTimeout < 6) { |
| LCD_printfxy(0,3,"I2C Error!!!"); |
| } else if (missingMotor) { |
| LCD_printfxy(0,3,"Missing BL-Ctrl:%d", missingMotor); |
| } |
| else LCD_printfxy(0,3,"(c) Holger Buss"); |
| break; |
| /* |
| case 1:// Height Control Menu Item |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
| LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
| LCD_printfxy(0,2,"Air Press.:%5i",0); |
| LCD_printfxy(0,3,"Offset :%5i",0); |
| } |
| else |
| { |
| LCD_printfxy(0,1,"No "); |
| LCD_printfxy(0,2,"Height Control"); |
| } |
| break; |
| */ |
| case 2:// Attitude Menu Item |
| LCD_printfxy(0,0,"Attitude"); |
| LCD_printfxy(0,1,"Nick: %5i", angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0,2,"Roll: %5i", angle[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0,3,"Heading: %5i", compassHeading); |
| break; |
| case 3:// Remote Control Channel Menu Item |
| LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]); |
| LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]); |
| LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]); |
| LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]); |
| break; |
| case 4:// Remote Control Mapping Menu Item |
| LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[staticParams.ChannelAssignment[CH_PITCH]],PPM_in[staticParams.ChannelAssignment[CH_ROLL]]); |
| LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]],PPM_in[staticParams.ChannelAssignment[CH_YAW]]); |
| LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS]],PPM_in[staticParams.ChannelAssignment[CH_POTS+1]]); |
| LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS+2]],PPM_in[staticParams.ChannelAssignment[CH_POTS+3]]); |
| break; |
| /* |
| case 5:// Gyro Sensor Menu Item |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| switch(BoardRelease) { |
| case 10: |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)", AdValueGyroNick - HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)", AdValueGyroRoll - HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,3,"Yaw %4i (%3i)", AdValueGyroYaw , YawOffset); |
| break; |
| case 11: |
| case 12: |
| case 20: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)",YawOffset - AdValueGyroYaw , YawOffset/2); |
| break; |
| case 13: |
| default: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",YawOffset - AdValueGyroYaw , YawOffset/2, 0); |
| break; |
| } |
| break; |
| case 6:// Acceleration Sensor Menu Item |
| LCD_printfxy(0,0,"ACC - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,2,"Roll %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,3,"Height %4i (%3i)",0,0); |
| break; |
| */ |
| case 7:// Battery Voltage / Remote Control Level |
| LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10); |
| LCD_printfxy(0,2,"RC-Level: %5i",RC_Quality); |
| break; |
| case 8:// Compass Menu Item |
| LCD_printfxy(0,0,"Compass "); |
| LCD_printfxy(0,1,"Course: %5i", compassCourse); |
| LCD_printfxy(0,2,"Heading: %5i", compassHeading); |
| LCD_printfxy(0,3,"OffCourse: %5i", compassOffCourse); |
| break; |
| case 9:// Poti Menu Item |
| LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,variables[0], variables[4]); //PPM24-Extesion |
| LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,variables[1], variables[5]); //PPM24-Extesion |
| LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,variables[2], variables[6]); //PPM24-Extesion |
| LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,variables[3], variables[7]); //PPM24-Extesion |
| break; |
| /* |
| case 10:// Servo Menu Item |
| LCD_printfxy(0,0,"Servo " ); |
| LCD_printfxy(0,1,"Setpoint %3i",dynamicParams.ServoNickControl); |
| LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
| LCD_printfxy(0,3,"Range:%3i-%3i",staticParams.ServoNickMin, staticParams.ServoNickMax); |
| break; |
| */ |
| case 11://Extern Control |
| LCD_printfxy(0,0,"ExternControl " ); |
| LCD_printfxy(0,1,"Ni:%4i Ro:%4i ", externalControl.pitch, externalControl.roll); |
| LCD_printfxy(0,2,"Gs:%4i Ya:%4i ", externalControl.throttle, externalControl.yaw); |
| LCD_printfxy(0,3,"Hi:%4i Cf:%4i ", externalControl.height, externalControl.config); |
| break; |
| case 12://BL Communication errors |
| LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
| LCD_printfxy(0,1," %3d %3d %3d %3d ",Motor[0].Error,Motor[1].Error,Motor[2].Error,Motor[3].Error); |
| LCD_printfxy(0,2," %3d %3d %3d %3d ",Motor[4].Error,Motor[5].Error,Motor[6].Error,Motor[7].Error); |
| LCD_printfxy(0,3," %3d %3d %3d %3d ",Motor[8].Error,Motor[9].Error,Motor[10].Error,Motor[11].Error); |
| break; |
| case 13://BL Overview |
| LCD_printfxy(0,0,"BL-Ctrl found " ); |
| LCD_printfxy(0,1," %c %c %c %c ",Motor[0].Present + '-',Motor[1].Present + '-',Motor[2].Present + '-',Motor[3].Present + '-'); |
| LCD_printfxy(0,2," %c %c %c %c ",Motor[4].Present + '-',Motor[5].Present + '-',Motor[6].Present + '-',Motor[7].Present + '-'); |
| LCD_printfxy(0,3," %c - - - ",Motor[8].Present + '-'); |
| if(Motor[9].Present) LCD_printfxy(4,3,"10"); |
| if(Motor[10].Present) LCD_printfxy(8,3,"11"); |
| if(Motor[11].Present) LCD_printfxy(12,3,"12"); |
| break; |
| #if (defined (USE_MK3MAG)) |
| case 14://GPS Lat/Lon coords |
| if (GPSInfo.status == INVALID) { |
| LCD_printfxy(0,0,"No GPS data!"); |
| } else { |
| switch (GPSInfo.satfix) |
| { |
| case SATFIX_NONE: |
| LCD_printfxy(0,0,"Sats: %d Fix: No", GPSInfo.satnum); |
| break; |
| case SATFIX_2D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPSInfo.satnum); |
| break; |
| case SATFIX_3D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPSInfo.satnum); |
| break; |
| default: |
| LCD_printfxy(0,0,"Sats: %d Fix: ??", GPSInfo.satnum); |
| break; |
| } |
| int16_t i1,i2,i3; |
| i1 = (int16_t)(GPSInfo.longitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.latitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.altitude/1000L); |
| i2 = abs((int16_t)(GPSInfo.altitude%1000L)); |
| LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2); |
| } |
| break; |
| #endif |
| default: |
| MaxMenuItem = MenuItem - 1; |
| MenuItem = 0; |
| break; |
| } |
| RemoteKeys = 0; |
| } |
| /branches/dongfang_FC_rewrite/backup/menu.h |
|---|
| 0,0 → 1,17 |
| #ifndef _MENU_H |
| #define _MENU_H |
| #include <inttypes.h> |
| #define DISPLAYBUFFSIZE 80 |
| extern void LCD_PrintMenu(void); |
| extern void LCD_Clear(void); |
| extern int8_t DisplayBuff[DISPLAYBUFFSIZE]; |
| extern uint8_t DispPtr; |
| extern uint8_t MenuItem; |
| extern uint8_t MaxMenuItem; |
| extern uint8_t RemoteKeys; |
| #endif //_MENU_H |
| /branches/dongfang_FC_rewrite/backup/mk3mag.c |
|---|
| 0,0 → 1,131 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <stdlib.h> |
| #include <inttypes.h> |
| #include "timer0.h" |
| // #include "rc.h" |
| #include "attitude.h" |
| #include "eeprom.h" |
| #include "mk3mag.h" |
| uint8_t PWMTimeout = 12; |
| ToMk3Mag_t ToMk3Mag; |
| /*********************************************/ |
| /* Initialize Interface to MK3MAG Compass */ |
| /*********************************************/ |
| void MK3MAG_Init(void) |
| { |
| // Port PC4 connected to PWM output from compass module |
| DDRC &= ~(1<<DDC4); // set as input |
| PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
| PWMTimeout = 0; |
| ToMk3Mag.CalState = 0; |
| ToMk3Mag.Orientation = 1; |
| } |
| /*********************************************/ |
| /* Get PWM from MK3MAG */ |
| /*********************************************/ |
| void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR |
| { |
| static uint16_t PWMCount = 0; |
| static uint16_t BeepDelay = 0; |
| // The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
| // in other words 100us/° with a +1ms offset. |
| // The signal goes low for 65ms between pulses, |
| // so the cycle time is 65mS + the pulse width. |
| // pwm is high |
| if(PINC & (1<<PINC4)) |
| { // If PWM signal is high increment PWM high counter |
| // This counter is incremented by a periode of 102.4us, |
| // i.e. the resoluton of pwm coded heading is approx. 1 deg. |
| PWMCount++; |
| // pwm overflow? |
| if (PWMCount > 400) |
| { |
| if(PWMTimeout) PWMTimeout--; // decrement timeout |
| compassHeading = -1; // unknown heading |
| PWMCount = 0; // reset PWM Counter |
| } |
| } |
| else // pwm is low |
| { // ignore pwm values values of 0 and higher than 37 ms; |
| if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms |
| { |
| if(PWMCount <10) compassHeading = 0; |
| else compassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
| compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
| // 12 * 362 counts * 102.4 us |
| } |
| PWMCount = 0; // reset pwm counter |
| } |
| if(!PWMTimeout) |
| { |
| if(CheckDelay(BeepDelay)) |
| { |
| if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem |
| BeepDelay = SetDelay(100); |
| } |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/mk3mag.h |
|---|
| 0,0 → 1,20 |
| #ifndef _MK3MAG_H |
| #define _MK3MAG_H |
| typedef struct { |
| int16_t Attitude[2]; |
| uint8_t UserParam[2]; |
| uint8_t CalState; |
| uint8_t Orientation; |
| } ToMk3Mag_t; |
| extern ToMk3Mag_t ToMk3Mag; |
| // Initialization |
| void MK3MAG_Init(void); |
| // should be called cyclic to get actual compass heading |
| void MK3MAG_Update(void); |
| #endif //_MK3MAG_H |
| /branches/dongfang_FC_rewrite/backup/mm3.c |
|---|
| 0,0 → 1,532 |
| /* |
| Copyright 2008, by Killagreg |
| This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify |
| it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
| either version 3 of the License, or (at your option) any later version. |
| This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
| without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
| along with this program. If not, see <http://www.gnu.org/licenses/>. |
| Please note: The original implementation was done by Niklas Nold. |
| All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
| */ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <inttypes.h> |
| #include "mm3.h" |
| //#include "mymath.h" |
| #include "timer0.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| #include "printf_P.h" |
| // for compatibility reasons gcc3.x <-> gcc4.x |
| #ifndef SPCR |
| #define SPCR SPCR0 |
| #endif |
| #ifndef SPIE |
| #define SPIE SPIE0 |
| #endif |
| #ifndef SPE |
| #define SPE SPE0 |
| #endif |
| #ifndef DORD |
| #define DORD DORD0 |
| #endif |
| #ifndef MSTR |
| #define MSTR MSTR0 |
| #endif |
| #ifndef CPOL |
| #define CPOL CPOL0 |
| #endif |
| #ifndef CPHA |
| #define CPHA CPHA0 |
| #endif |
| #ifndef SPR1 |
| #define SPR1 SPR01 |
| #endif |
| #ifndef SPR0 |
| #define SPR0 SPR00 |
| #endif |
| #ifndef SPDR |
| #define SPDR SPDR0 |
| #endif |
| #ifndef SPSR |
| #define SPSR SPSR0 |
| #endif |
| #ifndef SPIF |
| #define SPIF SPIF0 |
| #endif |
| #ifndef WCOL |
| #define WCOL WCOL0 |
| #endif |
| #ifndef SPI2X |
| #define SPI2X SPI2X0 |
| #endif |
| // ------------------------- |
| #define MAX_AXIS_VALUE 500 |
| typedef struct |
| { |
| uint8_t STATE; |
| uint16_t DRDY; |
| uint8_t AXIS; |
| int16_t x_axis; |
| int16_t y_axis; |
| int16_t z_axis; |
| } MM3_working_t; |
| // MM3 State Machine |
| #define MM3_STATE_RESET 0 |
| #define MM3_STATE_START_TRANSFER 1 |
| #define MM3_STATE_WAIT_DRDY 2 |
| #define MM3_STATE_DRDY 3 |
| #define MM3_STATE_BYTE2 4 |
| #define MM3_X_AXIS 0x01 |
| #define MM3_Y_AXIS 0x02 |
| #define MM3_Z_AXIS 0x03 |
| #define MM3_PERIOD_32 0x00 |
| #define MM3_PERIOD_64 0x10 |
| #define MM3_PERIOD_128 0x20 |
| #define MM3_PERIOD_256 0x30 |
| #define MM3_PERIOD_512 0x40 |
| #define MM3_PERIOD_1024 0x50 |
| #define MM3_PERIOD_2048 0x60 |
| #define MM3_PERIOD_4096 0x70 |
| #if defined(USE_WALTER_EXT) // walthers board |
| // Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET |
| #define MM3_SS_PORT PORTC //J9->MM3_SS |
| #define MM3_SS_DDR DDRC |
| #define MM3_SS_PIN PC6 |
| #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
| #define MM3_RESET_DDR DDRB |
| #define MM3_RESET_PIN PB2 |
| #elif defined(USE_NICK666) // nick666 version 0.67g |
| #define MM3_SS_PORT PORTD //J5->MM3_SS |
| #define MM3_SS_DDR DDRD |
| #define MM3_SS_PIN PD3 |
| #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
| #define MM3_RESET_DDR DDRB |
| #define MM3_RESET_PIN PB2 |
| #else // killagregs board |
| // Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
| #define MM3_SS_PORT PORTC |
| #define MM3_SS_DDR DDRC |
| #define MM3_SS_PIN PC4 |
| #define MM3_RESET_PORT PORTC |
| #define MM3_RESET_DDR DDRC |
| #define MM3_RESET_PIN PC5 |
| #endif |
| #define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN); |
| #define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN); |
| #define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN); |
| #define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN); |
| MM3_calib_t MM3_calib; |
| volatile MM3_working_t MM3; |
| volatile uint8_t MM3_Timeout = 0; |
| /*********************************************/ |
| /* Initialize Interface to MM3 Compass */ |
| /*********************************************/ |
| void MM3_Init(void) |
| { |
| uint8_t sreg = SREG; |
| cli(); |
| // Configure Pins for SPI |
| // set SCK (PB7), MOSI (PB5) as output |
| DDRB |= (1<<DDB7)|(1<<DDB5); |
| // set MISO (PB6) as input |
| DDRB &= ~(1<<DDB6); |
| // Output Pins MM3_SS ,MM3_RESET |
| MM3_SS_DDR |= (1<<MM3_SS_PIN); |
| MM3_RESET_DDR |= (1<<MM3_RESET_PIN); |
| // set pins permanent to low |
| MM3_SS_PORT &= ~((1<<MM3_SS_PIN)); |
| MM3_RESET_PORT &= ~((1<<MM3_RESET_PIN)); |
| // 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 = (int8_t)GetParamByte(PID_MM3_X_OFF); |
| MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF); |
| MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
| MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
| MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
| MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
| MM3_Timeout = 0; |
| SREG = sreg; |
| } |
| /*********************************************/ |
| /* Get Data from MM3 */ |
| /*********************************************/ |
| void MM3_Update(void) // called every 102.4 µs by timer 0 ISR |
| { |
| switch (MM3.STATE) |
| { |
| case MM3_STATE_RESET: |
| MM3_SS_ON // select slave |
| MM3_RESET_ON // RESET to High, MM3 Reset |
| MM3.STATE = MM3_STATE_START_TRANSFER; |
| return; |
| case MM3_STATE_START_TRANSFER: |
| MM3_RESET_OFF // RESET auf Low (was 102.4 µs at high level) |
| // write to SPDR triggers automatically the transfer MOSI MISO |
| // MM3 Period, + AXIS code |
| switch(MM3.AXIS) |
| { |
| case MM3_X_AXIS: |
| SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
| break; |
| case MM3_Y_AXIS: |
| SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
| break; |
| case MM3_Z_AXIS: |
| SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
| break; |
| default: |
| MM3.AXIS = MM3_X_AXIS; |
| MM3.STATE = MM3_STATE_RESET; |
| return; |
| } |
| // DRDY line is not connected, therefore |
| // wait before reading data back |
| MM3.DRDY = SetDelay(8); // wait 8ms for data ready |
| MM3.STATE = MM3_STATE_WAIT_DRDY; |
| return; |
| case MM3_STATE_WAIT_DRDY: |
| if (CheckDelay(MM3.DRDY)) |
| { |
| // write something into SPDR to trigger data reading |
| SPDR = 0x00; |
| MM3.STATE = MM3_STATE_DRDY; |
| } |
| return; |
| } |
| } |
| /*********************************************/ |
| /* Interrupt SPI transfer complete */ |
| /*********************************************/ |
| ISR(SPI_STC_vect) |
| { |
| static int8_t tmp; |
| int16_t value; |
| switch (MM3.STATE) |
| { |
| // 1st byte received |
| case MM3_STATE_DRDY: |
| tmp = SPDR; // store 1st byte |
| SPDR = 0x00; // trigger transfer of 2nd byte |
| MM3.STATE = MM3_STATE_BYTE2; |
| return; |
| case MM3_STATE_BYTE2: // 2nd byte received |
| value = (int16_t)tmp; // combine the 1st and 2nd byte to a word |
| value <<= 8; // shift 1st byte to MSB-Position |
| value |= (int16_t)SPDR; // add 2nd byte |
| if(abs(value) < MAX_AXIS_VALUE) // ignore spikes |
| { |
| switch (MM3.AXIS) |
| { |
| case MM3_X_AXIS: |
| MM3.x_axis = value; |
| MM3.AXIS = MM3_Y_AXIS; |
| break; |
| case MM3_Y_AXIS: |
| MM3.y_axis = value; |
| MM3.AXIS = MM3_Z_AXIS; |
| break; |
| case MM3_Z_AXIS: |
| MM3.z_axis = value; |
| MM3.AXIS = MM3_X_AXIS; |
| break; |
| default: |
| MM3.AXIS = MM3_X_AXIS; |
| break; |
| } |
| } |
| MM3_SS_OFF // deselect slave |
| MM3.STATE = MM3_STATE_RESET; |
| // Update timeout is called every 102.4 µs. |
| // It takes 2 cycles to write a measurement data request for one axis and |
| // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
| // I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
| // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
| // decremtent the MM3_Timeout every 100 ms. |
| // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
| if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
| { // if all axis measurements give diffrent readings the data should be valid |
| if(MM3_Timeout < 20) MM3_Timeout++; |
| } |
| else // something is very strange here |
| { |
| if(MM3_Timeout ) MM3_Timeout--; |
| } |
| return; |
| default: |
| return; |
| } |
| } |
| /*********************************************/ |
| /* Calibrate Compass */ |
| /*********************************************/ |
| void MM3_Calibrate(void) |
| { |
| static int16_t x_min, x_max, y_min, y_max, z_min, z_max; |
| switch(CompassCalState) |
| { |
| case 1: // change to x-y axis |
| x_min = 10000; |
| x_max = -10000; |
| y_min = 10000; |
| y_max = -10000; |
| z_min = 10000; |
| z_max = -10000; |
| break; |
| case 2: |
| // find Min and Max of the X- and Y-Axis |
| if(MM3.x_axis < x_min) x_min = MM3.x_axis; |
| if(MM3.x_axis > x_max) x_max = MM3.x_axis; |
| if(MM3.y_axis < y_min) y_min = MM3.y_axis; |
| if(MM3.y_axis > y_max) y_max = MM3.y_axis; |
| break; |
| case 3: |
| // change to z-Axis |
| break; |
| case 4: |
| RED_ON; // find Min and Max of the Z-axis |
| if(MM3.z_axis < z_min) z_min = MM3.z_axis; |
| if(MM3.z_axis > z_max) z_max = MM3.z_axis; |
| break; |
| case 5: |
| // calc range 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); |
| // calc 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 |
| SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
| SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
| SetParamByte(PID_MM3_Z_OFF, (uint8_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); |
| CompassCalState = 0; |
| break; |
| default: |
| CompassCalState = 0; |
| break; |
| } |
| } |
| /* |
| 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; |
| RED_OFF; |
| // get maximum and minimum reading of all axis |
| while (measurement) |
| { |
| // reset range markers if yawstick ist leftmost |
| if(PPM_in[staticParams.ChannelAssignment[CH_YAW]] > 100) |
| { |
| x_min = 0; |
| x_max = 0; |
| y_min = 0; |
| y_max = 0; |
| z_min = 0; |
| z_max = 0; |
| } |
| 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) |
| { |
| RED_FLASH; |
| GRN_FLASH; |
| BeepTime = 50; |
| 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 gas is less than 100, stop calibration with a delay of 0.5 seconds |
| if (PPM_in[staticParams.ChannelAssignment[CH_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 |
| SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
| SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
| SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
| SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
| SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
| SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
| } |
| */ |
| /*********************************************/ |
| /* Calculate north direction (heading) */ |
| /*********************************************/ |
| void MM3_Heading(void) |
| { |
| int32_t sin_nick, cos_nick, 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)staticParams.UserParams2[0] + 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; |
| // tilt compensation |
| // calculate sinus cosinus of nick and tilt angle |
| angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); |
| sin_nick = (int32_t)(c_sin_8192(angle)); |
| cos_nick = (int32_t)(c_cos_8192(angle)); |
| angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); |
| sin_roll = (int32_t)(c_sin_8192(angle)); |
| cos_roll = (int32_t)(c_cos_8192(angle)); |
| Hx_corr = Hx * cos_nick; |
| Hx_corr -= Hz * sin_nick; |
| Hx_corr /= 8192; |
| Hy_corr = Hy * cos_roll; |
| Hy_corr += Hz * sin_roll; |
| Hy_corr /= 8192; |
| // calculate Heading |
| heading = c_atan2(Hy_corr, Hx_corr); |
| // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
| // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
| if (heading < 0) heading = -heading; |
| else heading = 360 - heading; |
| } |
| else // MM3_Timeout = 0 i.e now new data from external board |
| { |
| if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
| heading = -1; |
| } |
| // update compass values in fc variables |
| CompassHeading = heading; |
| if (CompassHeading < 0) CompassOffCourse = 0; |
| else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| } |
| /branches/dongfang_FC_rewrite/backup/mm3.h |
|---|
| 0,0 → 1,29 |
| #ifndef _MM3_H |
| #define _MM3_H |
| typedef struct |
| { |
| int8_t X_off; |
| int8_t Y_off; |
| int8_t Z_off; |
| int16_t X_range; |
| int16_t Y_range; |
| int16_t Z_range; |
| } MM3_calib_t; |
| extern MM3_calib_t MM3_calib; |
| // Initialization of the MM3 communication |
| void MM3_Init(void); |
| // should be called cyclic to get actual compass axis readings |
| void MM3_Update(void); |
| // this function calibrates the MM3 |
| // and returns immediately if the communication to the MM3-Board is broken. |
| void MM3_Calibrate(void); |
| // update compass heading |
| void MM3_Heading(void); |
| #endif //_MM3_H |
| /branches/dongfang_FC_rewrite/backup/navi-lang.txt |
|---|
| 0,0 → 1,52 |
| Navigation language def: |
| Som funktion af t: |
| - Pos. |
| - Hoejde |
| - Synsretning (X-akse antaget vandret?) (som: Fast, lineaer, as-takeoff, at-position, look-forward) |
| - Evt. naeseretning (som: Fast, lineaer, follow-look, as-takeoff, head-at, tail-at, point-forward) |
| - Positionsfunktion interpoleret mellem waypoints |
| - Hoejde det samme |
| - Retninger det samme |
| - For hvert waypoint: |
| Position |
| Hoejde |
| Standtid |
| Se-retning??? (i hvert optional) |
| - Valgmuligheder for positions-interpolation: Rette linier, curvefitting af en eller flere slags... |
| <flight> |
| <waypoint lon="...." lat="...." height="...." /> |
| <leg pos-interpolation="linear" /> |
| <waypoint lon="...." lat="...." height="...." head="270" /> |
| <leg pos-interpolation="linear" head="linear" /> |
| <waypoint lon="...." lat="...." height="...." head="180" /> |
| </flight> |
| class Waypoint { |
| Coordinates pos; |
| int height; |
| int heading; |
| } |
| function linear-height(Waypoint w1, Waypoint w2, float x) { |
| return w1.height + (w2.height - w1.height) * x; |
| } |
| Fancy udgave: |
| <waypoint> |
| <lat></lat> |
| <lon></lon> |
| <height></height> |
| </waypoint> |
| <leg> |
| <height>linear(w_o, w_t)</height> |
| </leg> |
| 2 interpreters: 1 til Google Maps ell l og 1 til navi-ctrl. |
| /branches/dongfang_FC_rewrite/backup/naviControl.c |
|---|
| --- backup/naviControl.h (nonexistent) |
| +++ backup/naviControl.h (revision 1775) |
| @@ -0,0 +1,95 @@ |
| +//void NC_update(void); |
| +//int16_t* NC_getPRTY(void); |
| +//uint8_t NC_getArgument(void); |
| +//uint8_t NC_getCommand(void); |
| +//int16_t NC_getVariable(uint8_t varNum); |
| +//void NC_calibrate(void); |
| +//uint8_t NC_getSignalQuality (void); |
| +//void NC_setNeutral(void); |
| + |
| +// ######################## SPI - FlightCtrl ################### |
| +#ifndef _NAVICONTROL_H |
| +#define _NAVICONTROL_H |
| + |
| +//#include <util/delay.h> |
| +#include <inttypes.h> |
| +#define SPI_PROTOCOL_COMP 1 |
| + |
| +#define SPI_CMD_USER 10 |
| +#define SPI_CMD_STICK 11 |
| +#define SPI_CMD_MISC 12 |
| +#define SPI_CMD_PARAMETER1 13 |
| +#define SPI_CMD_VERSION 14 |
| + |
| +typedef struct { |
| + uint8_t Sync1; |
| + uint8_t Sync2; |
| + uint8_t Command; |
| + int16_t IntegralPitch; |
| + int16_t IntegralRoll; |
| + int16_t AccPitch; |
| + int16_t AccRoll; |
| + int16_t GyroHeading; |
| + int16_t GyroPitch; |
| + int16_t GyroRoll; |
| + int16_t GyroYaw; |
| + union { |
| + int8_t sByte[12]; |
| + uint8_t Byte[12]; |
| + int16_t Int[6]; |
| + int32_t Long[3]; |
| + float Float[3]; |
| + } Param; |
| + uint8_t Chksum; |
| +} __attribute__((packed)) ToNaviCtrl_t; |
| + |
| +#define SPI_CMD_OSD_DATA 100 |
| +#define SPI_CMD_GPS_POS 101 |
| +#define SPI_CMD_GPS_TARGET 102 |
| +#define SPI_KALMAN 103 |
| + |
| +typedef struct { |
| + uint8_t Command; |
| + int16_t GPSStickPitch; |
| + int16_t GPSStickRoll; |
| + int16_t GPS_Yaw; |
| + int16_t CompassHeading; |
| + int16_t Status; |
| + uint16_t BeepTime; |
| + union { |
| + int8_t Byte[12]; |
| + int16_t Int[6]; |
| + int32_t Long[3]; |
| + float Float[3]; |
| + } Param; |
| + uint8_t Chksum; |
| +} __attribute__((packed)) FromNaviCtrl_t; |
| + |
| +typedef struct { |
| + uint8_t Major; |
| + uint8_t Minor; |
| + uint8_t Patch; |
| + uint8_t Compatible; |
| + // unsigned char Hardware; |
| +} __attribute__((packed)) SPI_VersionInfo_t; |
| + |
| +extern ToNaviCtrl_t toNaviCtrl; |
| +extern FromNaviCtrl_t fromNaviCtrl; |
| + |
| +typedef struct { |
| + int8_t KalmanK; |
| + int8_t KalmanMaxDrift; |
| + int8_t KalmanMaxFusion; |
| + uint8_t SerialDataOkay; |
| +} __attribute__((packed)) NCData_t; |
| + |
| +extern uint8_t NCDataOkay; |
| +extern uint8_t NCSerialDataOkay; |
| + |
| +void SPI_MasterInit(void); |
| +void SPI_StartTransmitPacket(void); |
| +void SPI_TransmitByte(void); |
| +// new: |
| +// extern void UpdateSPI_Buffer(void); |
| + |
| +#endif //_NAVICONTROL_H |
| /branches/dongfang_FC_rewrite/backup/old_macros.h |
|---|
| 0,0 → 1,47 |
| /* |
| For backwards compatibility only. |
| Ingo Busker ingo@mikrocontroller.com |
| */ |
| #ifndef cbi |
| #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) |
| #endif |
| #ifndef sbi |
| #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) |
| #endif |
| #ifndef inb |
| #define inb(sfr) _SFR_BYTE(sfr) |
| #endif |
| #ifndef outb |
| #define outb(sfr, val) (_SFR_BYTE(sfr) = (val)) |
| #endif |
| #ifndef inw |
| #define inw(sfr) _SFR_WORD(sfr) |
| #endif |
| #ifndef outw |
| #define outw(sfr, val) (_SFR_WORD(sfr) = (val)) |
| #endif |
| #ifndef outp |
| #define outp(val, sfr) outb(sfr, val) |
| #endif |
| #ifndef inp |
| #define inp(sfr) inb(sfr) |
| #endif |
| #ifndef BV |
| #define BV(bit) _BV(bit) |
| #endif |
| #ifndef PRG_RDB |
| #define PRG_RDB pgm_read_byte |
| #endif |
| /branches/dongfang_FC_rewrite/backup/output.c |
|---|
| 0,0 → 1,112 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <inttypes.h> |
| #include "output.h" |
| #include "eeprom.h" |
| // To access the DebugOut struct. |
| #include "uart0.h" |
| uint8_t flashCnt[2], flashMask[2]; |
| // initializes the LED control outputs J16, J17 |
| void output_init(void) { |
| // set PC2 & PC3 as output (control of J16 & J17) |
| DDRC |= (1<<DDC2)|(1<<DDC3); |
| OUTPUT_OFF(0); OUTPUT_OFF(1); |
| flashCnt[0] = flashCnt[1] = 0; |
| flashMask[0] = flashMask[1] = 128; |
| } |
| void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
| if (timing > 250 && manual > 230) { |
| // "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7. |
| OUTPUT_SET(port, bitmask & 128); |
| } else if (timing > 250 && manual < 10) { |
| // "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7. |
| OUTPUT_SET(port, !(bitmask & 128)); |
| } else if(!flashCnt[port]--) { |
| // rotating mask over bitmask... |
| flashCnt[port] = timing - 1; |
| if(flashMask[port] == 1) flashMask[port] = 128; else flashMask[port] >>= 1; |
| OUTPUT_SET(port, flashMask[port] & bitmask); |
| } |
| } |
| void flashingLights(void) { |
| static int8_t delay = 0; |
| if(!delay--) { // 10 ms intervall |
| delay = 4; |
| flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, dynamicParams.J16Timing); |
| flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, dynamicParams.J17Timing); |
| } |
| } |
| #define DIGITAL_DEBUG_MASK DEBUG_SERIAL |
| #define DIGITAL_DEBUG_INVERT 1 |
| void output_update(void) { |
| uint8_t output0, output1; |
| if (!DIGITAL_DEBUG_MASK) |
| flashingLights(); |
| else { |
| if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST) { |
| // Show the state for a SET bit. If inverse, then invert. |
| output0 = output1 = ~DIGITAL_DEBUG_INVERT; |
| } else if (DIGITAL_DEBUG_INVERT) { |
| output0 = (~DebugOut.Digital[0]) & DIGITAL_DEBUG_MASK; |
| output1 = (~DebugOut.Digital[1]) & DIGITAL_DEBUG_MASK; |
| } else { |
| output0 = DebugOut.Digital[0] & DIGITAL_DEBUG_MASK; |
| output1 = DebugOut.Digital[1] & DIGITAL_DEBUG_MASK; |
| } |
| OUTPUT_SET(0, output0); |
| OUTPUT_SET(1, output1); |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/output.h |
|---|
| 0,0 → 1,26 |
| #ifndef _OUTPUT_H |
| #define _OUTPUT_H |
| #include <avr/io.h> |
| // This is for LEDs connected directly between +5V and the AVR port, without transistors. |
| // PORTbit = 0 --> LED on. |
| // To use the normal transistor set-up where 1 --> transistor conductive, reverse the |
| // ON and OFF statements. |
| #define OUTPUT_ON(num) {PORTC |= (4 << (num));} |
| #define OUTPUT_OFF(num) {PORTC &= ~(4 << (num));} |
| #define OUTPUT_SET(num, state) {if ((state)) OUTPUT_ON(num) else OUTPUT_OFF(num)} |
| #define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));} |
| #define DEBUG_LEDTEST 256 |
| #define DEBUG_HEIGHT_SWITCH 1 |
| #define DEBUG_HEIGHT_DIFF 2 |
| #define DEBUG_HOVERTHROTTLE 4 |
| #define DEBUG_ACC0THORDER 8 |
| #define DEBUG_SERIAL 16 |
| void output_init(void); |
| void output_update(void); |
| #endif //_output_H |
| /branches/dongfang_FC_rewrite/backup/ports.txt |
|---|
| 0,0 → 1,21 |
| This is a list of the ATMega pins that are used for various ad-hoc I/O on the FC. |
| The A/D converter inputs, the ISP interface, I2C and the air pressure circuit and |
| red and green LED outputs are not included. |
| ATMEGA pin FC1.3 name FC 2.0 name Used for |
| ------------------------------------------------------------------------------------------- |
| PC7 SPEAKER SPEAKER Beeper. |
| PC6 J6 SERVO_R FC1.3: J9. FC2.0: Reset on IC5/4017. |
| PC5 PC5 PC5 Expansion port pin 6. "SPI slave select" in spi.c. |
| PC4 PC4 PC4 Expansion port pin 5. PWM in from MK3Mag compass. |
| PC3 PC3 PC3 SV2 pin 1 via transistor. Also known as J17. |
| PC2 PC2 PC2 SV2 pin 5 via transistor. Also known as J16. |
| PD7 SERVO SERVO_C FC1.3: J7. FC2.0: Clock on IC5/4017. |
| PD6 ICP(PPM) ICP(PPM) R/C PPM signal in. |
| PD5 J3 J3 R/C channel 5 forward (rc.c). Auto-zero feature on InvenSense gyros (InvenSense.c, dongfang only). |
| PD4 J4 J4 R/C channel 6 forward (rc.c). Execution time measurement of main loop (main.c). |
| PD3 J5 J5 TXD1 with ATMega644p. R/C channel 7 forward with ATMega644. |
| PD2 RXD1 with ATMega644p |
| TDX1 and RXD1 are the 2nd UART in ATMEGA644p. It can be used for direct connection of a UBlox GPS module (no Navi-Ctrl), or for connection of a Spectrum or DSL serial R/C satellite. |
| /branches/dongfang_FC_rewrite/backup/printf_P.c |
|---|
| 0,0 → 1,483 |
| // Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt |
| /* |
| Copyright (C) 1993 Free Software Foundation |
| This file is part of the GNU IO Library. This library is free |
| software; you can redistribute it and/or modify it under the |
| terms of the GNU General Public License as published by the |
| Free Software Foundation; either version 2, or (at your option) |
| any later version. |
| This library is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| You should have received a copy of the GNU General Public License |
| along with this library; see the file COPYING. If not, write to the Free |
| Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
| As a special exception, if you link this library with files |
| compiled with a GNU compiler to produce an executable, this does not cause |
| the resulting executable to be covered by the GNU General Public License. |
| This exception does not however invalidate any other reasons why |
| the executable file might be covered by the GNU General Public License. */ |
| /* |
| * Copyright (c) 1990 Regents of the University of California. |
| * All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions |
| * are met: |
| * 1. Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * 2. Redistributions in binary form must reproduce the above copyright |
| * notice, this list of conditions and the following disclaimer in the |
| * documentation and/or other materials provided with the distribution. |
| * 3. [rescinded 22 July 1999] |
| * 4. Neither the name of the University nor the names of its contributors |
| * may be used to endorse or promote products derived from this software |
| * without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND |
| * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE |
| * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
| * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
| * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
| * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
| * SUCH DAMAGE. |
| */ |
| /****************************************************************************** |
| This file is a patched version of printf called _printf_P |
| It is made to work with avr-gcc for Atmel AVR MCUs. |
| There are some differences from standard printf: |
| 1. There is no floating point support (with fp the code is about 8K!) |
| 2. Return type is void |
| 3. Format string must be in program memory (by using macro printf this is |
| done automaticaly) |
| 4. %n is not implemented (just remove the comment around it if you need it) |
| 5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the |
| folowing specifiers are disabled : |
| space # * . - + p s o O |
| 6. A function void uart_sendchar(char c) is used for output. The UART must |
| be initialized before using printf. |
| Alexander Popov |
| sasho@vip.orbitel.bg |
| ******************************************************************************/ |
| /* |
| * Actual printf innards. |
| * |
| * This code is large and complicated... |
| */ |
| #include <string.h> |
| #ifdef __STDC__ |
| #include <stdarg.h> |
| #else |
| #include <varargs.h> |
| #endif |
| #include "old_macros.h" |
| #include "printf_P.h" |
| #include "menu.h" |
| #include "uart0.h" |
| //#define LIGHTPRINTF |
| char PrintZiel; |
| char Putchar(char zeichen) |
| { |
| if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);} |
| else return(uart_putchar(zeichen)); |
| } |
| void PRINT(const char * ptr, unsigned int len) |
| { |
| for(;len;len--) Putchar(*ptr++); |
| } |
| void PRINTP(const char * ptr, unsigned int len) |
| { |
| for(;len;len--) Putchar(pgm_read_byte(ptr++)); |
| } |
| void PAD_SP(signed char howmany) |
| { |
| for(;howmany>0;howmany--) Putchar(' '); |
| } |
| void PAD_0(signed char howmany) |
| { |
| for(;howmany>0;howmany--) Putchar('0'); |
| } |
| #define BUF 40 |
| /* |
| * Macros for converting digits to letters and vice versa |
| */ |
| #define to_digit(c) ((c) - '0') |
| #define is_digit(c) ((c)<='9' && (c)>='0') |
| #define to_char(n) ((n) + '0') |
| /* |
| * Flags used during conversion. |
| */ |
| #define LONGINT 0x01 /* long integer */ |
| #define LONGDBL 0x02 /* long double; unimplemented */ |
| #define SHORTINT 0x04 /* short integer */ |
| #define ALT 0x08 /* alternate form */ |
| #define LADJUST 0x10 /* left adjustment */ |
| #define ZEROPAD 0x20 /* zero (as opposed to blank) pad */ |
| #define HEXPREFIX 0x40 /* add 0x or 0X prefix */ |
| void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */ |
| { |
| va_list ap; |
| register const char *fmt; /* format string */ |
| register char ch; /* character from fmt */ |
| register int n; /* handy integer (short term usage) */ |
| register char *cp; /* handy char pointer (short term usage) */ |
| const char *fmark; /* for remembering a place in fmt */ |
| register unsigned char flags; /* flags as above */ |
| signed char width; /* width from format (%8d), or 0 */ |
| signed char prec; /* precision from format (%.3d), or -1 */ |
| char sign; /* sign prefix (' ', '+', '-', or \0) */ |
| unsigned long _ulong=0; /* integer arguments %[diouxX] */ |
| #define OCT 8 |
| #define DEC 10 |
| #define HEX 16 |
| unsigned char base; /* base for [diouxX] conversion */ |
| signed char dprec; /* a copy of prec if [diouxX], 0 otherwise */ |
| signed char dpad; /* extra 0 padding needed for integers */ |
| signed char fieldsz; /* field size expanded by sign, dpad etc */ |
| /* The initialization of 'size' is to suppress a warning that |
| 'size' might be used unitialized. It seems gcc can't |
| quite grok this spaghetti code ... */ |
| signed char size = 0; /* size of converted field or string */ |
| char buf[BUF]; /* space for %c, %[diouxX], %[eEfgG] */ |
| char ox[2]; /* space for 0x hex-prefix */ |
| PrintZiel = ziel; // bestimmt, LCD oder UART |
| va_start(ap, fmt0); |
| fmt = fmt0; |
| /* |
| * Scan the format for conversions (`%' character). |
| */ |
| for (;;) { |
| for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++) |
| /* void */; |
| if ((n = fmt - fmark) != 0) { |
| PRINTP(fmark, n); |
| } |
| if (ch == '\0') |
| goto done; |
| fmt++; /* skip over '%' */ |
| flags = 0; |
| dprec = 0; |
| width = 0; |
| prec = -1; |
| sign = '\0'; |
| rflag: ch = PRG_RDB(fmt++); |
| reswitch: |
| #ifdef LIGHTPRINTF |
| if (ch=='o' || ch=='u' || (ch|0x20)=='x') { |
| #else |
| if (ch=='u' || (ch|0x20)=='x') { |
| #endif |
| if (flags&LONGINT) { |
| _ulong=va_arg(ap, unsigned long); |
| } else { |
| register unsigned int _d; |
| _d=va_arg(ap, unsigned int); |
| _ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d; |
| } |
| } |
| #ifndef LIGHTPRINTF |
| if(ch==' ') { |
| /* |
| * ``If the space and + flags both appear, the space |
| * flag will be ignored.'' |
| * -- ANSI X3J11 |
| */ |
| if (!sign) |
| sign = ' '; |
| goto rflag; |
| } else if (ch=='#') { |
| flags |= ALT; |
| goto rflag; |
| } else if (ch=='*'||ch=='-') { |
| if (ch=='*') { |
| /* |
| * ``A negative field width argument is taken as a |
| * - flag followed by a positive field width.'' |
| * -- ANSI X3J11 |
| * They don't exclude field widths read from args. |
| */ |
| if ((width = va_arg(ap, int)) >= 0) |
| goto rflag; |
| width = -width; |
| } |
| flags |= LADJUST; |
| flags &= ~ZEROPAD; /* '-' disables '0' */ |
| goto rflag; |
| } else if (ch=='+') { |
| sign = '+'; |
| goto rflag; |
| } else if (ch=='.') { |
| if ((ch = PRG_RDB(fmt++)) == '*') { |
| n = va_arg(ap, int); |
| prec = n < 0 ? -1 : n; |
| goto rflag; |
| } |
| n = 0; |
| while (is_digit(ch)) { |
| n = n*10 + to_digit(ch); |
| ch = PRG_RDB(fmt++); |
| } |
| prec = n < 0 ? -1 : n; |
| goto reswitch; |
| } else |
| #endif /* LIGHTPRINTF */ |
| if (ch=='0') { |
| /* |
| * ``Note that 0 is taken as a flag, not as the |
| * beginning of a field width.'' |
| * -- ANSI X3J11 |
| */ |
| if (!(flags & LADJUST)) |
| flags |= ZEROPAD; /* '-' disables '0' */ |
| goto rflag; |
| } else if (ch>='1' && ch<='9') { |
| n = 0; |
| do { |
| n = 10 * n + to_digit(ch); |
| ch = PRG_RDB(fmt++); |
| } while (is_digit(ch)); |
| width = n; |
| goto reswitch; |
| } else if (ch=='h') { |
| flags |= SHORTINT; |
| goto rflag; |
| } else if (ch=='l') { |
| flags |= LONGINT; |
| goto rflag; |
| } else if (ch=='c') { |
| *(cp = buf) = va_arg(ap, int); |
| size = 1; |
| sign = '\0'; |
| } else if (ch=='D'||ch=='d'||ch=='i') { |
| if(ch=='D') |
| flags |= LONGINT; |
| if (flags&LONGINT) { |
| _ulong=va_arg(ap, long); |
| } else { |
| register int _d; |
| _d=va_arg(ap, int); |
| _ulong = flags&SHORTINT ? (long)(short)_d : (long)_d; |
| } |
| if ((long)_ulong < 0) { |
| _ulong = -_ulong; |
| sign = '-'; |
| } |
| base = DEC; |
| goto number; |
| } else |
| /* |
| if (ch=='n') { |
| if (flags & LONGINT) |
| *va_arg(ap, long *) = ret; |
| else if (flags & SHORTINT) |
| *va_arg(ap, short *) = ret; |
| else |
| *va_arg(ap, int *) = ret; |
| continue; // no output |
| } else |
| */ |
| #ifndef LIGHTPRINTF |
| if (ch=='O'||ch=='o') { |
| if (ch=='O') |
| flags |= LONGINT; |
| base = OCT; |
| goto nosign; |
| } else if (ch=='p') { |
| /* |
| * ``The argument shall be a pointer to void. The |
| * value of the pointer is converted to a sequence |
| * of printable characters, in an implementation- |
| * defined manner.'' |
| * -- ANSI X3J11 |
| */ |
| /* NOSTRICT */ |
| _ulong = (unsigned int)va_arg(ap, void *); |
| base = HEX; |
| flags |= HEXPREFIX; |
| ch = 'x'; |
| goto nosign; |
| } else if (ch=='s') { // print a string from RAM |
| if ((cp = va_arg(ap, char *)) == NULL) { |
| cp=buf; |
| cp[0] = '('; |
| cp[1] = 'n'; |
| cp[2] = 'u'; |
| cp[4] = cp[3] = 'l'; |
| cp[5] = ')'; |
| cp[6] = '\0'; |
| } |
| if (prec >= 0) { |
| /* |
| * can't use strlen; can only look for the |
| * NUL in the first `prec' characters, and |
| * strlen() will go further. |
| */ |
| char *p = (char*)memchr(cp, 0, prec); |
| if (p != NULL) { |
| size = p - cp; |
| if (size > prec) |
| size = prec; |
| } else |
| size = prec; |
| } else |
| size = strlen(cp); |
| sign = '\0'; |
| } else |
| #endif /* LIGHTPRINTF */ |
| if(ch=='U'||ch=='u') { |
| if (ch=='U') |
| flags |= LONGINT; |
| base = DEC; |
| goto nosign; |
| } else if (ch=='X'||ch=='x') { |
| base = HEX; |
| /* leading 0x/X only if non-zero */ |
| if (flags & ALT && _ulong != 0) |
| flags |= HEXPREFIX; |
| /* unsigned conversions */ |
| nosign: sign = '\0'; |
| /* |
| * ``... diouXx conversions ... if a precision is |
| * specified, the 0 flag will be ignored.'' |
| * -- ANSI X3J11 |
| */ |
| number: if ((dprec = prec) >= 0) |
| flags &= ~ZEROPAD; |
| /* |
| * ``The result of converting a zero value with an |
| * explicit precision of zero is no characters.'' |
| * -- ANSI X3J11 |
| */ |
| cp = buf + BUF; |
| if (_ulong != 0 || prec != 0) { |
| register unsigned char _d,notlastdigit; |
| do { |
| notlastdigit=(_ulong>=base); |
| _d = _ulong % base; |
| if (_d<10) { |
| _d+='0'; |
| } else { |
| _d+='a'-10; |
| if (ch=='X') _d&=~0x20; |
| } |
| *--cp=_d; |
| _ulong /= base; |
| } while (notlastdigit); |
| #ifndef LIGHTPRINTF |
| // handle octal leading 0 |
| if (base==OCT && flags & ALT && *cp != '0') |
| *--cp = '0'; |
| #endif |
| } |
| size = buf + BUF - cp; |
| } else { //default |
| /* "%?" prints ?, unless ? is NUL */ |
| if (ch == '\0') |
| goto done; |
| /* pretend it was %c with argument ch */ |
| cp = buf; |
| *cp = ch; |
| size = 1; |
| sign = '\0'; |
| } |
| /* |
| * All reasonable formats wind up here. At this point, |
| * `cp' points to a string which (if not flags&LADJUST) |
| * should be padded out to `width' places. If |
| * flags&ZEROPAD, it should first be prefixed by any |
| * sign or other prefix; otherwise, it should be blank |
| * padded before the prefix is emitted. After any |
| * left-hand padding and prefixing, emit zeroes |
| * required by a decimal [diouxX] precision, then print |
| * the string proper, then emit zeroes required by any |
| * leftover floating precision; finally, if LADJUST, |
| * pad with blanks. |
| */ |
| /* |
| * compute actual size, so we know how much to pad. |
| */ |
| fieldsz = size; |
| dpad = dprec - size; |
| if (dpad < 0) |
| dpad = 0; |
| if (sign) |
| fieldsz++; |
| else if (flags & HEXPREFIX) |
| fieldsz += 2; |
| fieldsz += dpad; |
| /* right-adjusting blank padding */ |
| if ((flags & (LADJUST|ZEROPAD)) == 0) |
| PAD_SP(width - fieldsz); |
| /* prefix */ |
| if (sign) { |
| PRINT(&sign, 1); |
| } else if (flags & HEXPREFIX) { |
| ox[0] = '0'; |
| ox[1] = ch; |
| PRINT(ox, 2); |
| } |
| /* right-adjusting zero padding */ |
| if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD) |
| PAD_0(width - fieldsz); |
| /* leading zeroes from decimal precision */ |
| PAD_0(dpad); |
| /* the string or number proper */ |
| PRINT(cp, size); |
| /* left-adjusting padding (always blank) */ |
| if (flags & LADJUST) |
| PAD_SP(width - fieldsz); |
| } |
| done: |
| va_end(ap); |
| } |
| /branches/dongfang_FC_rewrite/backup/printf_P.h |
|---|
| 0,0 → 1,19 |
| #ifndef _PRINTF_P_H_ |
| #define _PRINTF_P_H_ |
| #include <avr/pgmspace.h> |
| #define OUT_V24 0 |
| #define OUT_LCD 1 |
| void _printf_P (char, char const *fmt0, ...); |
| extern char PrintZiel; |
| #define printf_P(format, args...) _printf_P(OUT_V24,format , ## args) |
| #define printf(format, args...) _printf_P(OUT_V24,PSTR(format) , ## args) |
| #define LCD_printfxy(x,y,format, args...) { DispPtr = y * 20 + x; _printf_P(OUT_LCD,PSTR(format) , ## args);} |
| #define LCD_printf(format, args...) { _printf_P(OUT_LCD,PSTR(format) , ## args);} |
| #endif |
| /branches/dongfang_FC_rewrite/backup/rc.c |
|---|
| 0,0 → 1,436 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include "rc.h" |
| #include "uart0.h" |
| #include "controlMixer.h" |
| #include "configuration.h" |
| #include "commands.h" |
| // The channel array is 1-based. The 0th entry is not used. |
| volatile int16_t PPM_in[MAX_CHANNELS]; |
| volatile int16_t PPM_diff[MAX_CHANNELS]; |
| volatile uint8_t NewPpmData = 1; |
| volatile int16_t RC_Quality = 0; |
| int16_t RC_PRTY[4]; |
| uint8_t lastRCCommand = COMMAND_NONE; |
| uint8_t commandTimer = 0; |
| // Useless. Just trim on the R/C instead. |
| // int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
| /*************************************************************** |
| * 16bit timer 1 is used to decode the PPM-Signal |
| ***************************************************************/ |
| void RC_Init (void) { |
| uint8_t sreg = SREG; |
| // disable all interrupts before reconfiguration |
| cli(); |
| // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
| DDRD &= ~(1<<DDD6); |
| PORTD |= (1<<PORTD6); |
| // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
| // set as output |
| DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3); |
| // low level |
| PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
| // PD3 can't be used if 2nd UART is activated |
| // because TXD1 is at that port |
| if(CPUType != ATMEGA644P) { |
| DDRD |= (1<<PORTD3); |
| PORTD &= ~(1<<PORTD3); |
| } |
| // Timer/Counter1 Control Register A, B, C |
| // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
| // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
| // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
| // Enable input capture noise cancler (bit: ICNC1=1) |
| // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
| // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
| // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
| TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10)); |
| TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12)); |
| TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); |
| TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B)); |
| // Timer/Counter1 Interrupt Mask Register |
| // Enable Input Capture Interrupt (bit: ICIE1=1) |
| // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
| // Enable Overflow Interrupt (bit: TOIE1=0) |
| TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1)); |
| TIMSK1 |= (1<<ICIE1); |
| RC_Quality = 0; |
| SREG = sreg; |
| } |
| /********************************************************************/ |
| /* Every time a positive edge is detected at PD6 */ |
| /********************************************************************/ |
| /* t-Frame |
| <-----------------------------------------------------------------------> |
| ____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | | |
| | | | | | | | | | | | |
| ___| |_| |_| |_| |_.............| |________________| |
| <-----><-------><------><--------> <------> <--- |
| t0 t1 t2 t4 tn t0 |
| The PPM-Frame length is 22.5 ms. |
| Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
| The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
| The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms. |
| The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
| The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
| The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
| the syncronization gap. |
| */ |
| ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
| int16_t signal = 0, tmp; |
| static int16_t index; |
| static uint16_t oldICR1 = 0; |
| // 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
| // at the time the edge was detected |
| // calculate the time delay to the previous event time which is stored in oldICR1 |
| // calculatiing the difference of the two uint16_t and converting the result to an int16_t |
| // implicit handles a timer overflow 65535 -> 0 the right way. |
| signal = (uint16_t) ICR1 - oldICR1; |
| oldICR1 = ICR1; |
| //sync gap? (3.52 ms < signal < 25.6 ms) |
| if((signal > 1100) && (signal < 8000)) { |
| // if a sync gap happens and there where at least 4 channels decoded before |
| // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
| if(index >= 4) { |
| NewPpmData = 0; // Null means NewData for the first 4 channels |
| } |
| // synchronize channel index |
| index = 1; |
| } else { // within the PPM frame |
| if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels |
| { |
| // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
| // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
| if((signal > 250) && (signal < 687)) { |
| // shift signal to zero symmetric range -154 to 159 |
| signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
| // check for stable signal |
| if(abs(signal - PPM_in[index]) < 6) { |
| if(RC_Quality < 200) RC_Quality +=10; |
| else RC_Quality = 200; |
| } |
| // If signal is the same as before +/- 1, just keep it there. |
| if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
| // In addition, if the signal is very close to 0, just set it to 0. |
| if (signal >=-1 && signal <= 1) { |
| tmp = 0; |
| } else { |
| tmp = PPM_in[index]; |
| } |
| } |
| else |
| tmp = signal; |
| // calculate signal difference on good signal level |
| if(RC_Quality >= 195) |
| PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; // update channel value |
| } |
| index++; // next channel |
| // demux sum signal for channels 5 to 7 to J3, J4, J5 |
| // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
| // channels are usually available at the receiver anyway. |
| // if(index == 5) J3HIGH; else J3LOW; |
| // if(index == 6) J4HIGH; else J4LOW; |
| // if(CPUType != ATMEGA644P) // not used as TXD1 |
| // { |
| // if(index == 7) J5HIGH; else J5LOW; |
| // } |
| } |
| } |
| } |
| #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
| #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
| #define COMMAND_THRESHOLD 85 |
| #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
| #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
| // Internal. |
| uint8_t RC_getStickCommand(void) { |
| if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
| // vertical is up |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_GYROCAL; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_ACCCAL; |
| return COMMAND_NONE; |
| } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
| // vertical is down |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_STOP; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_START; |
| return COMMAND_NONE; |
| } |
| // vertical is around center |
| return COMMAND_NONE; |
| } |
| /* |
| * This must be called (as the only thing) for each control loop cycle (488 Hz). |
| */ |
| void RC_update() { |
| int16_t tmp1, tmp2; |
| if(RC_Quality) { |
| RC_Quality--; |
| if (NewPpmData-- == 0) { |
| RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
| RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
| RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
| if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| // exponential stick sensitivity in yawing rate |
| tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
| tmp2 += (staticParams.StickYawP * tmp1) / 4; |
| RC_PRTY[CONTROL_YAW] = tmp2; |
| DebugOut.Analog[12] = RCChannel(CH_PITCH); |
| DebugOut.Analog[13] = RCChannel(CH_ROLL); |
| } |
| uint8_t command = RC_getStickCommand(); |
| if (lastRCCommand == command && commandTimer < COMMAND_TIMER) { |
| commandTimer++; |
| } else { |
| lastRCCommand = command; |
| commandTimer = 0; |
| } |
| } else { // Bad signal |
| RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
| } |
| } |
| /* |
| * Get Pitch, Roll, Throttle, Yaw values |
| */ |
| int16_t* RC_getPRTY(void) { |
| return RC_PRTY; |
| } |
| /* |
| * Get other channel value |
| */ |
| int16_t RC_getVariable (uint8_t varNum) { |
| if (varNum < 4) |
| // 0th variable is 5th channel (1-based) etc. |
| return RCChannel(varNum + 4) + POT_OFFSET; |
| /* |
| * Let's just say: |
| * The RC variable 4 is hardwired to channel 5 |
| * The RC variable 5 is hardwired to channel 6 |
| * The RC variable 6 is hardwired to channel 7 |
| * The RC variable 7 is hardwired to channel 8 |
| * Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
| */ |
| return PPM_in[varNum + 1] + POT_OFFSET; |
| } |
| uint8_t RC_getSignalQuality(void) { |
| if (RC_Quality >= 160) |
| return SIGNAL_GOOD; |
| if (RC_Quality >= 140) |
| return SIGNAL_OK; |
| if (RC_Quality >= 120) |
| return SIGNAL_BAD; |
| return SIGNAL_LOST; |
| } |
| /* |
| * To should fired only when the right stick is in the center position. |
| * This will cause the value of pitch and roll stick to be adjusted |
| * to zero (not just to near zero, as per the assumption in rc.c |
| * about the rc signal. I had values about 50..70 with a Futaba |
| * R617 receiver.) This calibration is not strictly necessary, but |
| * for control logic that depends on the exact (non)center position |
| * of a stick, it may be useful. |
| */ |
| void RC_calibrate(void) { |
| // Do nothing. |
| } |
| /* |
| if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
| // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
| stickOffsetPitch = stickOffsetRoll = 0; |
| } else { |
| stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
| stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
| } |
| } |
| */ |
| uint8_t RC_getCommand(void) { |
| if (commandTimer == COMMAND_TIMER) { |
| // Stick has been held long enough; command committed. |
| return lastRCCommand; |
| } |
| // Not yet sure what the command is. |
| return COMMAND_NONE; |
| } |
| /* |
| * Command arguments on R/C: |
| * 2--3--4 |
| * | | + |
| * 1 0 5 ^ 0 |
| * | | | |
| * 8--7--6 |
| * |
| * + <-- |
| * 0 |
| * |
| * Not in any of these positions: 0 |
| */ |
| #define ARGUMENT_THRESHOLD 70 |
| #define ARGUMENT_CHANNEL_VERTICAL CH_PITCH |
| #define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL |
| uint8_t RC_getArgument(void) { |
| if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
| // vertical is up |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| return 2; |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| return 4; |
| return 3; |
| } else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
| // vertical is down |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| return 8; |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| return 6; |
| return 7; |
| } else { |
| // vertical is around center |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| return 1; |
| if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| return 5; |
| return 0; |
| } |
| } |
| uint8_t RC_getLooping(uint8_t looping) { |
| // static uint8_t looping = 0; |
| if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) { |
| looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
| } else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
| } |
| if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) { |
| looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
| } else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
| } |
| if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) { |
| looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
| } else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
| } |
| if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) { |
| looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
| } else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
| } |
| return looping; |
| } |
| uint8_t RC_testCompassCalState(void) { |
| static uint8_t stick = 1; |
| // if pitch is centered or top set stick to zero |
| if(RCChannel(CH_PITCH) > -20) stick = 0; |
| // if pitch is down trigger to next cal state |
| if((RCChannel(CH_PITCH) < -70) && !stick) { |
| stick = 1; |
| return 1; |
| } |
| return 0; |
| } |
| /* |
| * Abstract controls are not used at the moment. |
| t_control rc_control = { |
| RC_getPitch, |
| RC_getRoll, |
| RC_getYaw, |
| RC_getThrottle, |
| RC_getSignalQuality, |
| RC_calibrate |
| }; |
| */ |
| /branches/dongfang_FC_rewrite/backup/rc.h |
|---|
| 0,0 → 1,55 |
| #ifndef _RC_H |
| #define _RC_H |
| #include <inttypes.h> |
| #define J3HIGH PORTD |= (1<<PORTD5) |
| #define J3LOW PORTD &= ~(1<<PORTD5) |
| #define J3TOGGLE PORTD ^= (1<<PORTD5) |
| #define J4HIGH PORTD |= (1<<PORTD4) |
| #define J4LOW PORTD &= ~(1<<PORTD4) |
| #define J4TOGGLE PORTD ^= (1<<PORTD4) |
| #define J5HIGH PORTD |= (1<<PORTD3) |
| #define J5LOW PORTD &= ~(1<<PORTD3) |
| #define J5TOGGLE PORTD ^= (1<<PORTD3) |
| #define MAX_CHANNELS 10 |
| // Number of cycles a command must be repeated before commit. |
| #define COMMAND_TIMER 200 |
| extern void RC_Init (void); |
| // the RC-Signal. todo: Not export any more. |
| extern volatile int16_t PPM_in[MAX_CHANNELS]; |
| // extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
| extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
| extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200) |
| // defines for lookup staticParams.ChannelAssignment |
| #define CH_PITCH 0 |
| #define CH_ROLL 1 |
| #define CH_THROTTLE 2 |
| #define CH_YAW 3 |
| #define CH_POTS 4 |
| #define POT_OFFSET 115 |
| /* |
| int16_t RC_getPitch (void); |
| int16_t RC_getYaw (void); |
| int16_t RC_getRoll (void); |
| uint16_t RC_getThrottle (void); |
| uint8_t RC_hasNewRCData (void); |
| */ |
| void RC_update(void); |
| int16_t* RC_getPRTY(void); |
| uint8_t RC_getArgument(void); |
| uint8_t RC_getCommand(void); |
| int16_t RC_getVariable(uint8_t varNum); |
| void RC_calibrate(void); |
| uint8_t RC_getSignalQuality(void); |
| uint8_t RC_getLooping(uint8_t looping); |
| uint8_t RC_testCompassCalState(void); |
| #endif //_RC_H |
| /branches/dongfang_FC_rewrite/backup/readme.txt |
|---|
| 0,0 → 1,45 |
| Dongfang's FC firmware rewrite |
| ------------------------------ |
| The basis was V0.74d Code Redesign Killagreg. |
| Goal: |
| - To be the preferred hacker-friendly FC firmware. |
| - To use H&Is design experience but make the code clearer and easier to extend+modify. |
| Non-goal: |
| - To follow up on all changes and all new features in the H&I firmware. |
| Features: |
| - Readable, reasonably documented source code makes it easy to implement the features you |
| want yourself (and to remove those you don't like). |
| - The code was broken into smaller pieces and modularized (fc.c in particular). |
| Global variables are (almost) only written to by one module each. |
| - New or experimental hardware is easy to incorporate. Gyro and acc. meter axes are reversible, |
| and resetting the sensitivity actually works. |
| - New or experiemental controls are easy to incorporate. All controls (eg. R/C, external |
| serial, NC, automatic emergency landing pilot and automatic altitude controller pilot) |
| are potentially abstractable to one interface. |
| - Reversal of gyro or accelerometer axes is easy. It is easy to adapt the firmware for |
| upside down installation of the FC too. |
| - The firmware is compatible with MK-Tool. This may be changed, if somebody writes a new |
| MK-Tool which is easy to adapt for addition of / removal of features. |
| Non-features (currently): |
| - Navi support temporarily removed (should be added again later). |
| - Compass support temporarily removed (should be added again later). |
| - Parts of menu.c are dummy implemented or removed. It is possible that menu.c and all use |
| of printf will be removed later, and replaced by something else (debud on steroids). |
| - Control rate limiter removed. |
| - Altitude control temporarily removed (should be added again later). |
| - Automatic board detection removed. This firmware is for compiling yourself, possibly |
| with nonstandard or experimental hardware. That conflicts with automatically switching |
| between standard hardware versions, so the feature was removed. Instead, is was made |
| easy to choose gyro types etc. in the makefile. |
| How to build: |
| - Choose a gyro definition, depending on your hardware, and enable it in the makefile |
| and (GYRO=.....) and #include its header file in analog.h (how to make that follow the |
| makefile automatically?). Currently, ENC-03_FC1.3, ADXRS610_FC2.0 and invenSense are |
| supported (each has a .c and a .h file). |
| - make all |
| /branches/dongfang_FC_rewrite/backup/sendOutData.c |
|---|
| 0,0 → 1,45 |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t txd_bufferIndex = 0; |
| uint8_t *currentBuffer; |
| uint8_t currentBufferIndex; |
| uint16_t lengthOfCurrentBuffer; |
| uint8_t shift = 0; |
| txd_buffer[txd_bufferIndex++] = '#'; // Start character |
| txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[txd_bufferIndex++] = cmd; // Command |
| va_start(ap, numofbuffers); |
| while(numofbuffers) { |
| currentBuffer = va_arg(ap, uint8_t*); |
| lengthOfCurrentBuffer = va_arg(ap, int); |
| currentBufferIndex = 0; |
| // Encode data: 3 bytes of data are encoded into 4 bytes, |
| // where the 2 most significant bits are both 0. |
| while(currentBufferIndex != lengthOfCurrentBuffer) { |
| if (!shift) txd_buffer[txd_bufferIndex] = 0; |
| txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
| txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
| shift += 2; |
| if (shift == 6) { shift=0; txd_bufferIndex++; } |
| currentBufferIndex++; |
| } |
| } |
| // If the number of data bytes was not divisible by 3, stuff |
| // with 0 pseudodata until length is again divisible by 3. |
| if (shift == 2) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex] &= 0b00110000; |
| txd_buffer[++txd_bufferIndex] = 0; |
| shift = 4; |
| } |
| if (shift == 4) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |
| /branches/dongfang_FC_rewrite/backup/sensors.h |
|---|
| 0,0 → 1,29 |
| #ifndef _SENSORS_H |
| #define _SENSORS_H |
| #include <inttypes.h> |
| /* |
| * Whether (pitch, roll, yaw) gyros are reversed (see analog.h). |
| */ |
| extern const uint8_t GYRO_REVERSED[3]; |
| /* |
| * Whether (pitch, roll, Z) acc. meters are reversed(see analog.h). |
| */ |
| extern const uint8_t ACC_REVERSED[3]; |
| /* |
| * Common procedures for all gyro types. |
| * FC 1.3 hardware: Searching the DAC values that return neutral readings. |
| * FC 2.0 hardware: Nothing to do. |
| * InvenSense hardware: Output a pulse on the AUTO_ZERO line. |
| */ |
| void gyro_calibrate(void); |
| /* |
| * Set some default FC parameters, depending on gyro type: Drift correction etc. |
| */ |
| void gyro_setDefaults(void); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/spectrum.c |
|---|
| 0,0 → 1,309 |
| /*####################################################################################### |
| Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit |
| #######################################################################################*/ |
| #include "Spectrum.h" |
| //--------------------------------------------------------------// |
| //--------------------------------------------------------------// |
| void SpektrumBinding(void) |
| { |
| unsigned int timerTimeout = SetDelay(10000); // Timeout 10 sec. |
| unsigned char connected = 0; |
| unsigned int delaycounter; |
| UCSR1B &= ~(1 << RXCIE1); // disable rx-interrupt |
| UCSR1B &= ~(1<<RXEN1); // disable Uart-Rx |
| PORTD &= ~(1 << PORTD2); // disable pull-up |
| printf("\n\rPlease connect Spektrum receiver for binding NOW..."); |
| while(!CheckDelay(timerTimeout)) |
| { |
| if (PIND & (1 << PORTD2)) { timerTimeout = SetDelay(90); connected = 1; break; } |
| } |
| if (connected) |
| { |
| printf("ok.\n\r"); |
| DDRD |= (1 << DDD2); // Rx as output |
| while(!CheckDelay(timerTimeout)); // delay after startup of RX |
| for (delaycounter = 0; delaycounter < 100; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
| for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
| } |
| else |
| { printf("Timeout.\n\r"); |
| } |
| DDRD &= ~(1 << DDD2); // RX as input |
| PORTD &= ~(1 << PORTD2); |
| Uart1Init(); // init Uart again |
| } |
| //############################################################################ |
| // zum Decodieren des Spektrum Satelliten wird USART1 benutzt. |
| // USART1 initialisation from killagreg |
| void Uart1Init(void) |
| //############################################################################ |
| { |
| // -- Start of USART1 initialisation for Spekturm seriell-mode |
| // USART1 Control and Status Register A, B, C and baud rate register |
| uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1); |
| // 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); |
| // USART0 Baud Rate Register |
| // set clock divider |
| UBRR1H = (uint8_t)(ubrr>>8); |
| UBRR1L = (uint8_t)ubrr; |
| // enable double speed operation |
| UCSR1A |= (1 << U2X1); |
| // enable receiver and transmitter |
| //UCSR1B = (1<<RXEN1)|(1<<TXEN1); |
| UCSR1B = (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 RX-interrupts at the end |
| UCSR1B |= (1 << RXCIE1); |
| // -- End of USART1 initialisation |
| return; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) Rainer Walther |
| // + RC-routines from original MK rc.c (c) H&I |
| // + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2 |
| // + only for non-profit use |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // |
| // 20080808 rw Modified for Spektrum AR6100 (PPM) |
| // 20080823 rw Add Spektrum satellite receiver on USART1 (644P only) |
| // 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels) |
| // Replace AR6100-coding with original composit-signal routines |
| // |
| // --- |
| // Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben! |
| // Binding is not implemented. Bind with external Receiver. |
| // Servo output J3, J4, J5 not serviced |
| // |
| // Anschuß Spektrum Receiver |
| // Orange: 3V von der FC (keinesfalls an 5V anschließen!) |
| // Schwarz: GND |
| // Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker |
| // |
| // --- |
| // Satellite-Reciever connected on USART1: |
| // |
| // DX7/DX6i: One data-frame at 115200 baud every 22ms. |
| // DX7se: One data-frame at 115200 baud every 11ms. |
| // byte1: unknown |
| // byte2: unknown |
| // byte3: and byte4: channel data (FLT-Mode) |
| // byte5: and byte6: channel data (Roll) |
| // byte7: and byte8: channel data (Nick) |
| // byte9: and byte10: channel data (Gier) |
| // byte11: and byte12: channel data (Gear Switch) |
| // byte13: and byte14: channel data (Gas) |
| // byte15: and byte16: channel data (AUX2) |
| // |
| // DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9 |
| // 1st Frame: |
| // byte1: unknown |
| // byte2: unknown |
| // byte3: and byte4: channel data |
| // byte5: and byte6: channel data |
| // byte7: and byte8: channel data |
| // byte9: and byte10: channel data |
| // byte11: and byte12: channel data |
| // byte13: and byte14: channel data |
| // byte15: and byte16: channel data |
| // 2nd Frame: |
| // byte1: unknown |
| // byte2: unknown |
| // byte3: and byte4: channel data |
| // byte5: and byte6: channel data |
| // byte7: and byte8: 0xffff |
| // byte9: and byte10: 0xffff |
| // byte11: and byte12: 0xffff |
| // byte13: and byte14: 0xffff |
| // byte15: and byte16: 0xffff |
| // |
| // Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as: |
| // |
| // Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0 |
| // |
| // 0 means a '0' bit |
| // F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only) |
| // C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter) |
| // D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel |
| // |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //############################################################################ |
| //Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever |
| SIGNAL(USART1_RX_vect) |
| //############################################################################ |
| { |
| static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer; |
| unsigned int Channel, index; |
| signed int signal, tmp; |
| int bCheckDelay; |
| uint8_t c; |
| c = UDR1; // get data byte |
| if (ReSync == 1) |
| { |
| // wait for beginning of new frame |
| ReSync = 0; |
| FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames |
| FrameCnt = 0; |
| Sync = 0; |
| ByteHigh = 0; |
| } |
| else |
| { |
| bCheckDelay = CheckDelay(FrameTimer); |
| if ( Sync == 0 ) |
| { |
| if(bCheckDelay) |
| { |
| // nach einer Pause von mind. 7ms erstes Sync-Character gefunden |
| // Zeichen ignorieren, da Bedeutung unbekannt |
| Sync = 1; |
| FrameCnt ++; |
| } |
| else |
| { |
| // Zeichen kam vor Ablauf der 7ms Sync-Pause |
| // warten auf erstes Sync-Zeichen |
| } |
| } |
| else if((Sync == 1) && !bCheckDelay) |
| { |
| // zweites Sync-Character ignorieren, Bedeutung unbekannt |
| Sync = 2; |
| FrameCnt ++; |
| } |
| else if((Sync == 2) && !bCheckDelay) |
| { |
| // Datenbyte high |
| ByteHigh = c; |
| if (FrameCnt == 2) |
| { |
| // is 1st Byte of Channel-data |
| // Frame 1 with Channel 1-7 comming next |
| Frame2 = 0; |
| if(ByteHigh & 0x80) |
| { |
| // DS9: Frame 2 with Channel 8-9 comming next |
| Frame2 = 1; |
| } |
| } |
| Sync = 3; |
| FrameCnt ++; |
| } |
| else if((Sync == 3) && !bCheckDelay) |
| { |
| // Datenbyte low |
| // High-Byte for next channel comes next |
| Sync = 2; |
| FrameCnt ++; |
| index = (ByteHigh >> 2) & 0x0f; |
| index ++; |
| Channel = (ByteHigh << 8) | c; |
| signal = Channel & 0x3ff; |
| signal -= 0x200; // Offset, range 0x000..0x3ff? |
| signal = signal/3; // scaling to fit PPM resolution |
| if(index >= 0 && index <= 10) |
| { |
| // Stabiles Signal |
| if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;} |
| tmp = (3 * (PPM_in[index]) + signal) / 4; |
| if(tmp > signal+1) tmp--; else |
| if(tmp < signal-1) tmp++; |
| if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; |
| } |
| } |
| else |
| { |
| // hier stimmt was nicht: neu synchronisieren |
| ReSync = 1; |
| FrameCnt = 0; |
| Frame2 = 0; |
| } |
| // 16 Bytes per frame |
| if(FrameCnt >= 16) |
| { |
| // Frame complete |
| if(Frame2 == 0) |
| { |
| // Null bedeutet: Neue Daten |
| // nur beim ersten Frame (CH 0-7) setzen |
| NewPpmData = 0; |
| } |
| // new frame next, nach fruehestens 7ms erwartet |
| FrameCnt = 0; |
| Frame2 = 0; |
| Sync = 0; |
| } |
| // Zeit bis zum nächsten Zeichen messen |
| FrameTimer = SetDelay(7); |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/spectrum.h |
|---|
| 0,0 → 1,9 |
| /*####################################################################################### |
| Dekodieren eines Spectrum Signals |
| #######################################################################################*/ |
| #ifndef _SPECTRUM_H |
| #define _SPECTRUM_H |
| void Uart1Init(void); |
| void SpektrumBinding(void); |
| #endif //_RC_H |
| /branches/dongfang_FC_rewrite/backup/spi.c |
|---|
| 0,0 → 1,421 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <string.h> |
| #include <stdlib.h> |
| #include "spi.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| #include "uart0.h" |
| #include "timer0.h" |
| #include "analog.h" |
| #include "attitude.h" |
| #include "GPSControl.h" |
| #include "flight.h" |
| //----------------------------------------- |
| #define DDR_SPI DDRB |
| #define DD_SS PB4 |
| #define DD_SCK PB7 |
| #define DD_MOSI PB5 |
| #define DD_MISO PB6 |
| // for compatibility reasons gcc3.x <-> gcc4.x |
| #ifndef SPCR |
| #define SPCR SPCR0 |
| #endif |
| #ifndef SPIE |
| #define SPIE SPIE0 |
| #endif |
| #ifndef SPE |
| #define SPE SPE0 |
| #endif |
| #ifndef DORD |
| #define DORD DORD0 |
| #endif |
| #ifndef MSTR |
| #define MSTR MSTR0 |
| #endif |
| #ifndef CPOL |
| #define CPOL CPOL0 |
| #endif |
| #ifndef CPHA |
| #define CPHA CPHA0 |
| #endif |
| #ifndef SPR1 |
| #define SPR1 SPR01 |
| #endif |
| #ifndef SPR0 |
| #define SPR0 SPR00 |
| #endif |
| #ifndef SPDR |
| #define SPDR SPDR0 |
| #endif |
| #ifndef SPSR |
| #define SPSR SPSR0 |
| #endif |
| #ifndef SPIF |
| #define SPIF SPIF0 |
| #endif |
| #ifndef WCOL |
| #define WCOL WCOL0 |
| #endif |
| #ifndef SPI2X |
| #define SPI2X SPI2X0 |
| #endif |
| // ------------------------- |
| #define SLAVE_SELECT_DDR_PORT DDRC |
| #define SLAVE_SELECT_PORT PORTC |
| #define SPI_SLAVE_SELECT PC5 |
| #define SPI_TXSYNCBYTE1 0xAA |
| #define SPI_TXSYNCBYTE2 0x83 |
| #define SPI_RXSYNCBYTE1 0x81 |
| #define SPI_RXSYNCBYTE2 0x55 |
| typedef enum { |
| SPI_SYNC1, |
| SPI_SYNC2, |
| SPI_DATA |
| } SPI_RXState_t; |
| // data exchange packets to and From NaviCtrl |
| ToNaviCtrl_t toNaviCtrl; |
| FromNaviCtrl_t fromNaviCtrl; |
| SPI_VersionInfo_t SPI_VersionInfo; |
| // rx packet buffer |
| #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
| uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
| uint8_t SPI_RxBufferIndex = 0; |
| uint8_t SPI_RxBuffer_Request = 0; |
| // tx packet buffer |
| #define SPI_TXBUFFER_LEN sizeof(toNaviCtrl) |
| uint8_t *SPI_TxBuffer; |
| uint8_t SPI_TxBufferIndex = 0; |
| uint8_t SPITransferCompleted, SPI_ChkSum; |
| uint8_t SPI_RxDataValid = 0; |
| uint8_t NCDataOkay = 0; |
| uint8_t NCSerialDataOkay = 0; |
| uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
| uint8_t SPI_CommandCounter = 0; |
| /*********************************************/ |
| /* Initialize SPI interface to NaviCtrl */ |
| /*********************************************/ |
| void SPI_MasterInit(void) { |
| DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK); // Set MOSI and SCK output, all others input |
| SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port |
| SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64 |
| SPSR = 0;//(1<<SPI2X); |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
| SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
| SPITransferCompleted = 1; |
| // initialize data packet to NaviControl |
| toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| toNaviCtrl.Command = SPI_CMD_USER; |
| toNaviCtrl.IntegralPitch = 0; |
| toNaviCtrl.IntegralRoll = 0; |
| NCSerialDataOkay = 0; |
| NCDataOkay = 0; |
| SPI_RxDataValid = 0; |
| SPI_VersionInfo.Major = VERSION_MAJOR; |
| SPI_VersionInfo.Minor = VERSION_MINOR; |
| SPI_VersionInfo.Patch = VERSION_PATCH; |
| SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| } |
| /**********************************************************/ |
| /* Update Data transferd by the SPI from/to NaviCtrl */ |
| /**********************************************************/ |
| void UpdateSPI_Buffer(void) { |
| uint8_t i; |
| int16_t tmp; |
| cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| // update content of packet to NaviCtrl |
| toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
| toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
| toNaviCtrl.GyroYaw = yawRate; |
| toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| // TODO: What are these little bastards? |
| averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
| switch(toNaviCtrl.Command) { |
| case SPI_CMD_USER: |
| for (i=0; i<sizeof(dynamicParams.UserParams); i++) { |
| toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| } |
| toNaviCtrl.Param.Byte[8] = MKFlags; |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
| toNaviCtrl.Param.Byte[9] = (uint8_t)UBat; |
| toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| break; |
| case SPI_CMD_PARAMETER1: |
| toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
| toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
| toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
| toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
| toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
| toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
| toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| break; |
| case SPI_CMD_STICK: |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
| toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| break; |
| case SPI_CMD_MISC: |
| toNaviCtrl.Param.Byte[0] = compassCalState; |
| if(compassCalState > 4) { // jump from 5 to 0 |
| compassCalState = 0; |
| } |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| // TODO: Height and in the correct scaling... |
| toNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| break; |
| case SPI_CMD_VERSION: |
| toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| toNaviCtrl.Param.Byte[4] = BoardRelease; |
| break; |
| default: |
| break; |
| } |
| sei(); // enable all interrupts |
| // analyze content of packet from NaviCtrl if valid |
| if (SPI_RxDataValid) { |
| // update gps controls |
| if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
| GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
| NCDataOkay = 250; |
| } |
| // update compass readings |
| if(fromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = fromNaviCtrl.CompassHeading; |
| } |
| if(compassHeading < 0) compassOffCourse = 0; |
| else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| // NaviCtrl wants to beep? |
| if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime; |
| switch (fromNaviCtrl.Command) { |
| case SPI_KALMAN: |
| dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
| dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1]; |
| dynamicParams.KalmanMaxDrift = fromNaviCtrl.Param.Byte[2]; |
| NCSerialDataOkay = fromNaviCtrl.Param.Byte[3]; |
| break; |
| default: |
| break; |
| } |
| } else { // no valid data from NaviCtrl |
| // disable GPS control |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| } |
| } |
| /*********************************************/ |
| /* Start Transmission of packet to NaviCtrl */ |
| /*********************************************/ |
| void SPI_StartTransmitPacket(void){ |
| if (!SPITransferCompleted) return; // return immediately if transfer is in progress |
| else // transmission was completed |
| { |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
| // cyclic commands |
| toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| SPITransferCompleted = 0; // transfer is in progress |
| UpdateSPI_Buffer(); // update data in toNaviCtrl |
| SPI_TxBufferIndex = 1; //proceed with 2nd byte |
| // -- Debug-Output --- |
| //---- |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum |
| SPDR = toNaviCtrl.Sync1; // send first byte |
| } |
| } |
| //------------------------------------------------------ |
| // This is the spi data transfer between FlightCtrl and NaviCtrl |
| // Every time this routine is called within the mainloop one byte of the packet to |
| // the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered |
| void SPI_TransmitByte(void) { |
| static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
| uint8_t rxdata; |
| static uint8_t rxchksum; |
| if (SPITransferCompleted) return; // return immediatly if transfer was completed |
| if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending |
| SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
| rxdata = SPDR; // save spi data register |
| switch (SPI_RXState) { |
| case SPI_SYNC1: // first sync byte |
| SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
| rxchksum = rxdata; // initialize checksum |
| if (rxdata == SPI_RXSYNCBYTE1 ) |
| { // 1st Syncbyte found |
| SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
| } |
| break; |
| case SPI_SYNC2: // second sync byte |
| if (rxdata == SPI_RXSYNCBYTE2) |
| { // 2nd Syncbyte found |
| rxchksum += rxdata; // update checksum |
| SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
| } |
| else // 2nd Syncbyte not found |
| { |
| SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte |
| } |
| break; |
| case SPI_DATA: // data bytes |
| SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
| // if all bytes are received of a packet from the NaviCtrl |
| if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) { // last byte transfered is the checksum of the packet |
| if (rxdata == rxchksum) { // checksum matching? |
| // copy SPI_RxBuffer -> FromFlightCtrl |
| uint8_t *ptr = (uint8_t *)&fromNaviCtrl; |
| cli(); |
| memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl)); |
| sei(); |
| SPI_RxDataValid = 1; |
| } else { // checksum does not match |
| SPI_RxDataValid = 0; // reset valid flag |
| } |
| SPI_RXState = SPI_SYNC1; // reset state sync |
| } else { // not all bytes transfered |
| rxchksum += rxdata; // update checksum |
| } |
| break; |
| }// eof switch(SPI_RXState) |
| // if still some bytes left for transmission to NaviCtrl |
| if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
| toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
| SPI_TxBufferIndex++; |
| } else { |
| //Transfer of all bytes of the packet to NaviCtrl completed |
| SPITransferCompleted = 1; |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/spi.h |
|---|
| 0,0 → 1,86 |
| // ######################## SPI - FlightCtrl ################### |
| #ifndef _SPI_H |
| #define _SPI_H |
| //#include <util/delay.h> |
| #include <inttypes.h> |
| #define SPI_PROTOCOL_COMP 1 |
| #define SPI_CMD_USER 10 |
| #define SPI_CMD_STICK 11 |
| #define SPI_CMD_MISC 12 |
| #define SPI_CMD_PARAMETER1 13 |
| #define SPI_CMD_VERSION 14 |
| typedef struct { |
| uint8_t Sync1; |
| uint8_t Sync2; |
| uint8_t Command; |
| int16_t IntegralPitch; |
| int16_t IntegralRoll; |
| int16_t AccPitch; |
| int16_t AccRoll; |
| int16_t GyroHeading; |
| int16_t GyroPitch; |
| int16_t GyroRoll; |
| int16_t GyroYaw; |
| union { |
| int8_t sByte[12]; |
| uint8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) ToNaviCtrl_t; |
| #define SPI_CMD_OSD_DATA 100 |
| #define SPI_CMD_GPS_POS 101 |
| #define SPI_CMD_GPS_TARGET 102 |
| #define SPI_KALMAN 103 |
| typedef struct { |
| uint8_t Command; |
| int16_t GPSStickPitch; |
| int16_t GPSStickRoll; |
| int16_t GPS_Yaw; |
| int16_t CompassHeading; |
| int16_t Status; |
| uint16_t BeepTime; |
| union { |
| int8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) FromNaviCtrl_t; |
| typedef struct { |
| uint8_t Major; |
| uint8_t Minor; |
| uint8_t Patch; |
| uint8_t Compatible; |
| // unsigned char Hardware; |
| } __attribute__((packed)) SPI_VersionInfo_t; |
| extern ToNaviCtrl_t toNaviCtrl; |
| extern FromNaviCtrl_t fromNaviCtrl; |
| typedef struct { |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| uint8_t SerialDataOkay; |
| } __attribute__((packed)) NCData_t; |
| extern uint8_t NCDataOkay; |
| extern uint8_t NCSerialDataOkay; |
| void SPI_MasterInit(void); |
| void SPI_StartTransmitPacket(void); |
| void SPI_TransmitByte(void); |
| // new: |
| // extern void UpdateSPI_Buffer(void); |
| #endif //_SPI_H |
| /branches/dongfang_FC_rewrite/backup/timer0.c |
|---|
| 0,0 → 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 (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 sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <inttypes.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include "eeprom.h" |
| #include "analog.h" |
| // for degugging! |
| #include "rc.h" |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| volatile uint16_t CountMilliseconds = 0; |
| volatile uint8_t runFlightControl = 0; |
| volatile uint16_t cntKompass = 0; |
| volatile uint16_t BeepTime = 0; |
| volatile uint16_t BeepModulation = 0xFFFF; |
| #ifdef USE_NAVICTRL |
| volatile uint8_t SendSPI = 0; |
| #endif |
| /***************************************************** |
| * Initialize Timer 0 |
| *****************************************************/ |
| // timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor |
| // Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate |
| void timer0_init(void) { |
| uint8_t sreg = SREG; |
| // disable all interrupts before reconfiguration |
| cli(); |
| // Configure speaker port as output. |
| if(BoardRelease == 10) { // Speaker at PD2 |
| DDRD |= (1<<DDD2); |
| PORTD &= ~(1<<PORTD2); |
| } else { // Speaker at PC7 |
| DDRC |= (1<<DDC7); |
| PORTC &= ~(1<<PORTC7); |
| } |
| // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor |
| DDRB |= (1<<DDB4)|(1<<DDB3); |
| PORTB &= ~((1<<PORTB4)|(1<<PORTB3)); |
| // Timer/Counter 0 Control Register A |
| // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1) |
| // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0) |
| // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0) |
| TCCR0A &= ~((1<<COM0A0)|(1<<COM0B0)); |
| TCCR0A |= (1<<COM0A1)|(1<<COM0B1)|(1<<WGM01)|(1<<WGM00); |
| // Timer/Counter 0 Control Register B |
| // set clock divider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz |
| // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
| // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
| // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
| TCCR0B &= ~((1<<FOC0A)|(1<<FOC0B)|(1<<WGM02)); |
| TCCR0B = (TCCR0B & 0xF8)|(0<<CS02)|(1<<CS01)|(0<<CS00); |
| // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4 |
| OCR0A = 0; // for PB3 |
| OCR0B = 120; // for PB4 |
| // init Timer/Counter 0 Register |
| TCNT0 = 0; |
| // Timer/Counter 0 Interrupt Mask Register |
| // enable timer overflow interrupt only |
| TIMSK0 &= ~((1<<OCIE0B)|(1<<OCIE0A)); |
| TIMSK0 |= (1<<TOIE0); |
| SREG = sreg; |
| } |
| /*****************************************************/ |
| /* Interrupt Routine of Timer 0 */ |
| /*****************************************************/ |
| ISR(TIMER0_OVF_vect) { // 9765.625 Hz |
| static uint8_t cnt_1ms = 1,cnt = 0; |
| uint8_t Beeper_On = 0; |
| #ifdef USE_NAVICTRL |
| if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
| #endif |
| if(!cnt--) { // every 10th run (9.765kHz/10 = 976Hz) |
| cnt = 9; |
| cnt_1ms^=1; |
| if(!cnt_1ms) { |
| runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
| } |
| CountMilliseconds++; // increment millisecond counter |
| } |
| // beeper on if duration is not over |
| if(BeepTime) { |
| BeepTime--; // decrement BeepTime |
| if(BeepTime & BeepModulation) Beeper_On = 1; |
| else Beeper_On = 0; |
| } |
| else { // beeper off if duration is over |
| Beeper_On = 0; |
| BeepModulation = 0xFFFF; |
| } |
| // if beeper is on |
| if(Beeper_On) { |
| // set speaker port to high. |
| if(BoardRelease == 10) PORTD |= (1<<PORTD2); // Speaker at PD2 |
| else PORTC |= (1<<PORTC7); // Speaker at PC7 |
| } else { // beeper is off |
| // set speaker port to low |
| if(BoardRelease == 10) PORTD &= ~(1<<PORTD2);// Speaker at PD2 |
| else PORTC &= ~(1<<PORTC7);// Speaker at PC7 |
| } |
| #ifndef USE_NAVICTRL |
| // update compass value if this option is enabled in the settings |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| #ifdef USE_MK3MAG |
| MK3MAG_Update(); // read out mk3mag pwm |
| #endif |
| } |
| #endif |
| } |
| // ----------------------------------------------------------------------- |
| uint16_t SetDelay (uint16_t t) { |
| return(CountMilliseconds + t - 1); |
| } |
| // ----------------------------------------------------------------------- |
| int8_t CheckDelay(uint16_t t) { |
| return(((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit |
| } |
| // ----------------------------------------------------------------------- |
| void Delay_ms(uint16_t w) { |
| uint16_t t_stop = SetDelay(w); |
| while (!CheckDelay(t_stop)); |
| } |
| // ----------------------------------------------------------------------- |
| void Delay_ms_Mess(uint16_t w) { |
| uint16_t t_stop; |
| t_stop = SetDelay(w); |
| while (!CheckDelay(t_stop)) { |
| if(analogDataReady) { |
| analogDataReady = 0; |
| analog_start(); |
| } |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/timer0.h |
|---|
| 0,0 → 1,21 |
| #ifndef _TIMER0_H |
| #define _TIMER0_H |
| #include <inttypes.h> |
| extern volatile uint16_t CountMilliseconds; |
| extern volatile uint8_t runFlightControl; |
| extern volatile uint16_t cntKompass; |
| extern volatile uint16_t BeepModulation; |
| extern volatile uint16_t BeepTime; |
| #ifdef USE_NAVICTRL |
| extern volatile uint8_t SendSPI; |
| #endif |
| extern void timer0_init(void); |
| extern void Delay_ms(uint16_t w); |
| extern void Delay_ms_Mess(uint16_t w); |
| extern uint16_t SetDelay (uint16_t t); |
| extern int8_t CheckDelay (uint16_t t); |
| #endif //_TIMER0_H |
| /branches/dongfang_FC_rewrite/backup/timer2.c |
|---|
| 0,0 → 1,296 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include "eeprom.h" |
| #include "uart0.h" |
| #include "rc.h" |
| #include "attitude.h" |
| volatile int16_t ServoPitchValue = 0; |
| volatile int16_t ServoRollValue = 0; |
| volatile uint8_t ServoActive = 0; |
| #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| /***************************************************** |
| * Initialize Timer 2 |
| *****************************************************/ |
| // The timer 2 is used to generate the PWM at PD7 (J7) |
| // to control a camera servo for pitch compensation. |
| void timer2_init(void) { |
| uint8_t sreg = SREG; |
| // disable all interrupts before reconfiguration |
| cli(); |
| // set PD7 as output of the PWM for pitch servo |
| DDRD |= (1<<DDD7); |
| PORTD &= ~(1<<PORTD7); // set PD7 to low |
| DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
| //PORTC &= ~(1<<PORTC6); // set PC6 to low |
| HEF4017R_ON; // enable reset |
| // Timer/Counter 2 Control Register A |
| // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1) |
| // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
| // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
| TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
| TCCR2A |= (1<<WGM21)|(1<<WGM20); |
| // Timer/Counter 2 Control Register B |
| // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
| // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
| // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
| // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
| TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22)); |
| TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22); |
| // Initialize the Timer/Counter 2 Register |
| TCNT2 = 0; |
| // Initialize the Output Compare Register A used for PWM generation on port PD7. |
| OCR2A = 255; |
| TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0 |
| // Timer/Counter 2 Interrupt Mask Register |
| // Enable timer output compare match A Interrupt only |
| TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
| TIMSK2 |= (1<<OCIE2A); |
| SREG = sreg; |
| } |
| void Servo_On(void) { |
| ServoActive = 1; |
| } |
| void Servo_Off(void) { |
| ServoActive = 0; |
| HEF4017R_ON; // enable reset |
| } |
| /***************************************************** |
| * Control Servo Position |
| *****************************************************/ |
| ISR(TIMER2_COMPA_vect) { |
| // frame len 22.5 ms = 14063 * 1.6 us |
| // stop pulse: 0.3 ms = 188 * 1.6 us |
| // min servo pulse: 0.6 ms = 375 * 1.6 us |
| // max servo pulse: 2.4 ms = 1500 * 1.6 us |
| // resolution: 1500 - 375 = 1125 steps |
| #define PPM_STOPPULSE 188 |
| #define PPM_FRAMELEN (1757 * .ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
| #define MINSERVOPULSE 375 |
| #define MAXSERVOPULSE 1500 |
| #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
| #if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS) |
| static uint8_t isGeneratingPulse = 0; |
| static uint16_t remainingPulseLength = 0; |
| static uint16_t ServoFrameTime = 0; |
| static uint8_t ServoIndex = 0; |
| #define MULTIPLIER 4 |
| static int16_t ServoPitchOffset = (255 / 2) * MULTIPLIER; // initial value near center position |
| static int16_t ServoRollOffset = (255 / 2) * MULTIPLIER; // initial value near center position |
| #endif |
| #ifdef USE_NON_4017_SERVO_OUTPUTS |
| //--------------------------- |
| // Pitch servo state machine |
| //--------------------------- |
| if (!isGeneratingPulse) { // pulse output complete on _next_ interrupt |
| if(TCCR2A & (1<<COM2A0)) { // we are still outputting a high pulse |
| TCCR2A &= ~(1<<COM2A0); // make a low pulse on _next_ interrupt, and now |
| remainingPulseLength = MINSERVOPULSE + SERVORANGE / 2; // center position ~ 1.5ms |
| ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl) / 4; // lowpass offset |
| if(staticParams.ServoPitchCompInvert & 0x01) { |
| // inverting movement of servo |
| // todo: function. |
| ServoPitchValue = ServoPitchOffset + (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L)); |
| } else { |
| // todo: function. |
| // non inverting movement of servo |
| ServoPitchValue = ServoPitchOffset - (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L)); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoPitchValue < (int16_t)staticParams.ServoPitchMin) { |
| ServoPitchValue = (int16_t)staticParams.ServoPitchMin; |
| } else if(ServoPitchValue > (int16_t)staticParams.ServoPitchMax) { |
| ServoPitchValue = (int16_t)staticParams.ServoPitchMax; |
| } |
| remainingPulseLength = (ServoPitchValue - 256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position |
| // range servo pulse width |
| if(remainingPulseLength > MAXSERVOPULSE ) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit |
| else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit |
| // accumulate time for correct update rate |
| ServoFrameTime = remainingPulseLength; |
| } else { // we had a high pulse |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| remainingPulseLength = PPM_FRAMELEN - ServoFrameTime; |
| } |
| // set pulse output active |
| isGeneratingPulse = 1; |
| } // EOF Pitch servo state machine |
| #elseif defined(USE_4017_SERVOS) |
| //----------------------------------------------------- |
| // PPM state machine, onboard demultiplexed by HEF4017 |
| //----------------------------------------------------- |
| if(!isGeneratingPulse) { // pulse output complete |
| if(TCCR2A & (1<<COM2A0)) { // we had a low pulse |
| TCCR2A &= ~(1<<COM2A0);// make a high pulse |
| if(ServoIndex == 0) { // if we are at the sync gap |
| remainingPulseLength = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
| ServoFrameTime = 0; // reset servo frame time |
| HEF4017R_ON; // enable HEF4017 reset |
| } else { // servo channels |
| remainingPulseLength = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| switch(ServoIndex) { // map servo channels |
| case 1: // Pitch Compensation Servo |
| ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl * MULTIPLIER) / 4; // lowpass offset |
| ServoPitchValue = ServoPitchOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(staticParams.ServoPitchCompInvert & 0x01) { |
| // inverting movement of servo |
| ServoPitchValue += (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) ); |
| } else { // non inverting movement of servo |
| ServoPitchValue -= (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoPitchValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) { |
| ServoPitchValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER; |
| } else if(ServoPitchValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) { |
| ServoPitchValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER; |
| } |
| remainingPulseLength += ServoPitchValue - (256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position |
| ServoPitchValue /= MULTIPLIER; |
| break; |
| case 2: // Roll Compensation Servo |
| ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLIER) / 4; // lowpass offset |
| ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
| //if(staticParams.ServoRollCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLIER * (integralGyroRoll / 128L ) ) / (256L) ); |
| } |
| /* else |
| { // non inverting movement of servo |
| ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLIER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
| } |
| */ // limit servo value to its parameter range definition |
| if(ServoRollValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) { |
| ServoRollValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER; |
| } else if(ServoRollValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) { |
| ServoRollValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER; |
| } |
| remainingPulseLength += ServoRollValue - (256 / 2) * MULTIPLIER; // shift ServoRollValue to center position |
| ServoRollValue /= MULTIPLIER; |
| break; |
| default: // other servo channels |
| remainingPulseLength += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
| break; |
| } |
| // range servo pulse width |
| if(remainingPulseLength > MAXSERVOPULSE) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit |
| else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit |
| // substract stop pulse width |
| remainingPulseLength -= PPM_STOPPULSE; |
| // accumulate time for correct sync gap |
| ServoFrameTime += remainingPulseLength; |
| } |
| } else { // we had a high pulse |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| // set pulsewidth to stop pulse width |
| remainingPulseLength = PPM_STOPPULSE; |
| // accumulate time for correct sync gap |
| ServoFrameTime += remainingPulseLength; |
| if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset |
| ServoIndex++; // change to next servo channel |
| if(ServoIndex > staticParams.ServoRefresh) ServoIndex = 0; // reset to the sync gap |
| } |
| // set pulse output active |
| isGeneratingPulse = 1; |
| } |
| #endif |
| /* |
| * Cases: |
| * 1) 255 + 128 <= remainingPulseLength --> delta = 255 |
| * 2) 255 <= remainingPulseLength < 255 + 128 --> delta = 255 - 128 |
| * this is to avoid a too short delta on the last cycle, which would cause |
| * an interupt-on-interrupt condition and the loss of the last interrupt. |
| * 3) remainingPulseLength < 255 --> delta = remainingPulseLength |
| */ |
| #if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS) |
| uint8_t delta; |
| if (remainingPulseLength >= (255 + 128)) { |
| delta = 255; |
| } else if (remainingPulseLength >= 255) { |
| delta = 255- 128; |
| } else { |
| delta = remainingPulseLength; |
| isGeneratingPulse = 0; // trigger to stop pulse |
| } |
| OCR2A = delta; |
| remainingPulseLength -= delta; |
| #endif |
| } |
| /branches/dongfang_FC_rewrite/backup/timer2.h |
|---|
| 0,0 → 1,14 |
| #ifndef _TIMER2_H |
| #define _TIMER2_H |
| #include <inttypes.h> |
| extern volatile int16_t ServoNickValue; |
| extern volatile int16_t ServoRollValue; |
| void timer2_init(void); |
| void Servo_On(void); |
| void Servo_Off(void); |
| #endif //_TIMER2_H |
| /branches/dongfang_FC_rewrite/backup/twimaster.c |
|---|
| 0,0 → 1,310 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <util/twi.h> |
| #include <util/delay.h> |
| //#include "eeprom.h" |
| #include "twimaster.h" |
| //#include "analog.h" |
| #include "configuration.h" |
| #include "printf_P.h" |
| volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
| volatile uint8_t dac_channel = 0; |
| volatile uint8_t motor_write = 0; |
| volatile uint8_t motor_read = 0; |
| volatile uint16_t I2CTimeout = 100; |
| uint8_t missingMotor = 0; |
| MotorData_t Motor[MAX_MOTORS]; |
| volatile uint8_t DACValues[4]; |
| uint8_t DACChannel = 0; |
| #define SCL_CLOCK 200000L |
| #define I2C_TIMEOUT 30000 |
| /************************************************** |
| * Initialize I2C (TWI) |
| **************************************************/ |
| void I2C_init(void) { |
| uint8_t i; |
| uint8_t sreg = SREG; |
| cli(); |
| // SDA is INPUT |
| DDRC &= ~(1<<DDC1); |
| // SCL is output |
| DDRC |= (1<<DDC0); |
| // pull up SDA |
| PORTC |= (1<<PORTC0)|(1<<PORTC1); |
| // TWI Status Register |
| // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
| TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
| // set TWI Bit Rate Register |
| TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
| twi_state = TWI_STATE_MOTOR_TX; |
| motor_write = 0; |
| motor_read = 0; |
| for(i=0; i < MAX_MOTORS; i++) { |
| Motor[i].SetPoint = 0; |
| Motor[i].Present = 0; |
| Motor[i].Error = 0; |
| Motor[i].MaxPWM = 0; |
| } |
| SREG = sreg; |
| } |
| /**************************************** |
| * Start I2C |
| ****************************************/ |
| void I2C_Start(uint8_t start_state) { |
| twi_state = start_state; |
| // TWI Control Register |
| // clear TWI interrupt flag (TWINT=1) |
| // disable TWI Acknowledge Bit (TWEA = 0) |
| // enable TWI START Condition Bit (TWSTA = 1), MASTER |
| // disable TWI STOP Condition Bit (TWSTO = 0) |
| // disable TWI Write Collision Flag (TWWC = 0) |
| // enable i2c (TWEN = 1) |
| // enable TWI Interrupt (TWIE = 1) |
| TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
| } |
| /**************************************** |
| * Stop I2C |
| ****************************************/ |
| void I2C_Stop(uint8_t start_state) { |
| twi_state = start_state; |
| // TWI Control Register |
| // clear TWI interrupt flag (TWINT=1) |
| // disable TWI Acknowledge Bit (TWEA = 0) |
| // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
| // enable TWI STOP Condition Bit (TWSTO = 1) |
| // disable TWI Write Collision Flag (TWWC = 0) |
| // enable i2c (TWEN = 1) |
| // disable TWI Interrupt (TWIE = 0) |
| TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
| } |
| /**************************************** |
| * Write to I2C |
| ****************************************/ |
| void I2C_WriteByte(int8_t byte) { |
| // move byte to send into TWI Data Register |
| TWDR = byte; |
| // clear interrupt flag (TWINT = 1) |
| // enable i2c bus (TWEN = 1) |
| // enable interrupt (TWIE = 1) |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| } |
| /**************************************** |
| * Receive byte and send ACK |
| ****************************************/ |
| void I2C_ReceiveByte(void) { |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
| } |
| /**************************************** |
| * I2C receive last byte and send no ACK |
| ****************************************/ |
| void I2C_ReceiveLastByte(void){ |
| TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| } |
| /**************************************** |
| * Reset I2C |
| ****************************************/ |
| void I2C_Reset(void) { |
| // stop i2c bus |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| twi_state = 0; |
| motor_write = TWDR; |
| motor_write = 0; |
| motor_read = 0; |
| TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
| TWAMR = 0; |
| TWAR = 0; |
| TWDR = 0; |
| TWSR = 0; |
| TWBR = 0; |
| I2C_init(); |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| } |
| /**************************************** |
| * I2C ISR |
| ****************************************/ |
| ISR (TWI_vect) { |
| static uint8_t missing_motor = 0; |
| switch (twi_state++) { // First i2c_start from SendMotorData() |
| // Master Transmit |
| case 0: // TWI_STATE_MOTOR_TX |
| // skip motor if not used in mixer |
| while((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
| if(motor_write >= MAX_MOTORS) { // writing finished, read now |
| motor_write = 0; |
| twi_state = TWI_STATE_MOTOR_RX; |
| I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
| } |
| else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
| break; |
| case 1: // Send Data to Slave |
| I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint |
| break; |
| case 2: // repeat case 0+1 for all motors |
| if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
| if(!missing_motor) missing_motor = motor_write + 1; |
| if(++Motor[motor_write].Error == 0) Motor[motor_write].Error = 255; // increment error counter and handle overflow |
| } |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| I2CTimeout = 10; |
| motor_write++; // next motor |
| I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
| break; |
| // Master Receive Data |
| case 3: |
| if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
| // no response from the addressed slave received |
| Motor[motor_read].Present = 0; |
| motor_read++; // next motor |
| if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| } else { |
| Motor[motor_read].Present = ('1' - '-') + motor_read; |
| I2C_ReceiveByte(); //Transmit 1st byte |
| } |
| missingMotor = missing_motor; |
| missing_motor = 0; |
| break; |
| case 4: //Read 1st byte and transmit 2nd Byte |
| Motor[motor_read].Current = TWDR; |
| I2C_ReceiveLastByte(); // nack |
| break; |
| case 5: |
| //Read 2nd byte |
| Motor[motor_read].MaxPWM = TWDR; |
| motor_read++; // next motor |
| if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| break; |
| // Writing ADC values. |
| case 7: |
| I2C_WriteByte(0x98); // Address the DAC |
| break; |
| case 8: |
| I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
| break; |
| case 9: |
| I2C_WriteByte(DACValues[DACChannel]); |
| break; |
| case 10: |
| I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
| break; |
| case 11: |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| I2CTimeout = 10; |
| // repeat case 7...10 until all DAC Channels are updated |
| if(DACChannel < 2) { |
| DACChannel ++; // jump to next channel |
| I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
| } else { |
| DACChannel = 0; // reset dac channel counter |
| } |
| break; |
| default: |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| I2CTimeout = 10; |
| motor_write = 0; |
| motor_read = 0; |
| } |
| } |
| extern void twi_diagnostics(void) { |
| // Check connected BL-Ctrls |
| uint8_t i; |
| printf("\n\rFound BL-Ctrl: "); |
| for(i = 0; i < MAX_MOTORS; i++) { |
| Motor[i].SetPoint = 0; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| _delay_ms(2); |
| motor_read = 0; // read the first I2C-Data |
| for(i = 0; i < MAX_MOTORS; i++) { |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| _delay_ms(2); |
| if(Motor[i].Present) printf("%d ",i+1); |
| } |
| for(i = 0; i < MAX_MOTORS; i++) { |
| if(!Motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
| Motor[i].Error = 0; |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/twimaster.h |
|---|
| 0,0 → 1,35 |
| #ifndef _I2C_MASTER_H |
| #define _I2C_MASTER_H |
| #include <inttypes.h> |
| #define TWI_STATE_MOTOR_TX 0 |
| #define TWI_STATE_MOTOR_RX 3 |
| #define TWI_STATE_GYRO_OFFSET_TX 7 |
| extern volatile uint8_t twi_state; |
| extern uint8_t missingMotor; |
| volatile extern uint8_t DACValues[4]; |
| typedef struct { |
| uint8_t SetPoint; // written by attitude controller |
| uint8_t Present; // 0 if BL was found |
| uint8_t Error; // I2C error counter |
| uint8_t Current; // read byck from BL |
| uint8_t MaxPWM; // read back from BL |
| } __attribute__((packed)) MotorData_t; |
| #define MAX_MOTORS 12 |
| extern MotorData_t Motor[MAX_MOTORS]; |
| extern volatile uint16_t I2CTimeout; |
| extern void I2C_init (void); // Initialize I2C |
| extern void I2C_Start(uint8_t start_state); // Start I2C |
| extern void I2C_Stop (uint8_t start_state); // Stop I2C |
| extern void I2C_Reset(void); // Reset I2C |
| extern void twi_diagnostics(void); |
| #endif |
| /branches/dongfang_FC_rewrite/backup/uart0.c |
|---|
| 0,0 → 1,734 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <avr/wdt.h> |
| #include <avr/pgmspace.h> |
| #include <stdarg.h> |
| #include <string.h> |
| #include "eeprom.h" |
| #include "menu.h" |
| #include "timer0.h" |
| #include "uart0.h" |
| #include "attitude.h" |
| #include "rc.h" |
| #include "externalControl.h" |
| #include "output.h" |
| #if defined (USE_MK3MAG) |
| #include "ubx.h" |
| #endif |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| #define FC_ADDRESS 1 |
| #define NC_ADDRESS 2 |
| #define MK3MAG_ADDRESS 3 |
| #define FALSE 0 |
| #define TRUE 1 |
| //int8_t test __attribute__ ((section (".noinit"))); |
| uint8_t request_VerInfo = FALSE; |
| uint8_t request_ExternalControl = FALSE; |
| uint8_t request_Display = FALSE; |
| uint8_t request_Display1 = FALSE; |
| uint8_t request_DebugData = FALSE; |
| uint8_t request_Data3D = FALSE; |
| uint8_t request_DebugLabel = 255; |
| uint8_t request_PPMChannels = FALSE; |
| uint8_t request_MotorTest = FALSE; |
| uint8_t request_variables = FALSE; |
| uint8_t DisplayLine = 0; |
| volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
| volatile uint8_t rxd_buffer_locked = FALSE; |
| volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
| volatile uint8_t txd_complete = TRUE; |
| volatile uint8_t ReceivedBytes = 0; |
| volatile uint8_t *pRxData = 0; |
| volatile uint8_t RxDataLen = 0; |
| uint8_t motorTestActive = 0; |
| uint8_t motorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; |
| uint8_t ConfirmFrame; |
| typedef struct { |
| int16_t Heading; |
| } __attribute__((packed)) Heading_t; |
| DebugOut_t DebugOut; |
| Data3D_t Data3D; |
| UART_VersionInfo_t UART_VersionInfo; |
| uint16_t DebugData_Timer; |
| uint16_t Data3D_Timer; |
| uint16_t DebugData_Interval = 500; // in 1ms |
| uint16_t Data3D_Interval = 0; // in 1ms |
| #ifdef USE_MK3MAG |
| int16_t Compass_Timer; |
| #endif |
| // keep lables in flash to save 512 bytes of sram space |
| const prog_uint8_t ANALOG_LABEL[32][16] = { |
| //1234567890123456 |
| "AnglePitch ", //0 |
| "AngleRoll ", |
| "AngleYaw ", |
| "GyroPitch(PID) ", |
| "GyroRoll(PID) ", |
| "GyroYaw ", //5 |
| "GyroPitch(AC) ", |
| "GyroRoll(AC) ", |
| "GyroYaw(AC) ", |
| "AccPitch (angle)", |
| "AccRoll (angle) ", //10 |
| "UBat ", |
| "RC Pitch ", |
| "RC Roll ", |
| "Corr 0/00 pitch ", |
| "Corr 0/00 roll ", //15 |
| "Corr pitch ", |
| "Corr roll ", |
| "Acc pitch filter", |
| "Acc roll filter ", |
| "ADPitchGyroOffs ", //20 |
| "ADRollGyroOffs ", |
| "M1 ", |
| "M2 ", |
| "M3 ", |
| "M4 ", //25 |
| "Pitch acc noise ", |
| "Roll acc noise ", |
| "DriftCompPitch ", |
| "DriftCompRoll ", |
| "Pitch Noise ", //30 |
| "Roll Noise " |
| }; |
| /****************************************************************/ |
| /* Initialization of the USART0 */ |
| /****************************************************************/ |
| void usart0_Init (void) { |
| uint8_t sreg = SREG; |
| uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1); |
| // disable all interrupts before configuration |
| cli(); |
| // disable RX-Interrupt |
| UCSR0B &= ~(1 << RXCIE0); |
| // disable TX-Interrupt |
| UCSR0B &= ~(1 << TXCIE0); |
| // set direction of RXD0 and TXD0 pins |
| // set RXD0 (PD0) as an input pin |
| PORTD |= (1 << PORTD0); |
| DDRD &= ~(1 << DDD0); |
| // set TXD0 (PD1) as an output pin |
| PORTD |= (1 << PORTD1); |
| DDRD |= (1 << DDD1); |
| // USART0 Baud Rate Register |
| // set clock divider |
| UBRR0H = (uint8_t)(ubrr >> 8); |
| UBRR0L = (uint8_t)ubrr; |
| // USART0 Control and Status Register A, B, C |
| // enable double speed operation in |
| UCSR0A |= (1 << U2X0); |
| // enable receiver and transmitter in |
| UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
| // set asynchronous mode |
| UCSR0C &= ~(1 << UMSEL01); |
| UCSR0C &= ~(1 << UMSEL00); |
| // no parity |
| UCSR0C &= ~(1 << UPM01); |
| UCSR0C &= ~(1 << UPM00); |
| // 1 stop bit |
| UCSR0C &= ~(1 << USBS0); |
| // 8-bit |
| UCSR0B &= ~(1 << UCSZ02); |
| UCSR0C |= (1 << UCSZ01); |
| UCSR0C |= (1 << UCSZ00); |
| // flush receive buffer |
| while ( UCSR0A & (1<<RXC0) ) UDR0; |
| // enable interrupts at the end |
| // enable RX-Interrupt |
| UCSR0B |= (1 << RXCIE0); |
| // enable TX-Interrupt |
| UCSR0B |= (1 << TXCIE0); |
| // initialize the debug timer |
| DebugData_Timer = SetDelay(DebugData_Interval); |
| // unlock rxd_buffer |
| rxd_buffer_locked = FALSE; |
| pRxData = 0; |
| RxDataLen = 0; |
| // no bytes to send |
| txd_complete = TRUE; |
| #ifdef USE_MK3MAG |
| Compass_Timer = SetDelay(220); |
| #endif |
| UART_VersionInfo.SWMajor = VERSION_MAJOR; |
| UART_VersionInfo.SWMinor = VERSION_MINOR; |
| UART_VersionInfo.SWPatch = VERSION_PATCH; |
| UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
| UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
| // restore global interrupt flags |
| SREG = sreg; |
| } |
| /****************************************************************/ |
| /* USART0 transmitter ISR */ |
| /****************************************************************/ |
| ISR(USART0_TX_vect) { |
| static uint16_t ptr_txd_buffer = 0; |
| uint8_t tmp_tx; |
| if(!txd_complete) { // transmission not completed |
| ptr_txd_buffer++; // die [0] wurde schon gesendet |
| tmp_tx = txd_buffer[ptr_txd_buffer]; |
| // if terminating character or end of txd buffer was reached |
| if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) { |
| ptr_txd_buffer = 0; // reset txd pointer |
| txd_complete = 1; // stop transmission |
| } |
| UDR0 = tmp_tx; // send current byte will trigger this ISR again |
| } |
| // transmission completed |
| else ptr_txd_buffer = 0; |
| } |
| /****************************************************************/ |
| /* USART0 receiver ISR */ |
| /****************************************************************/ |
| ISR(USART0_RX_vect) { |
| static uint16_t crc; |
| static uint8_t ptr_rxd_buffer = 0; |
| uint8_t crc1, crc2; |
| uint8_t c; |
| DebugOut.Digital[0] ^= DEBUG_SERIAL; |
| DebugOut.Digital[1] ^= DEBUG_SERIAL; |
| c = UDR0; // catch the received byte |
| #if (defined (USE_MK3MAG)) |
| // If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart. |
| if(CPUType != ATMEGA644P) ubx_parser(c); |
| #endif |
| if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return |
| // the rxd buffer is unlocked |
| if((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
| crc = c; // init crc |
| } |
| #if 0 |
| else if (ptr_rxd_buffer == 1) { // handle address |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| crc += c; // update crc |
| } |
| #endif |
| else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
| if(c != '\r') { // no termination character |
| rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
| crc += c; // update crc |
| } else { // termination character was received |
| // the last 2 bytes are no subject for checksum calculation |
| // they are the checksum itself |
| crc -= rxd_buffer[ptr_rxd_buffer-2]; |
| crc -= rxd_buffer[ptr_rxd_buffer-1]; |
| // calculate checksum from transmitted data |
| crc %= 4096; |
| crc1 = '=' + crc / 64; |
| crc2 = '=' + crc % 64; |
| // compare checksum to transmitted checksum bytes |
| if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) { |
| // checksum valid |
| rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
| ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
| rxd_buffer_locked = TRUE; // lock the rxd buffer |
| // if 2nd byte is an 'R' enable watchdog that will result in an reset |
| if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
| } else { // checksum invalid |
| rxd_buffer_locked = FALSE; // unlock rxd buffer |
| } |
| ptr_rxd_buffer = 0; // reset rxd buffer pointer |
| } |
| } else { // rxd buffer overrun |
| ptr_rxd_buffer = 0; // reset rxd buffer |
| rxd_buffer_locked = FALSE; // unlock rxd buffer |
| } |
| } |
| // -------------------------------------------------------------------------- |
| void AddCRC(uint16_t datalen) { |
| uint16_t tmpCRC = 0, i; |
| for(i = 0; i < datalen; i++) { |
| tmpCRC += txd_buffer[i]; |
| } |
| tmpCRC %= 4096; |
| txd_buffer[i++] = '=' + tmpCRC / 64; |
| txd_buffer[i++] = '=' + tmpCRC % 64; |
| txd_buffer[i++] = '\r'; |
| txd_complete = FALSE; |
| UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
| } |
| // -------------------------------------------------------------------------- |
| // application example: |
| // SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
| /* |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t txd_bufferIndex = 0; |
| uint8_t *currentBuffer; |
| uint8_t currentBufferIndex; |
| uint16_t lengthOfCurrentBuffer; |
| uint8_t shift = 0; |
| txd_buffer[txd_bufferIndex++] = '#'; // Start character |
| txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[txd_bufferIndex++] = cmd; // Command |
| va_start(ap, numofbuffers); |
| while(numofbuffers) { |
| currentBuffer = va_arg(ap, uint8_t*); |
| lengthOfCurrentBuffer = va_arg(ap, int); |
| currentBufferIndex = 0; |
| // Encode data: 3 bytes of data are encoded into 4 bytes, |
| // where the 2 most significant bits are both 0. |
| while(currentBufferIndex != lengthOfCurrentBuffer) { |
| if (!shift) txd_buffer[txd_bufferIndex] = 0; |
| txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
| txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
| shift += 2; |
| if (shift == 6) { shift=0; txd_bufferIndex++; } |
| currentBufferIndex++; |
| } |
| } |
| // If the number of data bytes was not divisible by 3, stuff |
| // with 0 pseudodata until length is again divisible by 3. |
| if (shift == 2) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex] &= 0b00110000; |
| txd_buffer[++txd_bufferIndex] = 0; |
| shift = 4; |
| } |
| if (shift == 4) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |
| */ |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t pt = 0; |
| uint8_t a,b,c; |
| uint8_t ptr = 0; |
| uint8_t *pdata = 0; |
| int len = 0; |
| txd_buffer[pt++] = '#'; // Start character |
| txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[pt++] = cmd; // Command |
| va_start(ap, numofbuffers); |
| if(numofbuffers) { |
| pdata = va_arg(ap, uint8_t*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| numofbuffers--; |
| } |
| while(len){ |
| if(len) { |
| a = pdata[ptr++]; |
| len--; |
| if((!len) && numofbuffers) { |
| pdata = va_arg(ap, uint8_t*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| numofbuffers--; |
| } |
| } |
| else a = 0; |
| if(len) { |
| b = pdata[ptr++]; |
| len--; |
| if((!len) && numofbuffers) { |
| pdata = va_arg(ap, uint8_t*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| numofbuffers--; |
| } |
| } else b = 0; |
| if(len) { |
| c = pdata[ptr++]; |
| len--; |
| if((!len) && numofbuffers) { |
| pdata = va_arg(ap, uint8_t*); |
| len = va_arg(ap, int); |
| ptr = 0; |
| numofbuffers--; |
| } |
| } |
| else c = 0; |
| txd_buffer[pt++] = '=' + (a >> 2); |
| txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| txd_buffer[pt++] = '=' + ( c & 0x3f); |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |
| // -------------------------------------------------------------------------- |
| void Decode64(void) { |
| uint8_t a,b,c,d; |
| uint8_t x,y,z; |
| uint8_t ptrIn = 3; |
| uint8_t ptrOut = 3; |
| uint8_t len = ReceivedBytes - 6; |
| while(len) { |
| a = rxd_buffer[ptrIn++] - '='; |
| b = rxd_buffer[ptrIn++] - '='; |
| c = rxd_buffer[ptrIn++] - '='; |
| d = rxd_buffer[ptrIn++] - '='; |
| //if(ptrIn > ReceivedBytes - 3) break; |
| x = (a << 2) | (b >> 4); |
| y = ((b & 0x0f) << 4) | (c >> 2); |
| z = ((c & 0x03) << 6) | d; |
| if(len--) rxd_buffer[ptrOut++] = x; else break; |
| if(len--) rxd_buffer[ptrOut++] = y; else break; |
| if(len--) rxd_buffer[ptrOut++] = z; else break; |
| } |
| pRxData = &rxd_buffer[3]; |
| RxDataLen = ptrOut - 3; |
| } |
| // -------------------------------------------------------------------------- |
| void usart0_ProcessRxData(void) { |
| // We control the motorTestActive var from here: Count it down. |
| if (motorTestActive) motorTestActive--; |
| // if data in the rxd buffer are not locked immediately return |
| if(!rxd_buffer_locked) return; |
| uint8_t tempchar1, tempchar2; |
| Decode64(); // decode data block in rxd_buffer |
| switch(rxd_buffer[1] - 'a') { |
| case FC_ADDRESS: |
| switch(rxd_buffer[2]) { |
| #ifdef USE_MK3MAG |
| case 'K':// compass value |
| compassHeading = ((Heading_t *)pRxData)->Heading; |
| compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| break; |
| #endif |
| case 't': // motor test |
| if(RxDataLen > 20) { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest)); |
| } else { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, 4); |
| } |
| motorTestActive = 255; |
| externalControlActive = 255; |
| break; |
| case 'n':// "Get Mixer Table |
| while(!txd_complete); // wait for previous frame to be sent |
| SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
| break; |
| case 'm':// "Set Mixer Table |
| if(pRxData[0] == EEMIXER_REVISION) { |
| memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
| MixerTable_WriteToEEProm(); |
| while(!txd_complete); // wait for previous frame to be sent |
| tempchar1 = 1; |
| } else { |
| tempchar1 = 0; |
| } |
| SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
| break; |
| case 'p': // get PPM channels |
| request_PPMChannels = TRUE; |
| break; |
| case 'q':// request settings |
| if(pRxData[0] == 0xFF) { |
| pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
| } |
| // limit settings range |
| if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1 |
| else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
| // load requested parameter set |
| ParamSet_ReadFromEEProm(pRxData[0]); |
| tempchar1 = pRxData[0]; |
| tempchar2 = EEPARAM_REVISION; |
| while(!txd_complete); // wait for previous frame to be sent |
| SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, sizeof(staticParams)); |
| break; |
| case 's': // save settings |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off |
| { |
| if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
| { |
| memcpy(&staticParams, (uint8_t*)&pRxData[2], sizeof(staticParams)); |
| ParamSet_WriteToEEProm(pRxData[0]); |
| /* |
| TODO: Remove this encapsulation breach |
| turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L; |
| turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L; |
| */ |
| tempchar1 = getActiveParamSet(); |
| beepNumber(tempchar1); |
| } |
| else |
| { |
| tempchar1 = 0; //indicate bad data |
| } |
| while(!txd_complete); // wait for previous frame to be sent |
| SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1)); |
| } |
| break; |
| default: |
| //unsupported command received |
| break; |
| } // case FC_ADDRESS: |
| default: // any Slave Address |
| switch(rxd_buffer[2]) { |
| case 'a':// request for labels of the analog debug outputs |
| request_DebugLabel = pRxData[0]; |
| if(request_DebugLabel > 31) request_DebugLabel = 31; |
| externalControlActive = 255; |
| break; |
| case 'b': // submit extern control |
| memcpy(&externalControl, (uint8_t*)pRxData, sizeof(externalControl)); |
| ConfirmFrame = externalControl.frame; |
| externalControlActive = 255; |
| break; |
| case 'h':// request for display columns |
| externalControlActive = 255; |
| RemoteKeys |= pRxData[0]; |
| if(RemoteKeys) DisplayLine = 0; |
| request_Display = TRUE; |
| break; |
| case 'l':// request for display columns |
| externalControlActive = 255; |
| MenuItem = pRxData[0]; |
| request_Display1 = TRUE; |
| break; |
| case 'v': // request for version and board release |
| request_VerInfo = TRUE; |
| break; |
| case 'x': |
| request_variables = TRUE; |
| break; |
| case 'g':// get external control data |
| request_ExternalControl = TRUE; |
| break; |
| case 'd': // request for the debug data |
| DebugData_Interval = (uint16_t) pRxData[0] * 10; |
| if(DebugData_Interval > 0) request_DebugData = TRUE; |
| break; |
| case 'c': // request for the 3D data |
| Data3D_Interval = (uint16_t) pRxData[0] * 10; |
| if(Data3D_Interval > 0) request_Data3D = TRUE; |
| break; |
| default: |
| //unsupported command received |
| break; |
| } |
| break; // default: |
| } |
| // unlock the rxd buffer after processing |
| pRxData = 0; |
| RxDataLen = 0; |
| rxd_buffer_locked = FALSE; |
| } |
| /************************************************************************/ |
| /* Routine für die Serielle Ausgabe */ |
| /************************************************************************/ |
| int16_t uart_putchar (int8_t c) { |
| if (c == '\n') |
| uart_putchar('\r'); |
| // wait until previous character was send |
| loop_until_bit_is_set(UCSR0A, UDRE0); |
| // send character |
| UDR0 = c; |
| return (0); |
| } |
| //--------------------------------------------------------------------------------------------- |
| void usart0_TransmitTxData(void) { |
| if(!txd_complete) return; |
| if(request_VerInfo && txd_complete) { |
| SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
| request_VerInfo = FALSE; |
| } |
| if(request_Display && txd_complete) { |
| LCD_PrintMenu(); |
| SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
| DisplayLine++; |
| if(DisplayLine >= 4) DisplayLine = 0; |
| request_Display = FALSE; |
| } |
| if(request_Display1 && txd_complete) { |
| LCD_PrintMenu(); |
| SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
| request_Display1 = FALSE; |
| } |
| if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
| uint8_t label[16]; // local sram buffer |
| memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
| SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
| request_DebugLabel = 0xFF; |
| } |
| if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
| SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
| ConfirmFrame = 0; |
| } |
| if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) { |
| SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
| DebugData_Timer = SetDelay(DebugData_Interval); |
| request_DebugData = FALSE; |
| } |
| if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) { |
| SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
| Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| Data3D_Timer = SetDelay(Data3D_Interval); |
| request_Data3D = FALSE; |
| } |
| if(request_ExternalControl && txd_complete) { |
| SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl)); |
| request_ExternalControl = FALSE; |
| } |
| #ifdef USE_MK3MAG |
| if((CheckDelay(Compass_Timer)) && txd_complete) { |
| ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
| ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
| ToMk3Mag.CalState = compassCalState; |
| SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
| // the last state is 5 and should be send only once to avoid multiple flash writing |
| if(compassCalState > 4) compassCalState = 0; |
| Compass_Timer = SetDelay(99); |
| } |
| #endif |
| if(request_MotorTest && txd_complete) { |
| SendOutData('T', FC_ADDRESS, 0); |
| request_MotorTest = FALSE; |
| } |
| if(request_PPMChannels && txd_complete) { |
| SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in)); |
| request_PPMChannels = FALSE; |
| } |
| if (request_variables && txd_complete) { |
| SendOutData('X', FC_ADDRESS, 1, (uint8_t *)&variables, sizeof(variables)); |
| request_variables = FALSE; |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/uart0.h |
|---|
| 0,0 → 1,47 |
| #ifndef _UART0_H |
| #define _UART0_H |
| #define RXD_BUFFER_LEN 150 |
| // must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
| #define TXD_BUFFER_LEN 150 |
| #define RXD_BUFFER_LEN 150 |
| #include <inttypes.h> |
| //Baud rate of the USART |
| #define USART0_BAUD 57600 |
| extern void usart0_Init (void); |
| extern void usart0_TransmitTxData(void); |
| extern void usart0_ProcessRxData(void); |
| extern int16_t uart_putchar(int8_t c); |
| extern uint8_t RemotePollDisplayLine; |
| extern uint8_t motorTestActive; |
| extern uint8_t motorTest[16]; |
| typedef struct { |
| uint8_t Digital[2]; |
| uint16_t Analog[32]; // Debugvalues |
| } __attribute__((packed)) DebugOut_t; |
| extern DebugOut_t DebugOut; |
| typedef struct { |
| int16_t AngleNick; // in 0.1 deg |
| int16_t AngleRoll; // in 0.1 deg |
| int16_t Heading; // in 0.1 deg |
| uint8_t reserve[8]; |
| } __attribute__((packed)) Data3D_t; |
| typedef struct { |
| uint8_t SWMajor; |
| uint8_t SWMinor; |
| uint8_t ProtoMajor; |
| uint8_t ProtoMinor; |
| uint8_t SWPatch; |
| uint8_t Reserved[5]; |
| } __attribute__((packed)) UART_VersionInfo_t; |
| #endif //_UART0_H |
| /branches/dongfang_FC_rewrite/backup/uart1.c |
|---|
| 0,0 → 1,163 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include "uart1.h" |
| #include "configuration.h" |
| #if defined (USE_MK3MAG) |
| #include "ubx.h" |
| #else |
| #ifdef USE_RC_DSL |
| #include "dsl.h" |
| #endif |
| #ifdef USE_RC_SPECTRUM |
| #include "spectrum.h" |
| #endif |
| #endif |
| #ifndef USART1_BAUD |
| #define USART1_BAUD 57600 |
| #endif |
| /****************************************************************/ |
| /* Initialization of the USART1 */ |
| /****************************************************************/ |
| void usart1_Init (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 = (uint8_t)(ubrr>>8); |
| UBRR1L = (uint8_t)ubrr; |
| // enable double speed operation |
| UCSR1A |= (1 << 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; |
| } |
| /****************************************************************/ |
| /* USART1 data register empty ISR */ |
| /****************************************************************/ |
| /*ISR(USART1_UDRE_vect) { |
| } |
| */ |
| /****************************************************************/ |
| /* USART1 transmitter ISR */ |
| /****************************************************************/ |
| /*ISR(USART1_TX_vect) { |
| } |
| */ |
| /****************************************************************/ |
| /* USART1 receiver ISR */ |
| /****************************************************************/ |
| ISR(USART1_RX_vect) { |
| uint8_t c; |
| c = UDR1; // get data byte |
| #if defined (USE_MK3MAG) |
| ubx_parser(c); // and put it into the ubx protocol parser |
| #else |
| #ifdef USE_RC_DSL |
| dsl_parser(c); // parse dsl data stream |
| #endif |
| #ifdef USE_RC_SPECTRUM |
| spectrum_parser(c); // parse spectrum data stream |
| #endif |
| #endif |
| } |
| /branches/dongfang_FC_rewrite/backup/uart1.h |
|---|
| 0,0 → 1,6 |
| #ifndef _UART1_H |
| #define _UART1_H |
| extern void usart1_Init (void); |
| #endif //_UART1_H |
| /branches/dongfang_FC_rewrite/backup/ubx.c |
|---|
| 0,0 → 1,288 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <inttypes.h> |
| #include "ubx.h" |
| #include <avr/io.h> |
| //#include "uart0.h" |
| // ubx protocol parser state machine |
| #define UBXSTATE_IDLE 0 |
| #define UBXSTATE_SYNC1 1 |
| #define UBXSTATE_SYNC2 2 |
| #define UBXSTATE_CLASS 3 |
| #define UBXSTATE_LEN1 4 |
| #define UBXSTATE_LEN2 5 |
| #define UBXSTATE_DATA 6 |
| #define UBXSTATE_CKA 7 |
| #define UBXSTATE_CKB 8 |
| // ublox protocoll identifier |
| #define UBX_CLASS_NAV 0x01 |
| #define UBX_ID_POSLLH 0x02 |
| #define UBX_ID_SOL 0x06 |
| #define UBX_ID_VELNED 0x12 |
| #define UBX_SYNC1_CHAR 0xB5 |
| #define UBX_SYNC2_CHAR 0x62 |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t Frac; // ns remainder of rounded ms above |
| int16_t week; // GPS week |
| uint8_t GPSfix; // GPSfix Type, range 0..6 |
| uint8_t Flags; // Navigation Status Flags |
| int32_t ECEF_X; // cm ECEF X coordinate |
| int32_t ECEF_Y; // cm ECEF Y coordinate |
| int32_t ECEF_Z; // cm ECEF Z coordinate |
| uint32_t PAcc; // cm 3D Position Accuracy Estimate |
| int32_t ECEFVX; // cm/s ECEF X velocity |
| int32_t ECEFVY; // cm/s ECEF Y velocity |
| int32_t ECEFVZ; // cm/s ECEF Z velocity |
| uint32_t SAcc; // cm/s Speed Accuracy Estimate |
| uint16_t PDOP; // 0.01 Position DOP |
| uint8_t res1; // reserved |
| uint8_t numSV; // Number of SVs used in navigation solution |
| uint32_t res2; // reserved |
| Status_t Status; |
| } UBX_SOL_t; |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t LON; // 1e-07 deg Longitude |
| int32_t LAT; // 1e-07 deg Latitude |
| int32_t HEIGHT; // mm Height above Ellipsoid |
| int32_t HMSL; // mm Height above mean sea level |
| uint32_t Hacc; // mm Horizontal Accuracy Estimate |
| uint32_t Vacc; // mm Vertical Accuracy Estimate |
| Status_t Status; |
| } UBX_POSLLH_t; |
| typedef struct { |
| uint32_t ITOW; // ms GPS Millisecond Time of Week |
| int32_t VEL_N; // cm/s NED north velocity |
| int32_t VEL_E; // cm/s NED east velocity |
| int32_t VEL_D; // cm/s NED down velocity |
| uint32_t Speed; // cm/s Speed (3-D) |
| uint32_t GSpeed; // cm/s Ground Speed (2-D) |
| int32_t Heading; // 1e-05 deg Heading 2-D |
| uint32_t SAcc; // cm/s Speed Accuracy Estimate |
| uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
| Status_t Status; |
| } UBX_VELNED_t; |
| UBX_SOL_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
| UBX_POSLLH_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
| UBX_VELNED_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
| GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID}; |
| volatile uint8_t GPSTimeout = 0; |
| void UpdateGPSInfo (void) |
| { |
| if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
| { |
| RED_FLASH; |
| if(GPSInfo.status != NEWDATA) |
| { |
| GPSInfo.status = INVALID; |
| // NAV SOL |
| GPSInfo.flags = UbxSol.Flags; |
| GPSInfo.satfix = UbxSol.GPSfix; |
| GPSInfo.satnum = UbxSol.numSV; |
| GPSInfo.PAcc = UbxSol.PAcc; |
| GPSInfo.VAcc = UbxSol.SAcc; |
| // NAV POSLLH |
| GPSInfo.longitude = UbxPosLlh.LON; |
| GPSInfo.latitude = UbxPosLlh.LAT; |
| GPSInfo.altitude = UbxPosLlh.HEIGHT; |
| GPSInfo.veleast = UbxVelNed.VEL_E; |
| GPSInfo.velnorth = UbxVelNed.VEL_N; |
| GPSInfo.veltop = -UbxVelNed.VEL_D; |
| GPSInfo.velground = UbxVelNed.GSpeed; |
| GPSInfo.status = NEWDATA; |
| } |
| // set state to collect new data |
| UbxSol.Status = PROCESSED; // never update old data |
| UbxPosLlh.Status = PROCESSED; // never update old data |
| UbxVelNed.Status = PROCESSED; // never update old data |
| } |
| } |
| // this function should be called within the UART RX ISR |
| void ubx_parser(uint8_t c) |
| { |
| static uint8_t ubxstate = UBXSTATE_IDLE; |
| static uint8_t cka, ckb; |
| static uint16_t msglen; |
| static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
| switch(ubxstate) |
| { |
| case UBXSTATE_IDLE: // check 1st sync byte |
| if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1; |
| else ubxstate = UBXSTATE_IDLE; // out of synchronization |
| break; |
| case UBXSTATE_SYNC1: // check 2nd sync byte |
| if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2; |
| else ubxstate = UBXSTATE_IDLE; // out of synchronization |
| break; |
| case UBXSTATE_SYNC2: // check msg class to be NAV |
| if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS; |
| else ubxstate = UBXSTATE_IDLE; // unsupported message class |
| break; |
| case UBXSTATE_CLASS: // check message identifier |
| switch(c) |
| { |
| case UBX_ID_POSLLH: // geodetic position |
| ubxP = (int8_t *)&UbxPosLlh; // data start pointer |
| ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer |
| ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer |
| break; |
| case UBX_ID_SOL: // navigation solution |
| ubxP = (int8_t *)&UbxSol; // data start pointer |
| ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer |
| ubxSp = (int8_t *)&UbxSol.Status; // status pointer |
| break; |
| case UBX_ID_VELNED: // velocity vector in tangent plane |
| ubxP = (int8_t *)&UbxVelNed; // data start pointer |
| ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer |
| ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer |
| break; |
| default: // unsupported identifier |
| ubxstate = UBXSTATE_IDLE; |
| break; |
| } |
| if (ubxstate != UBXSTATE_IDLE) |
| { |
| ubxstate = UBXSTATE_LEN1; |
| cka = UBX_CLASS_NAV + c; |
| ckb = UBX_CLASS_NAV + cka; |
| } |
| break; |
| case UBXSTATE_LEN1: // 1st message length byte |
| msglen = c; |
| cka += c; |
| ckb += cka; |
| ubxstate = UBXSTATE_LEN2; |
| break; |
| case UBXSTATE_LEN2: // 2nd message length byte |
| msglen += ((uint16_t)c)<<8; |
| cka += c; |
| ckb += cka; |
| // if the old data are not processed so far then break parsing now |
| // to avoid writing new data in ISR during reading by another function |
| if ( *ubxSp == NEWDATA ) |
| { |
| UpdateGPSInfo(); //update GPS info respectively |
| ubxstate = UBXSTATE_IDLE; |
| } |
| else // data invalid or allready processd |
| { |
| *ubxSp = INVALID; |
| ubxstate = UBXSTATE_DATA; |
| } |
| break; |
| case UBXSTATE_DATA: |
| if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left |
| cka += c; |
| ckb += cka; |
| if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read |
| break; |
| case UBXSTATE_CKA: |
| if (c == cka) ubxstate = UBXSTATE_CKB; |
| else |
| { |
| *ubxSp = INVALID; |
| ubxstate = UBXSTATE_IDLE; |
| } |
| break; |
| case UBXSTATE_CKB: |
| if (c == ckb) |
| { |
| *ubxSp = NEWDATA; // new data are valid |
| UpdateGPSInfo(); //update GPS info respectively |
| GPSTimeout = 255; |
| } |
| else |
| { // if checksum not fit then set data invalid |
| *ubxSp = INVALID; |
| } |
| ubxstate = UBXSTATE_IDLE; // ready to parse new data |
| break; |
| default: // unknown ubx state |
| ubxstate = UBXSTATE_IDLE; |
| break; |
| } |
| } |
| /branches/dongfang_FC_rewrite/backup/ubx.h |
|---|
| 0,0 → 1,61 |
| #ifndef _UBX_H |
| #define _UBX_H |
| #include <inttypes.h> |
| typedef enum |
| { |
| INVALID, |
| NEWDATA, |
| PROCESSED |
| } Status_t; |
| // Satfix types for GPSData.satfix |
| #define SATFIX_NONE 0x00 |
| #define SATFIX_DEADRECKOING 0x01 |
| #define SATFIX_2D 0x02 |
| #define SATFIX_3D 0x03 |
| #define SATFIX_GPS_DEADRECKOING 0x04 |
| #define SATFIX_TIMEONLY 0x05 |
| // Flags for interpretation of the GPSData.flags |
| #define FLAG_GPSFIXOK 0x01 // (i.e. within DOP & ACC Masks) |
| #define FLAG_DIFFSOLN 0x02 // (is DGPS used) |
| #define FLAG_WKNSET 0x04 // (is Week Number valid) |
| #define FLAG_TOWSET 0x08 // (is Time of Week valid) |
| /* enable the UBX protocol at the gps receiver with the following messages enabled |
| 01-02 NAV - POSLLH |
| 01-06 Nav - SOL |
| 01-12 NAV - VELNED */ |
| typedef struct |
| { |
| uint8_t flags; // flags |
| uint8_t satnum; // number of satelites |
| uint8_t satfix; // type of satfix |
| int32_t longitude; // in 1e-07 deg |
| int32_t latitude; // in 1e-07 deg |
| int32_t altitude; // in mm |
| uint32_t PAcc; // in cm 3d position accuracy |
| int32_t velnorth; // in cm/s |
| int32_t veleast; // in cm/s |
| int32_t veltop; // in cm/s |
| uint32_t velground; // 2D ground speed in cm/s |
| uint32_t VAcc; // in cm/s 3d velocity accuracy |
| Status_t status; // status of data: invalid | valid |
| } GPS_INFO_t; |
| //here you will find the current gps info |
| extern GPS_INFO_t GPSInfo; // measured position (last gps record) |
| // this variable should be decremted by the application |
| extern volatile uint8_t GPSTimeout; // is reset to 255 if a new UBX msg was received |
| #define USART1_BAUD 57600 |
| // this function should be called within the UART RX ISR |
| extern void ubx_parser(uint8_t c); |
| #endif //_UBX_H |
| /branches/dongfang_FC_rewrite/backup/userparams.txt |
|---|
| 0,0 → 1,36 |
| Userparam 3: Projection throttle effect. |
| Userparam 4: Throttle stick D value. Recommended: 12-15. Good fun to fly with. |
| Userparam 5: Filter control. |
| bits 0-1: Gyro signal for the flight control PID (filter constants 1-4) |
| bits 2-3: Gyro signal for the attitude angle integration (ATT) (filter constants 1-4) |
| bits 4-5: GyroD (filter constants 1-4) |
| bits 6-7: Acc. (filter constants 1-4) |
| Userparam6: Motor smoothing. |
| 0: No filter |
| 1: 50% new 50% old |
| 2: As H&I |
| 3: Reverse H&I |
| Userparam7: Yaw I factor. Default 0 (may be nasty to fly!). Recommended: ca. 100. |
| Userparam8: Acc integral correction Z axis limit. Default 0 (!) |
| Other params: |
| GyroAccFactor = 0'th order integral drift correction - acceleration sensor part as 1/1000s. |
| Default 5 (ENC-03) |
| GyroAccTrim = -1'th order integral drift correction - |
| Offset corrections are divided by this before added to offsets. Default 2. |
| DriftComp = Max offset correction per iteration (=per 1/2 second). |
| This limits the value _before_ division by GyroAccTrim. Default 32 (ENC-03) |
| Rotary rate limiter flag ON = The gyro signals for flight control are extrapolated with |
| a sharp rise in "virtual" rotation rates when the gyros are near saturation. This |
| prevents (via the flight control) the copter from turning fast enough that the gyros |
| saturate. Saturation will have the effect that stick controls overwhelm the (now clipped) |
| gyro feedback in the flight control, and the copter flips VERY fast. Saturation is |
| normally undesirable but it provides for a very entertaining stunt! |
| The rotary rate limiter (beginner feature to make the copter _very_ irresponsive to |
| stick controls) was removed. |
| /branches/dongfang_FC_rewrite/backup/yaw-analysis.txt |
|---|
| 0,0 → 1,353 |
| // gyro readings |
| int16_t GyroNick, GyroRoll, GyroYaw; |
| int16_t TrimNick, TrimRoll; |
| int32_t IntegralGyroYaw = 0; |
| int32_t ReadingIntegralGyroYaw = 0; |
| // compass course |
| int16_t CompassHeading = -1; // negative angle indicates invalid data. |
| int16_t CompassCourse = -1; |
| int16_t CompassOffCourse = 0; |
| uint8_t CompassCalState = 0; |
| uint16_t BadCompassHeading = 500; |
| int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t YawGyroDrift; |
| uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
| int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
| int16_t GPSStickNick = 0, GPSStickRoll = 0; |
| // stick values derived by uart inputs |
| int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
| /************************************************************************/ |
| /* Neutral Readings */ |
| /************************************************************************/ |
| void SetNeutral(uint8_t AccAdjustment) |
| { |
| ... |
| AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
| GyroYaw = 0; |
| CompassCourse = CompassHeading; |
| // Inititialize YawGyroIntegral value with current compass heading |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| YawGyroDrift = 0; |
| // Something completely different: Here's why the turn-over's were vars. |
| TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
| TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
| } |
| /************************************************************************/ |
| /* Averaging Measurement Readings */ |
| /************************************************************************/ |
| void Mean(void) |
| { |
| GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
| // Yaw |
| // calculate yaw gyro integral (~ to rotation angle) |
| YawGyroHeading += GyroYaw; |
| ReadingIntegralGyroYaw += GyroYaw; |
| // Coupling fraction |
| if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| { |
| .... |
| YawGyroHeading += tmp14; |
| if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
| if(abs(GyroYaw > 64)) |
| { |
| if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
| } |
| } |
| // Yaw |
| // limit YawGyroHeading proportional to 0? to 360? |
| if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap |
| if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR; |
| IntegralGyroYaw = ReadingIntegralGyroYaw; |
| } |
| void SetCompassCalState(void) |
| { |
| static uint8_t stick = 1; |
| // if nick is centered or top set stick to zero |
| if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0; |
| // if nick is down trigger to next cal state |
| if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick) |
| { |
| stick = 1; |
| CompassCalState++; |
| if(CompassCalState < 5) Beep(CompassCalState); |
| else BeepTime = 1000; |
| } |
| } |
| /************************************************************************/ |
| /* MotorControl */ |
| /************************************************************************/ |
| void MotorControl(void) |
| { |
| int16_t h, tmp_int; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
| int16_t PDPartYaw; |
| static int32_t SetPointYaw = 0; |
| static uint16_t UpdateCompassCourse = 0; |
| Mean(); |
| GRN_ON; |
| if(RC_Quality > 140) |
| { |
| if(ModelIsFlying < 256) |
| { |
| StickYaw = 0; |
| if(ModelIsFlying == 250) |
| { |
| UpdateCompassCourse = 1; |
| ReadingIntegralGyroYaw = 0; |
| SetPointYaw = 0; |
| } |
| } |
| else MKFlags |= (MKFLAG_FLY); // set fly flag |
| ...... |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| // if roll stick is centered and nick stick is down |
| if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
| { |
| CompassCalState = 1; |
| BeepTime = 1000; |
| } |
| (R/C data): |
| // mapping of yaw |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| if (StickYaw > 2) StickYaw-= 2; |
| else if (StickYaw< -2) StickYaw += 2; |
| else StickYaw = 0; |
| } |
| if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
| { |
| StickYaw += ExternControl.Yaw; |
| // disable I part of gyro control feedback |
| if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0; |
| // MeasurementCounter is incremented in the isr of analog.c |
| if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
| { |
| if(ParamSet.DriftComp) |
| { |
| if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
| if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
| } |
| YawGyroDrift = 0; |
| // Yawing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(abs(StickYaw) > 15 ) // yaw stick is activated |
| { |
| BadCompassHeading = 1000; |
| if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) |
| { |
| UpdateCompassCourse = 1; |
| } |
| } |
| // exponential stick sensitivity in yawring rate |
| tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx? |
| tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
| SetPointYaw = tmp_int; |
| // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
| ReadingIntegralGyroYaw -= tmp_int; |
| // limit the effect |
| CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Compass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // compass code is used if Compass option is selected |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| int16_t w, v, r,correction, error; |
| if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
| { |
| SetCompassCalState(); |
| #ifdef USE_KILLAGREG |
| MM3_Calibrate(); |
| #endif |
| } |
| else |
| { |
| #ifdef USE_KILLAGREG |
| static uint8_t updCompass = 0; |
| if (!updCompass--) |
| { |
| updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| MM3_Heading(); |
| } |
| #endif |
| // get maximum attitude angle |
| w = abs(IntegralGyroNick / 512); |
| v = abs(IntegralGyroRoll / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
| if(abs(GyroYaw) > 128) // spinning fast |
| { |
| error = 0; |
| } |
| if(!BadCompassHeading && w < 25) |
| { |
| YawGyroDrift += error; |
| if(UpdateCompassCourse) |
| { |
| BeepTime = 200; |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
| UpdateCompassCourse = 0; |
| } |
| } |
| YawGyroHeading += (error * 8) / correction; |
| w = (w * FCParam.CompassYawEffect) / 32; |
| w = FCParam.CompassYawEffect - w; |
| if(w >= 0) |
| { |
| if(!BadCompassHeading) |
| { |
| v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
| // calc course deviation |
| r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * FCParam.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| ReadingIntegralGyroYaw += v; |
| } |
| else |
| { // wait a while |
| BadCompassHeading--; |
| } |
| } |
| else |
| { // ignore compass at extreme attitudes for a while |
| BadCompassHeading = 500; |
| } |
| } |
| } |
| #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
| PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| // limit YawMixFraction |
| if(GasMixFraction > MIN_YAWGAS) |
| { |
| CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2)); |
| } |
| else |
| { |
| CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| tmp_int = ParamSet.GasMax * CONTROL_SCALING; |
| CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
| --------------- |
| anal-lyse: |
| YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; |
| OK |
| SetPointYaw->setPointYaw: |
| - static |
| - Set to 0 at take off |
| - Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw) |
| OK |
| PDPartYaw: |
| - nonstatic nonglobal |
| - = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) |
| / |
| (256L / CONTROL_SCALING) |
| + (int32_t)(IntegralGyroYaw * GyroYawIFactor) |
| / |
| (2 * (44000 / CONTROL_SCALING)); |
| OK |
| GyroYaw: |
| - global |
| - = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro) |
| OK |
| YawGyroHeading->yawGyroHeading: |
| - GyroYaw is summed to it at each iteration |
| - It is wrapped around at <0 and >=360. |
| - It is used -- where???? To adjust CompassCourse... |
| OK |
| IntegralGyroYaw->yawAngle: Deviation of heading from desired??? |
| - GyroYaw is summed to it at each iteration |
| - SetPointYaw is subtracted from it (!) at each iteration. |
| - It is NOT wrapped, but just limited to +/- 50000 |
| - It is corrected / pulled in axis coupling and by the compass. |
| OK (Except that with the compass) |
| BadCompassHeading: Need to update the "CompassCourse". |
| CompassHeading: Opdateret fra mm3mag. |
| CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag. |
| UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR |
| /branches/dongfang_FC_rewrite/bugs.txt |
|---|
| 0,0 → 1,9 |
| tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
| Wer sieht hier Fehler? |
| 1) Nick- und Rollwinkeln werden mit Gierkonstant berechnet (1160 od. 1291) |
| 2) Pythagoras wird auf Winklen (statt Seitenlängen) verwendet. |
| /branches/dongfang_FC_rewrite/commands.c |
|---|
| 0,0 → 1,125 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include "commands.h" |
| #include "controlMixer.h" |
| #include "flight.h" |
| #include "eeprom.h" |
| #include "attitude.h" |
| void commands_handleCommands(void) { |
| /* |
| * Get the current command (start/stop motors, calibrate), if any. |
| */ |
| uint8_t command = controlMixer_getCommand(); |
| uint8_t repeated = controlMixer_isCommandRepeated(); |
| uint8_t argument = controlMixer_getArgument(); |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (command == COMMAND_GYROCAL && !repeated) { |
| // Run gyro calibration but do not repeat it. |
| GRN_OFF; |
| // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
| // isFlying = 0; |
| // check roll/pitch stick position |
| // if pitch stick is top or roll stick is left or right --> change parameter setting |
| // according to roll/pitch stick position |
| if (argument < 6) { |
| // Gyro calinbration, with or without selecting a new parameter-set. |
| if(argument > 0 && argument < 6) { |
| // A valid parameter-set (1..5) was chosen - use it. |
| setActiveParamSet(argument); |
| } |
| ParamSet_ReadFromEEProm(getActiveParamSet()); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| controlMixer_setNeutral(); |
| beepNumber(getActiveParamSet()); |
| } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
| // If right stick is centered and down |
| // TODO: Out of here! State machine instead. |
| compassCalState = 1; |
| beep(1000); |
| } |
| } |
| // save the ACC neutral setting to eeprom |
| else { |
| if(command == COMMAND_ACCCAL && !repeated) { |
| // Run gyro and acc. meter calibration but do not repeat it. |
| GRN_OFF; |
| analog_calibrateAcc(); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| controlMixer_setNeutral(); |
| beepNumber(getActiveParamSet()); |
| } |
| } |
| } // end !MOTOR_RUN condition. |
| if (command == COMMAND_START) { |
| isFlying = 1; // TODO: Really???? |
| // if (!controlMixer_isCommandRepeated()) { |
| // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors. |
| MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all??? |
| // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors. |
| // attitude_continueDynamicCalibration(); |
| // setPointYaw = 0; |
| // IPartPitch = 0; |
| // IPartRoll = 0; |
| // } |
| } else if (command == COMMAND_STOP) { |
| isFlying = 0; |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| } |
| } |
| /branches/dongfang_FC_rewrite/commands.h |
|---|
| 0,0 → 1,17 |
| #ifndef _COMMANDS_H |
| #define _COMMANDS_H |
| #include <inttypes.h> |
| void commands_handleCommands(void); |
| /* |
| * An enumeration over the start motors, stop motors, calibrate gyros |
| * and calibreate acc. meters commands. |
| */ |
| #define COMMAND_NONE 0 |
| #define COMMAND_START 6 |
| #define COMMAND_STOP 8 |
| #define COMMAND_GYROCAL 2 |
| #define COMMAND_ACCCAL 4 |
| #endif |
| /branches/dongfang_FC_rewrite/commands.txt |
|---|
| 0,0 → 1,8 |
| These are the control commands: |
| - startMotors |
| - stopMotors |
| - calibrateGyros |
| - calibrateAcc (+height?) |
| - calibrateCompass |
| - captureHeightSetpoint |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 50,21 → 50,24 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <util/delay.h> |
| #include <avr/eeprom.h> |
| #include <stddef.h> |
| #include "configuration.h" |
| #include "eeprom.h" |
| #include "timer0.h" |
| int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
| fc_param_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
| dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
| uint8_t CPUType = ATMEGA644; |
| uint8_t BoardRelease = 13; |
| /************************************************************************ |
| * Map the parameter to pot values |
| * Replacing this code by the code below saved almost 1 kbyte. |
| ************************************************************************/ |
| void configuration_applyVariablesToParams(void) { |
| uint8_t i; |
| #define SET_POT_MM(b,a,min,max) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;} |
| #define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;} |
| #define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}} |
| SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight); |
| SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100); |
| 103,6 → 106,85 |
| SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl); |
| } |
| const XLATION XLATIONS[] = { |
| {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)}, |
| {offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)}, |
| {offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)}, |
| {offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)}, |
| {offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)}, |
| {offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)}, |
| {offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)}, |
| {offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)}, |
| {offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)}, |
| {offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)}, |
| {offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)}, |
| {offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)}, |
| {offsetof(paramset_t, NaviGpsModeControl), |
| offsetof(dynamicParam_t, NaviGpsModeControl)}, |
| {offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)}, |
| {offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)}, |
| {offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)}, |
| {offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)}, |
| {offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)}, |
| {offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)}, |
| {offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)}, |
| {offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)} |
| }; |
| const MMXLATION MMXLATIONS[] = { |
| {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
| {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightD),0,150}, |
| {offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255}, |
| {offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255}, |
| {offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255}, |
| {offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255} |
| }; |
| uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
| uint8_t result; |
| if (src>=251) result = variables[src-251]; |
| else result = src; |
| if (result < min) result = min; |
| else if (result > max) result = max; |
| return result; |
| } |
| void configuration_applyVariablesToParams_dead(void) { |
| uint8_t i, src; |
| uint8_t* pointerToTgt; |
| for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) { |
| src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) { |
| src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max); |
| } |
| } |
| for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
| src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
| src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| } |
| uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values |
| uint8_t CPUType = ATMEGA644; |
| if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 5,53 → 5,50 |
| #include <avr/io.h> |
| typedef struct { |
| uint8_t HeightD; |
| uint8_t MaxHeight; |
| uint8_t HeightP; |
| uint8_t Height_ACC_Effect; |
| uint8_t CompassYawEffect; |
| uint8_t GyroD; |
| uint8_t GyroP; |
| uint8_t GyroI; |
| uint8_t StickYawP; |
| uint8_t IFactor; |
| uint8_t UserParams[8]; |
| /* |
| uint8_t UserParam2; |
| uint8_t UserParam3; |
| uint8_t UserParam4; |
| uint8_t UserParam5; |
| uint8_t UserParam6; |
| uint8_t UserParam7; |
| uint8_t UserParam8; |
| */ |
| uint8_t ServoPitchControl; |
| uint8_t LoopGasLimit; |
| uint8_t AxisCoupling1; |
| uint8_t AxisCoupling2; |
| uint8_t AxisCouplingYawCorrection; |
| uint8_t DynamicStability; |
| uint8_t ExternalControl; |
| uint8_t J16Timing; |
| uint8_t J17Timing; |
| #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
| uint8_t NaviGpsModeControl; |
| uint8_t NaviGpsGain; |
| uint8_t NaviGpsP; |
| uint8_t NaviGpsI; |
| uint8_t NaviGpsD; |
| uint8_t NaviGpsACC; |
| uint8_t NaviOperatingRadius; |
| uint8_t NaviWindCorrection; |
| uint8_t NaviSpeedCompensation; |
| #endif |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| } fc_param_t; |
| /*PMM*/ uint8_t HeightD; |
| /* P */ uint8_t MaxHeight; |
| /*PMM*/ uint8_t HeightP; |
| /* P */ uint8_t Height_ACC_Effect; |
| /* P */ uint8_t CompassYawEffect; |
| /* P */ uint8_t GyroD; |
| /*PMM*/ uint8_t GyroP; |
| /* P */ uint8_t GyroI; |
| /* Never used */ uint8_t StickYawP; |
| /* P */ uint8_t IFactor; |
| /* P */ uint8_t UserParams[8]; |
| /* P */ uint8_t ServoPitchControl; |
| /* P */ uint8_t LoopGasLimit; |
| /* P */ uint8_t AxisCoupling1; |
| /* P */ uint8_t AxisCoupling2; |
| /* P */ uint8_t AxisCouplingYawCorrection; |
| /* P */ uint8_t DynamicStability; |
| /* P */ uint8_t ExternalControl; |
| /*PMM*/ uint8_t J16Timing; |
| /*PMM*/ uint8_t J17Timing; |
| /* P */ uint8_t NaviGpsModeControl; |
| /* P */ uint8_t NaviGpsGain; |
| /* P */ uint8_t NaviGpsP; |
| /* P */ uint8_t NaviGpsI; |
| /* P */ uint8_t NaviGpsD; |
| /* P */ uint8_t NaviGpsACC; |
| /*PMM*/ uint8_t NaviOperatingRadius; |
| /* P */ uint8_t NaviWindCorrection; |
| /* P */ uint8_t NaviSpeedCompensation; |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| } dynamicParam_t; |
| extern dynamicParam_t dynamicParams; |
| extern fc_param_t dynamicParams; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| uint8_t min, max; |
| } MMXLATION; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| } XLATION; |
| // values above 250 representing poti1 to poti4 |
| typedef struct { |
| uint8_t ChannelAssignment[8]; // see upper defines for details |
| 164,7 → 161,7 |
| #define CFG_AXIS_COUPLING_ACTIVE (1<<6) |
| #define CFG_ROTARY_RATE_LIMITER (1<<7) |
| // bit mask for ParamSet.BitConfig |
| // bit mask for staticParams.BitConfig |
| #define CFG_LOOP_UP (1<<0) |
| #define CFG_LOOP_DOWN (1<<1) |
| #define CFG_LOOP_LEFT (1<<2) |
| /branches/dongfang_FC_rewrite/control.h |
|---|
| 73,3 → 73,4 |
| #endif //_CONTROL_H |
| */ |
| /branches/dongfang_FC_rewrite/controlMixer.c |
|---|
| 52,31 → 52,35 |
| #include <stdlib.h> |
| #include "controlMixer.h" |
| #include "rc.h" |
| #include "heightControl.h" |
| #include "attitudeControl.h" |
| #include "externalControl.h" |
| #include "configuration.h" |
| #include "attitude.h" |
| #include "eeprom.h" |
| #include "flight.h" |
| #include "commands.h" |
| #include "output.h" |
| /* |
| * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC. |
| */ |
| #define COMMAND_TIMER 200 |
| uint16_t maxControl[2] = {0,0}; |
| int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0; |
| int16_t control[2] = {0,0}; |
| int16_t controlYaw = 0, controlThrottle = 0; |
| uint8_t looping = 0; |
| // Internal variables for reading commands made with an R/C stick. |
| uint8_t lastCommand = COMMAND_NONE; |
| uint8_t commandTimer = 0; |
| uint8_t lastArgument; |
| uint8_t isCommandRepeated = 0; |
| // MK flags. TODO: Replace by enum. State machine. |
| uint16_t isFlying = 0; |
| volatile uint8_t MKFlags = 0; |
| /* |
| * This could be expanded to take arguments from ohter sources than the RC |
| * (read: Custom MK RC project) |
| */ |
| uint8_t controlMixer_getArgument(void) { |
| return RC_getArgument(); |
| return lastArgument; |
| } |
| /* |
| 84,21 → 88,16 |
| * than the R/C (read: Custom MK R/C project) |
| */ |
| uint8_t controlMixer_getCommand(void) { |
| // If the same command was made COMMAND_TIMER times, it's stable. |
| if (commandTimer >= COMMAND_TIMER) { |
| return lastCommand; |
| } |
| return COMMAND_NONE; |
| return lastCommand; |
| } |
| uint8_t controlMixer_isCommandRepeated(void) { |
| return commandTimer > COMMAND_TIMER ? 1 : 0; |
| return isCommandRepeated; |
| } |
| void controlMixer_setNeutral(uint8_t calibrate) { |
| if (calibrate) |
| RC_calibrate(); |
| EC_setNeutral(); |
| void controlMixer_setNeutral() { |
| EC_setNeutral(); |
| HC_setGround(); |
| } |
| /* |
| 114,6 → 113,7 |
| /* |
| * Update potentiometer values with limited slew rate. Could be made faster if desired. |
| * TODO: It assumes R/C as source. Not necessarily true. |
| */ |
| void controlMixer_updateVariables(void) { |
| uint8_t i; |
| 139,18 → 139,30 |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| // TODO: If no signal --> zero. |
| uint8_t axis; |
| // takes almost no time... |
| RC_update(); |
| // takes almost no time... |
| EC_update(); |
| // takes about 80 usec. |
| HC_update(); |
| int16_t* RC_PRTY = RC_getPRTY(); |
| int16_t* EC_PRTY = EC_getPRTY(); |
| control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
| control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
| controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]; |
| // This can be a CPU time killer if the function implementations are inefficient. |
| controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE])); |
| controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
| if (RC_getSignalQuality() >= SIGNAL_GOOD) { |
| DebugOut.Analog[12] = control[PITCH]; |
| DebugOut.Analog[13] = control[ROLL]; |
| //DebugOut.Analog[26] = controlYaw; |
| if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| controlMixer_updateVariables(); |
| configuration_applyVariablesToParams(); |
| looping = RC_getLooping(looping); |
| 160,15 → 172,15 |
| looping = 0; |
| } |
| DebugOut.Analog[18] = (int16_t)looping; |
| // part1a end. |
| // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
| // TODO: Correct, for changed range (stick gain + expo is now applied already) |
| /* This is not really necessary with the pull-towards-zero of all sticks (see rc.c) |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| if (controlYaw > 2) controlYaw-= 2; |
| else if (controlYaw< -2) controlYaw += 2; |
| else controlYaw = 0; |
| } |
| */ |
| /* |
| * Record maxima |
| 180,27 → 192,31 |
| } else if (maxControl[axis]) maxControl[axis]--; |
| } |
| // Here we could blend in other sources. |
| uint8_t commandNow = RC_getCommand(); |
| uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
| uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
| if (rcCommand != COMMAND_NONE) { |
| isCommandRepeated = (lastCommand == rcCommand); |
| lastCommand = rcCommand; |
| lastArgument = RC_getArgument(); |
| } else if (ecCommand != COMMAND_NONE) { |
| isCommandRepeated = (lastCommand == ecCommand); |
| lastCommand = ecCommand; |
| lastArgument = EC_getArgument(); |
| } else { |
| // Both sources have no command, or one or both are out. |
| // Just set to false. There is no reason to check if the none-command was repeated anyway. |
| isCommandRepeated = 0; |
| lastCommand = COMMAND_NONE; |
| } |
| if (commandNow != lastCommand) { |
| lastCommand = commandNow; |
| commandTimer = 0; |
| } else if (commandTimer < COMMAND_TIMER + 1) |
| commandTimer++; |
| if (isCommandRepeated) DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED; |
| if (rcCommand) DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED; |
| // part1 end. |
| } |
| /* |
| void setCompassCalState(void) { |
| static uint8_t stick = 1; |
| // if pitch is centered or top set stick to zero |
| if(RCChannel(CH_PITCH) > -20) stick = 0; |
| // if pitch is down trigger to next cal state |
| if((RCChannel(CH_PITCH) < -70) && !stick) { |
| stick = 1; |
| compassCalState++; |
| if(compassCalState < 5) beepNumber(compassCalState); |
| else beep(1000); |
| } |
| // TODO: Integrate into command system. |
| uint8_t controlMixer_testCompassCalState(void) { |
| return RC_testCompassCalState(); |
| } |
| */ |
| /branches/dongfang_FC_rewrite/controlMixer.h |
|---|
| 1,22 → 1,10 |
| #include <inttypes.h> |
| /* |
| * The throttle range is normalized to [0, THROTTLE_RANGE[ |
| * This is the normal range. Exceeding it a little is allowed. |
| */ |
| #define THROTTLE_RANGE 256 |
| /* |
| * The stick (pitch, roll, yaw) ranges are normalized to [-STICK_RANGE, STICK_RANGE] |
| * This is the normal range. Exceeding it a little is allowed. |
| */ |
| #define STICK_RANGE 256 |
| /* |
| * An attempt to define a generic control. That could be an R/C receiver, an external control |
| * (serial over Bluetooth, Wi232, XBee, whatever) or the NaviCtrl. |
| * This resembles somewhat an object-oriented class definition (except that there are no virtuals). |
| * The idea is that the combination of different control inputs, of the way they superimpose upon |
| * each other, the proirities between them and the behavior in case that one fails is simplified, |
| * each other, the priorities between them and the behavior in case that one fails is simplified, |
| * and all in one place. |
| */ |
| 35,22 → 23,12 |
| #define SIGNAL_GOOD 4 |
| /* |
| * An enumeration over the start motors, stop motors, calibrate gyros |
| * and calibreate acc. meters commands. |
| */ |
| #define COMMAND_NONE 0 |
| #define COMMAND_START 6 |
| #define COMMAND_STOP 8 |
| #define COMMAND_GYROCAL 2 |
| #define COMMAND_ACCCAL 4 |
| /* |
| * The PRTY arrays |
| */ |
| #define CONTROL_PITCH 0 |
| #define CONTROL_ROLL 1 |
| #define CONTROL_PITCH 0 |
| #define CONTROL_ROLL 1 |
| #define CONTROL_THROTTLE 2 |
| #define CONTROL_YAW 3 |
| #define CONTROL_YAW 3 |
| /* |
| * Looping flags. |
| 93,15 → 71,18 |
| /* |
| * Our output. |
| */ |
| extern int16_t control[2], controlYaw, controlThrottle; |
| // extern int16_t stickOffsetNick, stickOffsetRoll; |
| extern int16_t control[2]; |
| extern int16_t controlYaw, controlThrottle; |
| extern uint16_t maxControl[2]; |
| extern uint8_t looping; |
| extern volatile uint8_t MKFlags; |
| extern uint16_t isFlying; |
| void controlMixer_initVariables(void); |
| void controlMixer_updateVariables(void); |
| void controlMixer_setNeutral(uint8_t calibrate); |
| void controlMixer_setNeutral(void); |
| /* |
| * Update the exported variables. Called at every flight control cycle. |
| 118,11 → 99,11 |
| uint8_t controlMixer_getSignalQuality(void); |
| /* |
| * The motor throttles can be set in an [0..256[ interval. |
| * The calculation of motor throttle values from sensor and control information (basically |
| * flight.c) take place in an [0..256*CONTROL_SCALING[ interval for better precision. |
| * The controls operate in a [-150 * CONTROL_SCALING, 150 * CONTROL_SCALING] interval |
| * Throttle is [0..300 * CONTROL_SCALING]. |
| * (just about. No precision needed). |
| */ |
| #define CONTROL_SCALING 4 |
| #define CONTROL_SCALING (1024/256) |
| /* |
| * Gets the argument for the current command (a number). |
| 139,8 → 120,8 |
| * |
| * Not in any of these positions: 0 |
| */ |
| // void controlMixer_handleCommands(void); |
| uint8_t controlMixer_getArgument(void); |
| uint8_t controlMixer_isCommandRepeated(void); |
| // TODO: Abstract away if possible. |
| // void controlMixer_setCompassCalState(void); |
| // void controlMixer_updateCompass(void); |
| uint8_t controlMixer_testCompassCalState(void); |
| /branches/dongfang_FC_rewrite/displays.txt |
|---|
| 0,0 → 1,10 |
| Timerstyret eller Anforderung0 : 'H' format. Timerkonstant = interval fra request * 10 |
| H format response: H, address, buffer(80) |
| h format request: keys(invers!), interval |
| keys: key1(2): baglaens, key2(4): forlaens, begge: #0, ingen: Bliv paa samme side. |
| andre keys: key4(8): reset(???) flyvetimer. |
| Kun h request setter keys. |
| DisplayAnfordering1: 'L' format. |
| L format response: L, address, menupunkt, maxmenu, buffer(80) |
| l format request: l, address, menupunkt |
| /branches/dongfang_FC_rewrite/dongfangMath.h |
|---|
| 11,7 → 11,10 |
| * returns UNIT_FACTOR * 0.8, the result is to be understood as 0.8) |
| * a * sin(b) = (a * int_sin(b * DRG_FACTOR)) / UNIT_FACTOR |
| */ |
| #define MATH_UNIT_FACTOR 8192 |
| //#define MATH_UNIT_FACTOR 8192 |
| // Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
| // 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
| #define MATH_UNIT_FACTOR 4096 |
| int16_t int_sin(int32_t arg); |
| int16_t int_cos(int32_t arg); |
| /branches/dongfang_FC_rewrite/eeprom.c |
|---|
| 95,11 → 95,11 |
| * While we are still using userparams for flight parameters, do set |
| * some safe & meaningful default values. |
| */ |
| staticParams.UserParams1[3] = 10; // Throttle stick D=10 |
| staticParams.UserParams1[3] = 8; // Throttle stick D=8 |
| staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4 |
| staticParams.UserParams2[1] = 2; // H&I motor smoothing. |
| staticParams.UserParams2[2] = 120; // Yaw I factor |
| staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles. |
| staticParams.UserParams2[3] = 10; // Max Z acceleration for acc. correction of angles. |
| } |
| void setOtherDefaults(void) { |
| 114,7 → 114,7 |
| staticParams.ChannelAssignment[CH_POTS+1] = 6; |
| staticParams.ChannelAssignment[CH_POTS+2] = 7; |
| staticParams.ChannelAssignment[CH_POTS+3] = 8; |
| staticParams.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
| staticParams.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_HEADING_HOLD; // CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
| staticParams.HeightMinGas = 30; |
| staticParams.MaxHeight = 251; |
| staticParams.HeightP = 10; |
| 121,8 → 121,8 |
| staticParams.HeightD = 30; |
| staticParams.Height_ACC_Effect = 30; |
| staticParams.Height_Gain = 4; |
| staticParams.StickP = 12; |
| staticParams.StickD = 16; |
| staticParams.StickP = 8; |
| staticParams.StickD = 12; |
| staticParams.StickYawP = 12; |
| staticParams.MinThrottle = 8; |
| staticParams.MaxThrottle = 230; |
| 129,7 → 129,7 |
| staticParams.CompassYawEffect = 128; |
| staticParams.GyroP = 80; |
| staticParams.GyroI = 100; |
| staticParams.LowVoltageWarning = 94; |
| staticParams.LowVoltageWarning = 95; |
| staticParams.EmergencyGas = 35; |
| staticParams.EmergencyGasDuration = 30; |
| staticParams.Unused0 = 0; |
| 177,6 → 177,7 |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| memcpy(staticParams.Name, "Sport\0",6); |
| } |
| 187,6 → 188,7 |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| staticParams.Height_Gain = 3; |
| staticParams.J16Timing = 20; |
| staticParams.J17Timing = 20; |
| 200,7 → 202,7 |
| setOtherDefaults(); |
| gyro_setDefaults(); |
| setDefaultUserParams(); |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
| staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
| staticParams.Height_Gain = 3; |
| staticParams.EmergencyGasDuration = 20; |
| staticParams.AxisCouplingYawCorrection = 70; |
| 262,7 → 264,6 |
| output_init(); |
| } |
| /***************************************************/ |
| /* Get active parameter set */ |
| /***************************************************/ |
| 332,7 → 333,7 |
| /* Initialize EEPROM Parameter Sets */ |
| /***************************************************/ |
| void ParamSet_Init(void) { |
| uint8_t Channel_Backup = 0, i; |
| uint8_t Channel_Backup=1, i, j; |
| // parameter version check |
| if(eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) { |
| // if version check faild |
| 339,14 → 340,10 |
| printf("\n\rInit Parameter in EEPROM"); |
| eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table |
| // check if channel mapping backup is valid |
| if( (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) < 12) |
| && (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12) |
| && (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12) |
| && (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12) |
| ) |
| { |
| Channel_Backup = 1; |
| } |
| for (j=0; j<4 && Channel_Backup; j++) { |
| if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) >= 12) |
| Channel_Backup = 0; |
| } |
| // fill all 5 parameter settings |
| for (i=1; i<6; i++) { |
| switch(i) { |
| 365,14 → 362,9 |
| } |
| if(Channel_Backup) { // if we have a rc channel mapping backup in eeprom |
| // restore it |
| staticParams.ChannelAssignment[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]); |
| staticParams.ChannelAssignment[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]); |
| staticParams.ChannelAssignment[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]); |
| staticParams.ChannelAssignment[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]); |
| staticParams.ChannelAssignment[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]); |
| staticParams.ChannelAssignment[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]); |
| staticParams.ChannelAssignment[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]); |
| staticParams.ChannelAssignment[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]); |
| for (j=0; j<8; j++) { |
| staticParams.ChannelAssignment[j] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+j]); |
| } |
| } |
| ParamSet_WriteToEEProm(i); |
| } |
| /branches/dongfang_FC_rewrite/externalControl.c |
|---|
| 24,14 → 24,12 |
| return EC_PRTY; |
| } |
| // not implemented. |
| uint8_t EC_getArgument(void) { |
| return 0; |
| return externalControl.config; |
| } |
| // not implemented. |
| uint8_t EC_getCommand(void) { |
| return COMMAND_NONE; |
| return externalControl.free; |
| } |
| // not implemented. |
| 52,7 → 50,7 |
| } |
| uint8_t EC_getSignalQuality(void) { |
| if (externalControlActive > 100) |
| if (externalControlActive > 80) |
| // Configured and heard from recently |
| return SIGNAL_GOOD; |
| /branches/dongfang_FC_rewrite/externalControl.h |
|---|
| 6,15 → 6,15 |
| typedef struct { |
| uint8_t digital[2]; |
| uint8_t eemoteButtons; |
| uint8_t remoteButtons; |
| int8_t pitch; |
| int8_t roll; |
| int8_t yaw; |
| uint8_t throttle; |
| int8_t height; |
| uint8_t free; |
| uint8_t free; // Let's use that for commands now. |
| uint8_t frame; |
| uint8_t config; |
| uint8_t config; // Let's use that for arguemnts. |
| } __attribute__((packed)) ExternalControl_t; |
| extern ExternalControl_t externalControl; |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 61,9 → 61,14 |
| // Necessary for external control and motor test |
| #include "uart0.h" |
| // for scope debugging |
| // #include "rc.h" |
| #include "twimaster.h" |
| #include "attitude.h" |
| #include "controlMixer.h" |
| #include "commands.h" |
| #ifdef USE_MK3MAG |
| #include "gps.h" |
| #endif |
| 76,10 → 81,6 |
| */ |
| // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
| // MK flags |
| uint16_t isFlying = 0; |
| volatile uint8_t MKFlags = 0; |
| uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
| uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
| 117,9 → 118,6 |
| /* Neutral Readings */ |
| /************************************************************************/ |
| void flight_setNeutral() { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| MKFlags |= MKFLAG_CALIBRATE; |
| // not really used here any more. |
| 130,32 → 128,6 |
| controlMixer_initVariables(); |
| } |
| /************************************************************************/ |
| /* Transmit Motor Data via I2C */ |
| /************************************************************************/ |
| void sendMotorData(void) { |
| uint8_t i; |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
| // If pilot has not started the engines.... |
| MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off |
| for(i = 0; i < MAX_MOTORS; i++) { |
| // and if we are not in motor test mode, cut throttle on all motors. |
| if(!motorTestActive) Motor[i].SetPoint = 0; |
| else Motor[i].SetPoint = motorTest[i]; |
| } |
| if(motorTestActive) motorTestActive--; |
| } |
| /* |
| DebugOut.Analog[] = Motor[0].SetPoint; // Front |
| DebugOut.Analog[] = Motor[1].SetPoint; // Rear |
| DebugOut.Analog[] = Motor[3].SetPoint; // Left |
| DebugOut.Analog[] = Motor[2].SetPoint; // Right |
| */ |
| // Start I2C Interrupt Mode |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| } |
| void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
| Ki = 10300 / _Ki; |
| gyroPFactor = _gyroPFactor; |
| 177,67 → 149,6 |
| setFlightParameters(33, 90, 120, 90, 120); |
| } |
| void handleCommands(uint8_t command, uint8_t argument, uint8_t isCommandRepeated) { |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (command == COMMAND_GYROCAL && !isCommandRepeated) { |
| // Run gyro calibration but do not repeat it. |
| GRN_OFF; |
| // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
| // isFlying = 0; |
| // check roll/pitch stick position |
| // if pitch stick is top or roll stick is left or right --> change parameter setting |
| // according to roll/pitch stick position |
| if (argument < 6) { |
| // Gyro calinbration, with or without selecting a new parameter-set. |
| if(argument > 0 && argument < 6) { |
| // A valid parameter-set (1..5) was chosen - use it. |
| setActiveParamSet(argument); |
| } |
| ParamSet_ReadFromEEProm(getActiveParamSet()); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| // Right stick is centered; calibrate it to zero (hmm strictly does not belong here). |
| // If heading hold is active, do not do it. TODO: We also want to re-set old calibration. |
| controlMixer_setNeutral(!argument); |
| beepNumber(getActiveParamSet()); |
| } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
| // If right stick is centered and down |
| compassCalState = 1; |
| beep(1000); |
| } |
| } |
| // save the ACC neutral setting to eeprom |
| else { |
| if(command == COMMAND_ACCCAL && !isCommandRepeated) { |
| // Run gyro and acc. meter calibration but do not repeat it. |
| GRN_OFF; |
| analog_calibrateAcc(); |
| attitude_setNeutral(); |
| flight_setNeutral(); |
| controlMixer_setNeutral(1); // Calibrate right stick neutral position. |
| beepNumber(getActiveParamSet()); |
| } |
| } |
| } // end !MOTOR_RUN condition. |
| if (command == COMMAND_START) { |
| isFlying = 1; // TODO: Really???? |
| // if (!controlMixer_isCommandRepeated()) { |
| // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors. |
| MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all??? |
| // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors. |
| // attitude_continueDynamicCalibration(); |
| // setPointYaw = 0; |
| // IPartPitch = 0; |
| // IPartRoll = 0; |
| // } |
| } else if (command == COMMAND_STOP) { |
| isFlying = 0; |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| } |
| } |
| /************************************************************************/ |
| /* Main Flight Control */ |
| 250,7 → 161,7 |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw, PPart[2]; |
| static int32_t IPart[2] = {0,0}; |
| static int32_t setPointYaw = 0; |
| // static int32_t yawControlRate = 0; |
| // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
| // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
| 265,23 → 176,32 |
| uint8_t i, axis; |
| // Fire the main flight attitude calculation, including integration of angles. |
| calculateFlightAttitude(); |
| GRN_ON; |
| /* |
| * TODO: update should: Set the stick variables if good signal, set them to zero if bad. |
| * Set variables also. |
| */ |
| // start part 1: 750-800 usec. |
| // start part 1a: 750-800 usec. |
| // start part1b: 700 usec |
| // start part1c: 700 usec!!!!!!!!! WAY too slow. |
| controlMixer_update(); |
| // end part1c |
| throttleTerm = controlThrottle; |
| if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| // if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| // else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
| // end part1b: 700 usec. |
| /************************************************************************/ |
| /* RC-signal is bad */ |
| /* This part could be abstracted, as having yet another control input */ |
| /* to the control mixer: An emergency autopilot control. */ |
| /************************************************************************/ |
| if(controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| RED_ON; |
| beepRCAlarm(); |
| 289,7 → 209,7 |
| if(emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| if(isFlying > 1000) { |
| if(isFlying > 256) { |
| // We're probably still flying. Descend slowly. |
| throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
| MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
| 307,7 → 227,7 |
| // Reset emergency landing control variables. |
| MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
| // The time is in whole seconds. |
| emergencyFlightTime = staticParams.EmergencyGasDuration * 488; |
| emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488; |
| } |
| // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
| 321,35 → 241,28 |
| * or flip when taking off. |
| */ |
| if(isFlying < 256) { |
| IPart[PITCH] = IPart[ROLL] = 0; |
| // TODO: Don't stomp on other modules' variables!!! |
| controlYaw = 0; |
| if(isFlying == 250) { |
| updateCompassCourse = 1; |
| yawAngle = 0; |
| setPointYaw = 0; |
| IPart[PITCH] = IPart[ROLL] = 0; |
| // TODO: Don't stomp on other modules' variables!!! |
| // controlYaw = 0; |
| PDPartYaw = 0; // instead. |
| if(isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| yawAngleDiff = 0; |
| } |
| } else { |
| // DebugOut.Digital[1] = 0; |
| // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
| // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
| MKFlags |= (MKFLAG_FLY); |
| } |
| /* |
| * Get the current command (start/stop motors, calibrate), if any. |
| */ |
| uint8_t command = controlMixer_getCommand(); |
| uint8_t repeated = controlMixer_isCommandRepeated(); |
| uint8_t argument = controlMixer_getArgument(); |
| handleCommands(command, argument, repeated); |
| commands_handleCommands(); |
| // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| setNormalFlightParameters(); |
| // } |
| } // end else (not bad signal case) |
| // end part1a: 750-800 usec. |
| /* |
| * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
| * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
| 369,49 → 282,47 |
| } |
| } |
| setPointYaw = controlYaw; |
| // yawControlRate = controlYaw; |
| // Trim drift of yawAngle with controlYaw. |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| yawAngle -= controlYaw; |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| // limit the effect |
| CHECK_MIN_MAX(yawAngle, -50000, 50000) |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| /* |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| */ |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| #if defined (USE_MK3MAG) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| /* |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } |
| else { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| } |
| */ |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } |
| else { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| } |
| #endif |
| // end part 1: 750-800 usec. |
| // start part 3: 350 - 400 usec. |
| #define SENSOR_LIMIT (4096 * 4) |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| if(looping & (1<<(4+axis))) { |
| if(looping & ((1<<4)<<axis)) { |
| PPart[axis] = 0; |
| } else { // TODO: Where do the 44000 come from??? |
| PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| 427,12 → 338,13 |
| CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| } |
| PDPartYaw = |
| (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
| + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
| + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| // limit control feedback |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| /* |
| * Compose throttle term. |
| 440,15 → 352,12 |
| */ |
| if(missingMotor) { |
| // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0)) |
| if(isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
| isFlying = 1; // keep within lift off condition |
| throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
| } |
| /* |
| * Height control was here. |
| */ |
| if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
| // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
| throttleTerm *= CONTROL_SCALING; |
| /* |
| 459,14 → 368,14 |
| * between current throttle and maximum throttle). |
| */ |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING; |
| // limit yawTerm |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| // Limit yawTerm |
| if(throttleTerm > MIN_YAWGAS) { |
| CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
| } else { |
| CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
| CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
| 486,12 → 395,12 |
| // To keep up with a full stick PDPart should be about 156... |
| IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| } |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
| 500,26 → 409,49 |
| */ |
| CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
| } |
| // end part 3: 350 - 400 usec. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Universal Mixer |
| // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[12] = term[PITCH]; |
| DebugOut.Analog[13] = term[ROLL]; |
| DebugOut.Analog[14] = yawTerm; |
| DebugOut.Analog[15] = throttleTerm; |
| for(i = 0; i < MAX_MOTORS; i++) { |
| int16_t tmp; |
| if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered. |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
| tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
| tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
| tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / CONTROL_SCALING; |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
| Motor[i].SetPoint = tmp; |
| CHECK_MIN_MAX(tmp, 1, 255); |
| motor[i].SetPoint = tmp; |
| } |
| else Motor[i].SetPoint = 0; |
| else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| } |
| if (i < 4) |
| DebugOut.Analog[22+i] = motor[i].SetPoint; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugging |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 529,31 → 461,13 |
| DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| // DebugOut.Analog[9] = setPointYaw; |
| // DebugOut.Analog[10] = yawIFactor; |
| // DebugOut.Analog[11] = gyroIFactor; |
| // DebugOut.Analog[12] = RC_getVariable(0); |
| // DebugOut.Analog[13] = dynamicParams.UserParams[0]; |
| // DebugOut.Analog[14] = RC_getVariable(4); |
| // DebugOut.Analog[15] = dynamicParams.UserParams[4]; |
| /* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */ |
| // 12..15 are the controls. |
| // DebugOut.Analog[16] = pitchAxisAcc; |
| // DebugOut.Analog[17] = rollAxisAcc; |
| DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR; |
| DebugOut.Analog[19] = throttleTerm; |
| DebugOut.Analog[20] = term[PITCH]; |
| DebugOut.Analog[21] = term[ROLL]; |
| DebugOut.Analog[22] = yawTerm; |
| DebugOut.Analog[23] = PPart[PITCH]; // |
| DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille. |
| DebugOut.Analog[25] = PDPart[PITCH]; // omtrent lig ppart. |
| /* |
| DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
| DebugOut.Analog[24] = controlYaw; |
| DebugOut.Analog[25] = yawAngleDiff / 100L; |
| DebugOut.Analog[26] = accNoisePeak[PITCH]; |
| DebugOut.Analog[27] = accNoisePeak[ROLL]; |
| */ |
| DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| /branches/dongfang_FC_rewrite/flight.h |
|---|
| 12,6 → 12,16 |
| #define PITCH 0 |
| #define ROLL 1 |
| /* |
| * The output BLC throttles can be set in an [0..256[ interval. Some staticParams limits |
| * (min, throttle, max throttle etc) are in a [0..256[ interval. |
| * The calculation of motor throttle values from sensor and control information (basically |
| * flight.c) take place in an [0..1024[ interval for better precision. |
| * This is the conversion factor. |
| * --> Replaced back again by CONTROL_SCALING. Even though the 2 things are not quite the |
| * same, they are unseperable anyway. |
| */ |
| extern uint8_t RequiredMotors; |
| // looping params |
| 18,14 → 28,16 |
| // extern long TurnOver180Nick, TurnOver180Roll; |
| // external control |
| extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
| // extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
| // extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
| extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
| // TODO: Rip em up, replace by a flight-state machine. |
| // OK first step: Move to control mixer, where the state machine will be located anyway. |
| // extern volatile uint8_t MKFlags; |
| // extern uint16_t isFlying; |
| extern volatile uint8_t MKFlags; |
| void flight_control(void); |
| void sendMotorData(void); |
| void transmitMotorThrottleData(void); |
| void flight_setNeutral(void); |
| #endif //_FLIGHT_H |
| /branches/dongfang_FC_rewrite/gps.c |
|---|
| 51,7 → 51,7 |
| #include <inttypes.h> |
| #include <stdlib.h> |
| #include "ubx.h" |
| #include "mymath.h" |
| //#include "mymath.h" |
| #include "timer0.h" |
| #include "uart0.h" |
| #include "rc.h" |
| 87,108 → 87,90 |
| // --------------------------------------------------------------------------------- |
| void GPS_UpdateParameter(void) |
| { |
| void GPS_UpdateParameter(void) { |
| static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF; |
| if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) { |
| FlightMode = GPS_FLIGHT_MODE_FREE; |
| } else { |
| if (dynamicParams.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
| else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE; |
| else FlightMode = GPS_FLIGHT_MODE_HOME; |
| } |
| if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) |
| { |
| FlightMode = GPS_FLIGHT_MODE_FREE; |
| } |
| else |
| { |
| if (dynamicParams.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
| else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE; |
| else FlightMode = GPS_FLIGHT_MODE_HOME; |
| } |
| if (FlightMode != FlightModeOld) |
| { |
| BeepTime = 100; |
| } |
| if (FlightMode != FlightModeOld) { |
| BeepTime = 100; |
| } |
| FlightModeOld = FlightMode; |
| } |
| // --------------------------------------------------------------------------------- |
| // This function defines a good GPS signal condition |
| uint8_t GPS_IsSignalOK(void) |
| { |
| uint8_t GPS_IsSignalOK(void) { |
| static uint8_t GPSFix = 0; |
| if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) |
| { |
| GPSFix = 1; |
| return(1); |
| } |
| if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) { |
| GPSFix = 1; |
| return 1; |
| } |
| else return (0); |
| } |
| } |
| // --------------------------------------------------------------------------------- |
| // rescale xy-vector length to limit |
| uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) |
| { |
| uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) { |
| uint8_t retval = 0; |
| int32_t len; |
| len = (int32_t)c_sqrt(*x * *x + *y * *y); |
| if (len > limit) |
| { |
| // normalize control vector components to the limit |
| *x = (*x * limit) / len; |
| *y = (*y * limit) / len; |
| retval = 1; |
| } |
| if (len > limit) { |
| // normalize control vector components to the limit |
| *x = (*x * limit) / len; |
| *y = (*y * limit) / len; |
| retval = 1; |
| } |
| return(retval); |
| } |
| // checks nick and roll sticks for manual control |
| uint8_t GPS_IsManualControlled(void) |
| { |
| if ( (abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0; |
| uint8_t GPS_IsManualControlled(void) { |
| if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0; |
| else return 1; |
| } |
| // set given position to current gps position |
| uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) |
| { |
| uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) { |
| uint8_t retval = 0; |
| if(pGPSPos == NULL) return(retval); // bad pointer |
| if(GPS_IsSignalOK()) |
| { // is GPS signal condition is fine |
| pGPSPos->Longitude = GPSInfo.longitude; |
| pGPSPos->Latitude = GPSInfo.latitude; |
| pGPSPos->Altitude = GPSInfo.altitude; |
| pGPSPos->Status = NEWDATA; |
| retval = 1; |
| } |
| else |
| { // bad GPS signal condition |
| pGPSPos->Status = INVALID; |
| retval = 0; |
| } |
| if(GPS_IsSignalOK()) { // is GPS signal condition is fine |
| pGPSPos->Longitude = GPSInfo.longitude; |
| pGPSPos->Latitude = GPSInfo.latitude; |
| pGPSPos->Altitude = GPSInfo.altitude; |
| pGPSPos->Status = NEWDATA; |
| retval = 1; |
| } else { // bad GPS signal condition |
| pGPSPos->Status = INVALID; |
| retval = 0; |
| } |
| return(retval); |
| } |
| // clear position |
| uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) |
| { |
| uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) { |
| uint8_t retval = 0; |
| if(pGPSPos == NULL) return(retval); // bad pointer |
| else |
| { |
| pGPSPos->Longitude = 0; |
| pGPSPos->Latitude = 0; |
| pGPSPos->Altitude = 0; |
| pGPSPos->Status = INVALID; |
| retval = 1; |
| } |
| if(pGPSPos == NULL) |
| return retval; // bad pointer |
| else { |
| pGPSPos->Longitude = 0; |
| pGPSPos->Latitude = 0; |
| pGPSPos->Altitude = 0; |
| pGPSPos->Status = INVALID; |
| retval = 1; |
| } |
| return (retval); |
| } |
| // disable GPS control sticks |
| void GPS_Neutral(void) |
| { |
| void GPS_Neutral(void) { |
| GPSStickNick = 0; |
| GPSStickRoll = 0; |
| } |
| 196,8 → 178,7 |
| // calculates the GPS control stick values from the deviation to target position |
| // if the pointer to the target positin is NULL or is the target position invalid |
| // then the P part of the controller is deactivated. |
| void GPS_PIDController(GPS_Pos_t *pTargetPos) |
| { |
| void GPS_PIDController(GPS_Pos_t *pTargetPos) { |
| static int32_t PID_Nick, PID_Roll; |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 208,113 → 189,107 |
| static GPS_Pos_t *pLastTargetPos = 0; |
| // if GPS data and Compass are ok |
| if( GPS_IsSignalOK() && (CompassHeading >= 0) ) |
| { |
| if( GPS_IsSignalOK() && (CompassHeading >= 0)) { |
| if(pTargetPos != NULL) // if there is a target position |
| { |
| if(pTargetPos->Status != INVALID) // and the position data are valid |
| { |
| // if the target data are updated or the target pointer has changed |
| if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) ) |
| { |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| // recalculate latitude projection |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
| // remember last target pointer |
| pLastTargetPos = pTargetPos; |
| // mark data as processed |
| pTargetPos->Status = PROCESSED; |
| } |
| // calculate position deviation from latitude and longitude differences |
| GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| } |
| else // no valid target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| } |
| else // no target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| //Calculate PID-components of the controller |
| // D-Part |
| D_North = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.velnorth)/512; |
| D_East = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.veleast)/512; |
| // P-Part |
| P_North = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_North)/2048; |
| P_East = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_East)/2048; |
| // I-Part |
| I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| I_East = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| // combine P & I |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
| } |
| // combine PI- and D-Part |
| PID_North += D_North; |
| PID_East += D_East; |
| // scale combination with gain. |
| PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| PID_East = (PID_East * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| if(pTargetPos != NULL) // if there is a target position |
| { |
| if(pTargetPos->Status != INVALID) // and the position data are valid |
| { |
| // if the target data are updated or the target pointer has changed |
| if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) ) |
| { |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| // recalculate latitude projection |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
| // remember last target pointer |
| pLastTargetPos = pTargetPos; |
| // mark data as processed |
| pTargetPos->Status = PROCESSED; |
| } |
| // calculate position deviation from latitude and longitude differences |
| GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| } |
| else // no valid target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| } |
| else // no target position available |
| { |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| //Calculate PID-components of the controller |
| // D-Part |
| D_North = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.velnorth)/512; |
| D_East = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.veleast)/512; |
| // P-Part |
| P_North = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_North)/2048; |
| P_East = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_East)/2048; |
| // I-Part |
| I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| I_East = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| // combine P & I |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
| } |
| // combine PI- and D-Part |
| PID_North += D_North; |
| PID_East += D_East; |
| // scale combination with gain. |
| PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| PID_East = (PID_East * (int32_t)dynamicParams.NaviGpsGain) / 100; |
| // GPS to nick and roll settings |
| // A positive nick angle moves head downwards (flying forward). |
| // A positive roll angle tilts left side downwards (flying left). |
| // If compass heading is 0 the head of the copter is in north direction. |
| // A positive nick angle will fly to north and a positive roll angle will fly to west. |
| // In case of a positive north deviation/velocity the |
| // copter should fly to south (negative nick). |
| // In case of a positive east position deviation and a positive east velocity the |
| // copter should fly to west (positive roll). |
| // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
| // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| // limit resulting GPS control vector |
| GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| GPSStickNick = (int16_t)PID_Nick; |
| GPSStickRoll = (int16_t)PID_Roll; |
| } |
| // GPS to nick and roll settings |
| // A positive nick angle moves head downwards (flying forward). |
| // A positive roll angle tilts left side downwards (flying left). |
| // If compass heading is 0 the head of the copter is in north direction. |
| // A positive nick angle will fly to north and a positive roll angle will fly to west. |
| // In case of a positive north deviation/velocity the |
| // copter should fly to south (negative nick). |
| // In case of a positive east position deviation and a positive east velocity the |
| // copter should fly to west (positive roll). |
| // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values |
| // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| // limit resulting GPS control vector |
| GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| GPSStickNick = (int16_t)PID_Nick; |
| GPSStickRoll = (int16_t)PID_Roll; |
| } |
| else // invalid GPS data or bad compass reading |
| { |
| GPS_Neutral(); // do nothing |
| 324,11 → 299,7 |
| } |
| } |
| void GPS_Main(void) |
| { |
| void GPS_Main(void) { |
| static uint8_t GPS_P_Delay = 0; |
| static uint16_t beep_rythm = 0; |
| 335,131 → 306,128 |
| GPS_UpdateParameter(); |
| // store home position if start of flight flag is set |
| if(MKFlags & MKFLAG_CALIBRATE) |
| { |
| if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |
| if(MKFlags & MKFLAG_CALIBRATE) { |
| if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |
| } |
| switch(GPSInfo.status) { |
| case INVALID: // invalid gps data |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) { |
| BeepTime = 100; // beep if signal is neccesary |
| } |
| switch(GPSInfo.status) |
| { |
| case INVALID: // invalid gps data |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| BeepTime = 100; // beep if signal is neccesary |
| } |
| break; |
| case PROCESSED: // if gps data are already processed do nothing |
| // downcount timeout |
| if(GPSTimeout) GPSTimeout--; |
| // if no new data arrived within timeout set current data invalid |
| // and therefore disable GPS |
| else |
| { |
| GPS_Neutral(); |
| break; |
| case PROCESSED: // if gps data are already processed do nothing |
| // downcount timeout |
| if(GPSTimeout) GPSTimeout--; |
| // if no new data arrived within timeout set current data invalid |
| // and therefore disable GPS |
| else |
| { |
| GPS_Neutral(); |
| GPSInfo.status = INVALID; |
| } |
| break; |
| case NEWDATA: // new valid data from gps device |
| // if the gps data quality is good |
| beep_rythm++; |
| if (GPS_IsSignalOK()) |
| { |
| switch(FlightMode) // check what's to do |
| { |
| case GPS_FLIGHT_MODE_FREE: |
| // update hold position to current gps position |
| GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad |
| // disable gps control |
| GPS_Neutral(); |
| break; |
| case GPS_FLIGHT_MODE_AID: |
| if(HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| // update hold point to current gps position |
| GPS_SetCurrPosition(&HoldPosition); |
| // disable gps control |
| GPS_Neutral(); |
| GPS_P_Delay = 0; |
| } |
| else // GPS control active |
| { |
| if(GPS_P_Delay < 7) |
| { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position |
| GPS_PIDController(NULL); // activates only the D-Part |
| } |
| else GPS_PIDController(&HoldPosition);// activates the P&D-Part |
| } |
| } |
| else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| break; |
| case GPS_FLIGHT_MODE_HOME: |
| if(HomePosition.Status != INVALID) |
| { |
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| GPS_SetCurrPosition(&HoldPosition); |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HomePosition); |
| } |
| } |
| else // bad home position |
| { |
| BeepTime = 50; // signal invalid home position |
| // try to hold at least the position as a fallback option |
| if (HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HoldPosition); |
| } |
| } |
| else |
| { // try to catch a valid hold position |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| } |
| break; // eof TSK_HOME |
| default: // unhandled task |
| GPS_Neutral(); |
| break; // eof default |
| } // eof switch GPS_Task |
| } // eof gps data quality is good |
| else // gps data quality is bad |
| { // disable gps control |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| } |
| } |
| } |
| break; |
| case NEWDATA: // new valid data from gps device |
| // if the gps data quality is good |
| beep_rythm++; |
| if (GPS_IsSignalOK()) |
| { |
| switch(FlightMode) // check what's to do |
| { |
| case GPS_FLIGHT_MODE_FREE: |
| // update hold position to current gps position |
| GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad |
| // disable gps control |
| GPS_Neutral(); |
| break; |
| case GPS_FLIGHT_MODE_AID: |
| if(HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| // update hold point to current gps position |
| GPS_SetCurrPosition(&HoldPosition); |
| // disable gps control |
| GPS_Neutral(); |
| GPS_P_Delay = 0; |
| } |
| else // GPS control active |
| { |
| if(GPS_P_Delay < 7) |
| { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position |
| GPS_PIDController(NULL); // activates only the D-Part |
| } |
| else GPS_PIDController(&HoldPosition);// activates the P&D-Part |
| } |
| } |
| else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| break; |
| case GPS_FLIGHT_MODE_HOME: |
| if(HomePosition.Status != INVALID) |
| { |
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| GPS_SetCurrPosition(&HoldPosition); |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HomePosition); |
| } |
| } |
| else // bad home position |
| { |
| BeepTime = 50; // signal invalid home position |
| // try to hold at least the position as a fallback option |
| if (HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| { |
| GPS_PIDController(&HoldPosition); |
| } |
| } |
| else |
| { // try to catch a valid hold position |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| } |
| break; // eof TSK_HOME |
| default: // unhandled task |
| GPS_Neutral(); |
| break; // eof default |
| } // eof switch GPS_Task |
| } // eof gps data quality is good |
| else // gps data quality is bad |
| { // disable gps control |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| } |
| } |
| // set current data as processed to avoid further calculations on the same gps data |
| GPSInfo.status = PROCESSED; |
| break; |
| } // eof GPSInfo.status |
| GPSInfo.status = PROCESSED; |
| break; |
| } // eof GPSInfo.status |
| } |
| /branches/dongfang_FC_rewrite/heightControl.c |
|---|
| 0,0 → 1,178 |
| #include <inttypes.h> |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "controlMixer.h" |
| // for digital / LED debug. |
| #include "output.h" |
| // For scope debugging only! |
| #include "rc.h" |
| #define INTEGRAL_LIMIT 100000 |
| /* |
| #define DEBUGINTEGRAL 0 |
| #define DEBUGDIFFERENTIAL 0 |
| #define DEBUGHOVERTHROTTLE 0 |
| #define DEBUGHEIGHTSWITCH 0 |
| */ |
| #define LATCH_TIME 40 |
| int32_t groundPressure; |
| int32_t targetHeight; |
| int32_t rampedTargetHeight; |
| #define DEFAULT_HOVERTHROTTLE 50 |
| int32_t stronglyFilteredHeightDiff = 0; |
| uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
| uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
| #define HOVERTHROTTLEFILTER 25 |
| uint8_t heightRampingTimer = 0; |
| int32_t maxHeight; |
| int32_t iHeight; |
| /* |
| These parameters are free to take: |
| uint8_t HeightMinGas; // Value : 0-100 |
| uint8_t HeightD; // Value : 0-250 |
| uint8_t MaxHeight; // Value : 0-32 |
| uint8_t HeightP; // Value : 0-32 |
| uint8_t Height_Gain; // Value : 0-50 |
| uint8_t Height_ACC_Effect; // Value : 0-250 |
| */ |
| int32_t getHeight(void) { |
| return groundPressure - filteredAirPressure; |
| } |
| void HC_setGround(void) { |
| groundPressure = filteredAirPressure; |
| // This should also happen when height control is enabled in-flight. |
| rampedTargetHeight = getHeight(); |
| maxHeight = 0; |
| iHeight = 0; |
| } |
| void HC_update(void) { |
| int32_t height = getHeight(); |
| static uint8_t setHeightLatch = 0; |
| if (height > maxHeight) |
| maxHeight = height; |
| if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
| // If switch is activated in config |
| DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
| if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
| // Switch is ON |
| if (setHeightLatch <= LATCH_TIME) { |
| if (setHeightLatch == LATCH_TIME) { |
| // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| targetHeight = height; |
| DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
| } |
| // Time not yet reached. |
| setHeightLatch++; |
| } |
| } else { |
| // Switch is OFF. |
| setHeightLatch = 0; |
| DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
| } |
| } else { |
| // Switch is not activated; take the "max-height" as the target height. |
| DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
| targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
| } |
| if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) { |
| heightRampingTimer = 0; |
| if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
| rampedTargetHeight += staticParams.Height_Gain; |
| } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
| rampedTargetHeight -= staticParams.Height_Gain; |
| } |
| } |
| //DebugOut.Analog[16] = (int16_t)(height / 10); |
| //DebugOut.Analog[17] = (int16_t)(rampedTargetHeight / 10); |
| } |
| // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| // ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH |
| // ParamSet.BitConfig & CFG_HEIGHT_3SWITCH |
| // takes 180-200 usec (with integral term). That is too heavy!!! |
| // takes 100 usec without integral term. |
| uint16_t HC_getThrottle(uint16_t throttle) { |
| int32_t height = getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| static int32_t lastHeight; |
| int16_t dHeight = height - lastHeight; |
| lastHeight = height; |
| // DebugOut.Analog[20] = dHeight; |
| // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
| // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| // iHeight += heightError; |
| if (dHeight > 0) { |
| DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF; |
| DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| } else if (dHeight < 0) { |
| DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF; |
| DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF; |
| } |
| /* |
| if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
| else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
| else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
| else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
| */ |
| int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD; |
| // the "minGas" is now a limit for how much up / down the throttle can be varied |
| if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas; |
| else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas; |
| //DebugOut.Analog[18] = dThrottle; |
| //DebugOut.Analog[19] = iHeight / 10000L; |
| // TODO: Eliminate repitition. |
| if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40)) { |
| // If switch is not in use --> Just apply height control. |
| // If switch is in use --> only apply height control when switch is also ON. |
| throttle += dThrottle; |
| } |
| } |
| /* Experiment: Find hover-throttle */ |
| stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
| stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER; |
| if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) { |
| hoverThrottle = stronglyFilteredThrottle; |
| DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
| // DebugOut.Analog[18] = hoverThrottle; |
| } else |
| DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
| return throttle; |
| } |
| /* |
| For a variometer thingy: |
| When switch is thrown on, freeze throttle (capture it into variable) |
| For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
| Output = frozen throttle + whatever is computed +/-. Integral? |
| */ |
| /branches/dongfang_FC_rewrite/heightControl.h |
|---|
| 0,0 → 1,4 |
| void HC_setGround(void); |
| void HC_update(void); |
| uint16_t HC_getThrottle(uint16_t throttle); |
| /branches/dongfang_FC_rewrite/invenSense.h |
|---|
| 1,3 → 1,4 |
| /* |
| #ifndef _INVENSENSE_H |
| #define _INVENSENSE_H |
| 5,25 → 6,25 |
| #define GYRO_HW_NAME "ISens" |
| /* |
| / * |
| * The InvenSense gyros have a lower sensitivity than for example the ADXRS610s on the FC 2.0 ME, |
| * but they have a wider range too. |
| * 2mV/deg/s gyros and no amplifiers: |
| * H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
| */ |
| * / |
| #define GYRO_HW_FACTOR 0.6827f |
| /* |
| / * |
| * Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees. |
| * If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90. |
| * If the hardware related contants are set correctly, flight should be OK without bothering to |
| * make any adjustments here. It is only for luxury. |
| */ |
| * / |
| #define GYRO_PITCHROLL_CORRECTION 0.93f |
| /* |
| / * |
| * Same for yaw. |
| */ |
| * / |
| #define GYRO_YAW_CORRECTION 0.97f |
| #endif |
| */ |
| /branches/dongfang_FC_rewrite/main.c |
|---|
| 136,21 → 136,21 |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = SetDelay(200); |
| J16_ON; |
| OUTPUT_ON(0); |
| GRN_OFF; |
| RED_ON; |
| while(!CheckDelay(timer)); |
| timer = SetDelay(200); |
| J16_OFF; |
| OUTPUT_OFF(0); |
| OUTPUT_ON(1); |
| RED_OFF; |
| GRN_ON; |
| J17_ON; |
| while(!CheckDelay(timer)); |
| timer = SetDelay(200); |
| while(!CheckDelay(timer)); |
| J17_OFF; |
| OUTPUT_OFF(1); |
| twi_diagnostics(); |
| 186,7 → 186,7 |
| else printf("\n\rSupport for GPS at 1st UART"); |
| #endif |
| controlMixer_setNeutral(0); |
| controlMixer_setNeutral(); |
| // Cal. attitude sensors and reset integrals. |
| attitude_setNeutral(); |
| 218,7 → 218,13 |
| flight_control(); |
| J4LOW; |
| sendMotorData(); // the flight control code |
| /* |
| * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
| * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
| * the test throttle vector. If no testing, stop all motors. |
| */ |
| // Obsoleted. |
| // transmitMotorThrottleData(); |
| RED_OFF; |
| 269,9 → 275,13 |
| usart0_ProcessRxData(); |
| if(CheckDelay(timer)) { |
| if(UBat < staticParams.LowVoltageWarning) { |
| if (UBat <= UBAT_AT_5V) { |
| // Do nothing. The voltage on the input side of the regulator is <5V; |
| // we must be running off USB power. Keep it quiet. |
| } else if(UBat < staticParams.LowVoltageWarning) { |
| beepBatteryAlarm(); |
| } |
| #ifdef USE_NAVICTRL |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| /branches/dongfang_FC_rewrite/makefile |
|---|
| 5,11 → 5,11 |
| #------------------------------------------------------------------- |
| VERSION_MAJOR = 0 |
| VERSION_MINOR = 74 |
| VERSION_PATCH = 3 |
| VERSION_PATCH = 100 |
| VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
| VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
| NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
| NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
| #------------------------------------------------------------------- |
| #OPTIONS |
| 19,13 → 19,30 |
| #EXT = MK3MAG |
| # Use optional one the RCs if EXT = NAVICTRL has been used |
| RC = DSL |
| #RC = DSL |
| #RC = SPECTRUM |
| #GYRO=ENC-03_FC1.3 |
| #GYRO_HW_NAME=ENC |
| #GYRO_HW_FACTOR=1.304f |
| #GYRO_PITCHROLL_CORRECTION=0.83f |
| #GYRO_YAW_CORRECTION=0.93f |
| GYRO=ADXRS610_FC2.0 |
| GYRO_HW_NAME=ADXR |
| GYRO_HW_FACTOR=1.2288f |
| GYRO_PITCHROLL_CORRECTION=1.0f |
| GYRO_YAW_CORRECTION=1.0f |
| #GYRO=invenSense |
| #GYRO_HW_NAME=Isense |
| #GYRO_HW_FACTOR=0.6827f |
| #GYRO_PITCHROLL_CORRECTION=0.93f |
| #GYRO_YAW_CORRECTION=0.97f |
| #GYRO= |
| #GYRO=invenSense |
| #------------------------------------------------------------------- |
| # get SVN revision |
| REV=0001 |
| 110,6 → 127,9 |
| ifeq ($(VERSION_PATCH), 17) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV) |
| endif |
| ifeq ($(VERSION_PATCH), 100) |
| TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)df_SVN$(REV) |
| endif |
| # Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
| # (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
| 118,11 → 138,13 |
| ########################################################################################################## |
| # List C source files here. (C dependencies are automatically generated.) |
| SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c externalControl.c GPSControl.c |
| SRC += dongfangMath.c twimaster.c rc.c attitude.c flight.c eeprom.c uart1.c configuration.c $(GYRO).c |
| SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c |
| SRC += externalControl.c GPSControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
| SRC += eeprom.c uart1.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
| ifeq ($(EXT), MK3MAG) |
| SRC += mk3mag.c mymath.c gps.c ubx.c |
| SRC += mk3mag.c gps.c ubx.c |
| #mymath.c |
| endif |
| ifeq ($(EXT), NAVICTRL) |
| SRC += spi.c |
| 172,7 → 194,7 |
| #CFLAGS += -std=c99 |
| CFLAGS += -std=gnu99 |
| CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) |
| CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION} |
| ifeq ($(EXT), MK3MAG) |
| CFLAGS += -DUSE_MK3MAG |
| /branches/dongfang_FC_rewrite/menu.c |
|---|
| 231,19 → 231,19 |
| case 12://BL Communication errors |
| LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
| LCD_printfxy(0,1," %3d %3d %3d %3d ",Motor[0].Error,Motor[1].Error,Motor[2].Error,Motor[3].Error); |
| LCD_printfxy(0,2," %3d %3d %3d %3d ",Motor[4].Error,Motor[5].Error,Motor[6].Error,Motor[7].Error); |
| LCD_printfxy(0,3," %3d %3d %3d %3d ",Motor[8].Error,Motor[9].Error,Motor[10].Error,Motor[11].Error); |
| LCD_printfxy(0,1," %3d %3d %3d %3d ",motor[0].Error,motor[1].Error,motor[2].Error,motor[3].Error); |
| LCD_printfxy(0,2," %3d %3d %3d %3d ",motor[4].Error,motor[5].Error,motor[6].Error,motor[7].Error); |
| LCD_printfxy(0,3," %3d %3d %3d %3d ",motor[8].Error,motor[9].Error,motor[10].Error,motor[11].Error); |
| break; |
| case 13://BL Overview |
| LCD_printfxy(0,0,"BL-Ctrl found " ); |
| LCD_printfxy(0,1," %c %c %c %c ",Motor[0].Present + '-',Motor[1].Present + '-',Motor[2].Present + '-',Motor[3].Present + '-'); |
| LCD_printfxy(0,2," %c %c %c %c ",Motor[4].Present + '-',Motor[5].Present + '-',Motor[6].Present + '-',Motor[7].Present + '-'); |
| LCD_printfxy(0,3," %c - - - ",Motor[8].Present + '-'); |
| if(Motor[9].Present) LCD_printfxy(4,3,"10"); |
| if(Motor[10].Present) LCD_printfxy(8,3,"11"); |
| if(Motor[11].Present) LCD_printfxy(12,3,"12"); |
| LCD_printfxy(0,1," %c %c %c %c ",motor[0].Present + '-',motor[1].Present + '-',motor[2].Present + '-',motor[3].Present + '-'); |
| LCD_printfxy(0,2," %c %c %c %c ",motor[4].Present + '-',motor[5].Present + '-',motor[6].Present + '-',motor[7].Present + '-'); |
| LCD_printfxy(0,3," %c - - - ",motor[8].Present + '-'); |
| if(motor[9].Present) LCD_printfxy(4,3,"10"); |
| if(motor[10].Present) LCD_printfxy(8,3,"11"); |
| if(motor[11].Present) LCD_printfxy(12,3,"12"); |
| break; |
| #if (defined (USE_MK3MAG)) |
| /branches/dongfang_FC_rewrite/mk3mag.c |
|---|
| 52,7 → 52,8 |
| #include <stdlib.h> |
| #include <inttypes.h> |
| #include "timer0.h" |
| #include "rc.h" |
| // #include "rc.h" |
| #include "attitude.h" |
| #include "eeprom.h" |
| #include "mk3mag.h" |
| 99,7 → 100,7 |
| if (PWMCount > 400) |
| { |
| if(PWMTimeout) PWMTimeout--; // decrement timeout |
| CompassHeading = -1; // unknown heading |
| compassHeading = -1; // unknown heading |
| PWMCount = 0; // reset PWM Counter |
| } |
| 108,9 → 109,9 |
| { // ignore pwm values values of 0 and higher than 37 ms; |
| if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms |
| { |
| if(PWMCount <10) CompassHeading = 0; |
| else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
| CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| if(PWMCount <10) compassHeading = 0; |
| else compassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
| compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
| // 12 * 362 counts * 102.4 us |
| } |
| /branches/dongfang_FC_rewrite/mm3.c |
|---|
| 19,7 → 19,7 |
| #include <inttypes.h> |
| #include "mm3.h" |
| #include "mymath.h" |
| //#include "mymath.h" |
| #include "timer0.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| /branches/dongfang_FC_rewrite/navi-lang.txt |
|---|
| 0,0 → 1,52 |
| Navigation language def: |
| Som funktion af t: |
| - Pos. |
| - Hoejde |
| - Synsretning (X-akse antaget vandret?) (som: Fast, lineaer, as-takeoff, at-position, look-forward) |
| - Evt. naeseretning (som: Fast, lineaer, follow-look, as-takeoff, head-at, tail-at, point-forward) |
| - Positionsfunktion interpoleret mellem waypoints |
| - Hoejde det samme |
| - Retninger det samme |
| - For hvert waypoint: |
| Position |
| Hoejde |
| Standtid |
| Se-retning??? (i hvert optional) |
| - Valgmuligheder for positions-interpolation: Rette linier, curvefitting af en eller flere slags... |
| <flight> |
| <waypoint lon="...." lat="...." height="...." /> |
| <leg pos-interpolation="linear" /> |
| <waypoint lon="...." lat="...." height="...." head="270" /> |
| <leg pos-interpolation="linear" head="linear" /> |
| <waypoint lon="...." lat="...." height="...." head="180" /> |
| </flight> |
| class Waypoint { |
| Coordinates pos; |
| int height; |
| int heading; |
| } |
| function linear-height(Waypoint w1, Waypoint w2, float x) { |
| return w1.height + (w2.height - w1.height) * x; |
| } |
| Fancy udgave: |
| <waypoint> |
| <lat></lat> |
| <lon></lon> |
| <height></height> |
| </waypoint> |
| <leg> |
| <height>linear(w_o, w_t)</height> |
| </leg> |
| 2 interpreters: 1 til Google Maps ell l og 1 til navi-ctrl. |
| /branches/dongfang_FC_rewrite/naviControl.c |
|---|
| --- naviControl.h (nonexistent) |
| +++ naviControl.h (revision 1775) |
| @@ -0,0 +1,95 @@ |
| +//void NC_update(void); |
| +//int16_t* NC_getPRTY(void); |
| +//uint8_t NC_getArgument(void); |
| +//uint8_t NC_getCommand(void); |
| +//int16_t NC_getVariable(uint8_t varNum); |
| +//void NC_calibrate(void); |
| +//uint8_t NC_getSignalQuality (void); |
| +//void NC_setNeutral(void); |
| + |
| +// ######################## SPI - FlightCtrl ################### |
| +#ifndef _NAVICONTROL_H |
| +#define _NAVICONTROL_H |
| + |
| +//#include <util/delay.h> |
| +#include <inttypes.h> |
| +#define SPI_PROTOCOL_COMP 1 |
| + |
| +#define SPI_CMD_USER 10 |
| +#define SPI_CMD_STICK 11 |
| +#define SPI_CMD_MISC 12 |
| +#define SPI_CMD_PARAMETER1 13 |
| +#define SPI_CMD_VERSION 14 |
| + |
| +typedef struct { |
| + uint8_t Sync1; |
| + uint8_t Sync2; |
| + uint8_t Command; |
| + int16_t IntegralPitch; |
| + int16_t IntegralRoll; |
| + int16_t AccPitch; |
| + int16_t AccRoll; |
| + int16_t GyroHeading; |
| + int16_t GyroPitch; |
| + int16_t GyroRoll; |
| + int16_t GyroYaw; |
| + union { |
| + int8_t sByte[12]; |
| + uint8_t Byte[12]; |
| + int16_t Int[6]; |
| + int32_t Long[3]; |
| + float Float[3]; |
| + } Param; |
| + uint8_t Chksum; |
| +} __attribute__((packed)) ToNaviCtrl_t; |
| + |
| +#define SPI_CMD_OSD_DATA 100 |
| +#define SPI_CMD_GPS_POS 101 |
| +#define SPI_CMD_GPS_TARGET 102 |
| +#define SPI_KALMAN 103 |
| + |
| +typedef struct { |
| + uint8_t Command; |
| + int16_t GPSStickPitch; |
| + int16_t GPSStickRoll; |
| + int16_t GPS_Yaw; |
| + int16_t CompassHeading; |
| + int16_t Status; |
| + uint16_t BeepTime; |
| + union { |
| + int8_t Byte[12]; |
| + int16_t Int[6]; |
| + int32_t Long[3]; |
| + float Float[3]; |
| + } Param; |
| + uint8_t Chksum; |
| +} __attribute__((packed)) FromNaviCtrl_t; |
| + |
| +typedef struct { |
| + uint8_t Major; |
| + uint8_t Minor; |
| + uint8_t Patch; |
| + uint8_t Compatible; |
| + // unsigned char Hardware; |
| +} __attribute__((packed)) SPI_VersionInfo_t; |
| + |
| +extern ToNaviCtrl_t toNaviCtrl; |
| +extern FromNaviCtrl_t fromNaviCtrl; |
| + |
| +typedef struct { |
| + int8_t KalmanK; |
| + int8_t KalmanMaxDrift; |
| + int8_t KalmanMaxFusion; |
| + uint8_t SerialDataOkay; |
| +} __attribute__((packed)) NCData_t; |
| + |
| +extern uint8_t NCDataOkay; |
| +extern uint8_t NCSerialDataOkay; |
| + |
| +void SPI_MasterInit(void); |
| +void SPI_StartTransmitPacket(void); |
| +void SPI_TransmitByte(void); |
| +// new: |
| +// extern void UpdateSPI_Buffer(void); |
| + |
| +#endif //_NAVICONTROL_H |
| /branches/dongfang_FC_rewrite/output.c |
|---|
| 54,53 → 54,61 |
| // To access the DebugOut struct. |
| #include "uart0.h" |
| uint8_t J16Blinkcount = 0, J16Mask = 1; |
| uint8_t J17Blinkcount = 0, J17Mask = 1; |
| uint8_t flashCnt[2], flashMask[2]; |
| // initializes the LED control outputs J16, J17 |
| void output_init(void) { |
| // set PC2 & PC3 as output (control of J16 & J17) |
| DDRC |= (1<<DDC2)|(1<<DDC3); |
| J16_OFF; |
| J17_OFF; |
| J16Blinkcount = 0; J16Mask = 128; |
| J17Blinkcount = 0; J17Mask = 128; |
| OUTPUT_OFF(0); OUTPUT_OFF(1); |
| flashCnt[0] = flashCnt[1] = 0; |
| flashMask[0] = flashMask[1] = 128; |
| } |
| void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
| if (timing > 250 && manual > 230) { |
| // "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7. |
| OUTPUT_SET(port, bitmask & 128); |
| } else if (timing > 250 && manual < 10) { |
| // "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7. |
| OUTPUT_SET(port, !(bitmask & 128)); |
| } else if(!flashCnt[port]--) { |
| // rotating mask over bitmask... |
| flashCnt[port] = timing - 1; |
| if(flashMask[port] == 1) flashMask[port] = 128; else flashMask[port] >>= 1; |
| OUTPUT_SET(port, flashMask[port] & bitmask); |
| } |
| } |
| void flashingLights(void) { |
| static int8_t delay = 0; |
| if(!delay--) { // 10 ms intervall |
| if(!delay--) { // 10 ms intervals |
| delay = 4; |
| if ((staticParams.J16Timing > 250) && (dynamicParams.J16Timing > 230)) { |
| if(staticParams.J16Bitmask & 128) J16_ON; |
| else J16_OFF; |
| } else if ((staticParams.J16Timing > 250) && (dynamicParams.J16Timing < 10)) { |
| if(staticParams.J16Bitmask & 128) J16_OFF; |
| else J16_ON; |
| } else if(!J16Blinkcount--) { |
| J16Blinkcount = dynamicParams.J16Timing - 1; |
| if(J16Mask == 1) J16Mask = 128; else J16Mask = J16Mask >> 1; |
| if(J16Mask & staticParams.J16Bitmask) J16_ON; else J16_OFF; |
| } |
| if ((staticParams.J17Timing > 250) && (dynamicParams.J17Timing > 230)) { |
| if(staticParams.J17Bitmask & 128) J17_ON; |
| else J17_OFF; |
| } else if ((staticParams.J17Timing > 250) && (dynamicParams.J17Timing < 10)) { |
| if(staticParams.J17Bitmask & 128) J17_OFF; |
| else J17_ON; |
| } else if(!J17Blinkcount--) { |
| J17Blinkcount = dynamicParams.J17Timing - 1; |
| if(J17Mask == 1) J17Mask = 128; else J17Mask = J17Mask >> 1; |
| if(J17Mask & staticParams.J17Bitmask) J17_ON; else J17_OFF; |
| } |
| flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, dynamicParams.J16Timing); |
| flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, dynamicParams.J17Timing); |
| } |
| } |
| #define DIGITAL_DEBUG_MASK 0 |
| // invert means: An "1" bit in digital debug data will feed NO base current to output transistor. |
| #define DIGITAL_DEBUG_INVERT 0 |
| void output_update(void) { |
| // flashingLights(); |
| if (DebugOut.Digital[0]) J16_ON; else J16_OFF; |
| if (DebugOut.Digital[1]) J17_ON; else J17_OFF; |
| uint8_t output0, output1; |
| if (!DIGITAL_DEBUG_MASK) |
| flashingLights(); |
| else { |
| if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST) { |
| // Show the state for a SET bit. If inverse, then invert. |
| output0 = output1 = ~DIGITAL_DEBUG_INVERT; |
| } else if (DIGITAL_DEBUG_INVERT) { |
| output0 = (~DebugOut.Digital[0]) & DIGITAL_DEBUG_MASK; |
| output1 = (~DebugOut.Digital[1]) & DIGITAL_DEBUG_MASK; |
| } else { |
| output0 = DebugOut.Digital[0] & DIGITAL_DEBUG_MASK; |
| output1 = DebugOut.Digital[1] & DIGITAL_DEBUG_MASK; |
| } |
| OUTPUT_SET(0, output0); |
| OUTPUT_SET(1, output1); |
| } |
| } |
| /branches/dongfang_FC_rewrite/output.h |
|---|
| 7,16 → 7,19 |
| // PORTbit = 0 --> LED on. |
| // To use the normal transistor set-up where 1 --> transistor conductive, reverse the |
| // ON and OFF statements. |
| #define J16_ON PORTC &= ~(1<<PORTC2) |
| #define J16_OFF PORTC |= (1<<PORTC2) |
| #define J16_TOGGLE PORTC ^= (1<<PORTC2) |
| #define OUTPUT_ON(num) {PORTC |= (4 << (num));} |
| #define OUTPUT_OFF(num) {PORTC &= ~(4 << (num));} |
| #define OUTPUT_SET(num, state) {if ((state)) OUTPUT_ON(num) else OUTPUT_OFF(num)} |
| #define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));} |
| #define J17_ON PORTC &= ~(1<<PORTC3) |
| #define J17_OFF PORTC |= (1<<PORTC3) |
| #define J17_TOGGLE PORTC ^= (1<<PORTC3) |
| #define DEBUG_LEDTEST 256 |
| #define DEBUG_HEIGHT_SWITCH 1 |
| #define DEBUG_HEIGHT_DIFF 2 |
| #define DEBUG_HOVERTHROTTLE 4 |
| #define DEBUG_ACC0THORDER 8 |
| #define DEBUG_COMMANDREPEATED 16 |
| void output_init(void); |
| void output_update(void); |
| #endif //_output_H |
| /branches/dongfang_FC_rewrite/rc.c |
|---|
| 53,8 → 53,10 |
| #include <avr/interrupt.h> |
| #include "rc.h" |
| #include "uart0.h" |
| #include "controlMixer.h" |
| #include "configuration.h" |
| #include "commands.h" |
| // The channel array is 1-based. The 0th entry is not used. |
| volatile int16_t PPM_in[MAX_CHANNELS]; |
| 62,8 → 64,11 |
| volatile uint8_t NewPpmData = 1; |
| volatile int16_t RC_Quality = 0; |
| int16_t RC_PRTY[4]; |
| uint8_t lastRCCommand = COMMAND_NONE; |
| uint8_t commandTimer = 0; |
| int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
| // Useless. Just trim on the R/C instead. |
| // int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
| /*************************************************************** |
| * 16bit timer 1 is used to decode the PPM-Signal |
| 80,9 → 85,9 |
| // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
| // set as output |
| DDRD |= /*(1<<DDD5)|*/(1<<DDD4); |
| DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3); |
| // low level |
| PORTD &= ~(/*(1<<PORTD5)|*/(1<<PORTD4)); |
| PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
| // PD3 can't be used if 2nd UART is activated |
| // because TXD1 is at that port |
| 167,19 → 172,25 |
| { |
| // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
| // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
| if((signal > 250) && (signal < 687)) |
| { |
| if((signal > 250) && (signal < 687)) { |
| // shift signal to zero symmetric range -154 to 159 |
| signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
| signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
| // check for stable signal |
| if(abs(signal - PPM_in[index]) < 6) { |
| if(RC_Quality < 200) RC_Quality +=10; |
| else RC_Quality = 200; |
| } |
| // calculate exponential history for signal |
| tmp = (3 * PPM_in[index] + signal) / 4; |
| if(tmp > signal + 1) tmp--; else |
| if(tmp < signal-1) tmp++; |
| // If signal is the same as before +/- 1, just keep it there. |
| if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
| // In addition, if the signal is very close to 0, just set it to 0. |
| if (signal >=-1 && signal <= 1) { |
| tmp = 0; |
| } else { |
| tmp = PPM_in[index]; |
| } |
| } |
| else |
| tmp = signal; |
| // calculate signal difference on good signal level |
| if(RC_Quality >= 195) |
| PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
| 202,7 → 213,31 |
| #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
| #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
| #define COMMAND_THRESHOLD 85 |
| #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
| #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
| // Internal. |
| uint8_t RC_getStickCommand(void) { |
| if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
| // vertical is up |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_GYROCAL; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_ACCCAL; |
| return COMMAND_NONE; |
| } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
| // vertical is down |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_STOP; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_START; |
| return COMMAND_NONE; |
| } |
| // vertical is around center |
| return COMMAND_NONE; |
| } |
| /* |
| * This must be called (as the only thing) for each control loop cycle (488 Hz). |
| */ |
| 211,16 → 246,25 |
| if(RC_Quality) { |
| RC_Quality--; |
| if (NewPpmData-- == 0) { |
| RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD; |
| RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - stickOffsetRoll + RCDiff(CH_ROLL) * staticParams.StickD; |
| RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
| RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
| RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
| if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| // exponential stick sensitivity in yawring rate |
| // exponential stick sensitivity in yawing rate |
| tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
| tmp2 += (staticParams.StickYawP * tmp1) / 4; |
| RC_PRTY[CONTROL_YAW] = tmp2; |
| } |
| uint8_t command = RC_getStickCommand(); |
| if (lastRCCommand == command) { |
| // Keep timer from overrunning. |
| if (commandTimer < COMMAND_TIMER) commandTimer++; |
| } else { |
| // There was a change. |
| lastRCCommand = command; |
| commandTimer = 0; |
| } |
| } else { // Bad signal |
| RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
| } |
| 236,10 → 280,10 |
| /* |
| * Get other channel value |
| */ |
| int16_t RC_getVariable(uint8_t varNum) { |
| int16_t RC_getVariable (uint8_t varNum) { |
| if (varNum < 4) |
| // 0th variable is 5th channel (1-based) etc. |
| return RCChannel(varNum + 5); |
| return RCChannel(varNum + 4) + POT_OFFSET; |
| /* |
| * Let's just say: |
| * The RC variable 4 is hardwired to channel 5 |
| 248,7 → 292,7 |
| * The RC variable 7 is hardwired to channel 8 |
| * Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
| */ |
| return PPM_in[varNum + 1]; |
| return PPM_in[varNum + 1] + POT_OFFSET; |
| } |
| uint8_t RC_getSignalQuality(void) { |
| 271,6 → 315,10 |
| * of a stick, it may be useful. |
| */ |
| void RC_calibrate(void) { |
| // Do nothing. |
| } |
| /* |
| if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
| // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
| stickOffsetPitch = stickOffsetRoll = 0; |
| 279,30 → 327,15 |
| stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
| } |
| } |
| */ |
| #define COMMAND_THRESHOLD 85 |
| #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
| #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
| uint8_t RC_getCommand(void) { |
| if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
| // vertical is up |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_GYROCAL; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_ACCCAL; |
| return COMMAND_NONE; |
| } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
| // vertical is down |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| return COMMAND_STOP; |
| if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| return COMMAND_START; |
| return COMMAND_NONE; |
| } else { |
| // vertical is around center |
| return COMMAND_NONE; |
| if (commandTimer == COMMAND_TIMER) { |
| // Stick has been held long enough; command committed. |
| return lastRCCommand; |
| } |
| // Not yet sure what the command is. |
| return COMMAND_NONE; |
| } |
| /* |
| 378,6 → 411,17 |
| return looping; |
| } |
| uint8_t RC_testCompassCalState(void) { |
| static uint8_t stick = 1; |
| // if pitch is centered or top set stick to zero |
| if(RCChannel(CH_PITCH) > -20) stick = 0; |
| // if pitch is down trigger to next cal state |
| if((RCChannel(CH_PITCH) < -70) && !stick) { |
| stick = 1; |
| return 1; |
| } |
| return 0; |
| } |
| /* |
| * Abstract controls are not used at the moment. |
| t_control rc_control = { |
| /branches/dongfang_FC_rewrite/rc.h |
|---|
| 17,10 → 17,13 |
| #define MAX_CHANNELS 10 |
| // Number of cycles a command must be repeated before commit. |
| #define COMMAND_TIMER 200 |
| extern void RC_Init (void); |
| // the RC-Signal. todo: Not export any more. |
| extern volatile int16_t PPM_in[MAX_CHANNELS]; |
| extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
| // extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
| extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
| extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200) |
| 30,7 → 33,7 |
| #define CH_THROTTLE 2 |
| #define CH_YAW 3 |
| #define CH_POTS 4 |
| #define POT_OFFSET 110 |
| #define POT_OFFSET 115 |
| /* |
| int16_t RC_getPitch (void); |
| 48,5 → 51,5 |
| void RC_calibrate(void); |
| uint8_t RC_getSignalQuality(void); |
| uint8_t RC_getLooping(uint8_t looping); |
| uint8_t RC_testCompassCalState(void); |
| #endif //_RC_H |
| /branches/dongfang_FC_rewrite/sendOutData.c |
|---|
| 0,0 → 1,45 |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t txd_bufferIndex = 0; |
| uint8_t *currentBuffer; |
| uint8_t currentBufferIndex; |
| uint16_t lengthOfCurrentBuffer; |
| uint8_t shift = 0; |
| txd_buffer[txd_bufferIndex++] = '#'; // Start character |
| txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[txd_bufferIndex++] = cmd; // Command |
| va_start(ap, numofbuffers); |
| while(numofbuffers) { |
| currentBuffer = va_arg(ap, uint8_t*); |
| lengthOfCurrentBuffer = va_arg(ap, int); |
| currentBufferIndex = 0; |
| // Encode data: 3 bytes of data are encoded into 4 bytes, |
| // where the 2 most significant bits are both 0. |
| while(currentBufferIndex != lengthOfCurrentBuffer) { |
| if (!shift) txd_buffer[txd_bufferIndex] = 0; |
| txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
| txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
| shift += 2; |
| if (shift == 6) { shift=0; txd_bufferIndex++; } |
| currentBufferIndex++; |
| } |
| } |
| // If the number of data bytes was not divisible by 3, stuff |
| // with 0 pseudodata until length is again divisible by 3. |
| if (shift == 2) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex] &= 0b00110000; |
| txd_buffer[++txd_bufferIndex] = 0; |
| shift = 4; |
| } |
| if (shift == 4) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |
| /branches/dongfang_FC_rewrite/spi.c |
|---|
| 111,9 → 111,9 |
| #ifndef WCOL |
| #define WCOL WCOL0 |
| #endif |
| //#ifndef SPI2X |
| #ifndef SPI2X |
| #define SPI2X SPI2X0 |
| //#endif |
| #endif |
| // ------------------------- |
| #define SLAVE_SELECT_DDR_PORT DDRC |
| 125,28 → 125,26 |
| #define SPI_RXSYNCBYTE1 0x81 |
| #define SPI_RXSYNCBYTE2 0x55 |
| typedef enum |
| { |
| SPI_SYNC1, |
| SPI_SYNC2, |
| SPI_DATA |
| } SPI_RXState_t; |
| typedef enum { |
| SPI_SYNC1, |
| SPI_SYNC2, |
| SPI_DATA |
| } SPI_RXState_t; |
| // data exchange packets to and From NaviCtrl |
| ToNaviCtrl_t ToNaviCtrl; |
| FromNaviCtrl_t FromNaviCtrl; |
| ToNaviCtrl_t toNaviCtrl; |
| FromNaviCtrl_t fromNaviCtrl; |
| SPI_VersionInfo_t SPI_VersionInfo; |
| SPI_VersionInfo_t SPI_VersionInfo; |
| // rx packet buffer |
| #define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl) |
| #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
| uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
| uint8_t SPI_RxBufferIndex = 0; |
| uint8_t SPI_RxBuffer_Request = 0; |
| // tx packet buffer |
| #define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl) |
| #define SPI_TXBUFFER_LEN sizeof(toNaviCtrl) |
| uint8_t *SPI_TxBuffer; |
| uint8_t SPI_TxBufferIndex = 0; |
| 170,15 → 168,15 |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
| SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer |
| SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
| SPITransferCompleted = 1; |
| // initialize data packet to NaviControl |
| ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| ToNaviCtrl.Command = SPI_CMD_USER; |
| ToNaviCtrl.IntegralNick = 0; |
| ToNaviCtrl.IntegralRoll = 0; |
| toNaviCtrl.Command = SPI_CMD_USER; |
| toNaviCtrl.IntegralPitch = 0; |
| toNaviCtrl.IntegralRoll = 0; |
| NCSerialDataOkay = 0; |
| NCDataOkay = 0; |
| 190,7 → 188,6 |
| SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| } |
| /**********************************************************/ |
| /* Update Data transferd by the SPI from/to NaviCtrl */ |
| /**********************************************************/ |
| 200,80 → 197,83 |
| cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| // update content of packet to NaviCtrl |
| ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be?? |
| ToNaviCtrl.GyroRoll = rate_PID[ROLL]; |
| ToNaviCtrl.GyroYaw = yawRate; |
| ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| // naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
| toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
| toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
| toNaviCtrl.GyroYaw = yawRate; |
| toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| switch(ToNaviCtrl.Command) { |
| // TODO: What are these little bastards? |
| averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
| switch(toNaviCtrl.Command) { |
| case SPI_CMD_USER: |
| for (i=0; i<sizeof(dynamicParams.UserParams); i++) { |
| ToNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| } |
| ToNaviCtrl.Param.Byte[8] = MKFlags; |
| toNaviCtrl.Param.Byte[8] = MKFlags; |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
| ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat; |
| ToNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| ToNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| toNaviCtrl.Param.Byte[9] = (uint8_t)UBat; |
| toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| break; |
| case SPI_CMD_PARAMETER1: |
| ToNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
| ToNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
| ToNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
| ToNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
| ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
| ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
| ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
| ToNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
| ToNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
| ToNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
| ToNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| ToNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
| toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
| toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
| toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
| toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
| toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
| toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| break; |
| case SPI_CMD_STICK: |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| ToNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| ToNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| ToNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| ToNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
| ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
| toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| break; |
| case SPI_CMD_MISC: |
| ToNaviCtrl.Param.Byte[0] = compassCalState; |
| if(compassCalState > 4) |
| { // jump from 5 to 0 |
| compassCalState = 0; |
| } |
| ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| toNaviCtrl.Param.Byte[0] = compassCalState; |
| if(compassCalState > 4) { // jump from 5 to 0 |
| compassCalState = 0; |
| } |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| // TODO: Height and in the correct scaling... |
| toNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| break; |
| case SPI_CMD_VERSION: |
| ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| ToNaviCtrl.Param.Byte[4] = BoardRelease; |
| toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| toNaviCtrl.Param.Byte[4] = BoardRelease; |
| break; |
| default: |
| break; |
| } |
| 283,35 → 283,32 |
| // analyze content of packet from NaviCtrl if valid |
| if (SPI_RxDataValid) { |
| // update gps controls |
| if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| GPSStickPitch = FromNaviCtrl.GPSStickNick; |
| GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
| if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
| GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
| NCDataOkay = 250; |
| } |
| // update compass readings |
| if(FromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = FromNaviCtrl.CompassHeading; |
| if(fromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = fromNaviCtrl.CompassHeading; |
| } |
| if(compassHeading < 0) compassOffCourse = 0; |
| else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| // NaviCtrl wants to beep? |
| if (FromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = FromNaviCtrl.BeepTime; |
| if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime; |
| switch (FromNaviCtrl.Command) { |
| switch (fromNaviCtrl.Command) { |
| case SPI_KALMAN: |
| dynamicParams.KalmanK = FromNaviCtrl.Param.Byte[0]; |
| dynamicParams.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
| dynamicParams.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
| NCSerialDataOkay = FromNaviCtrl.Param.Byte[3]; |
| //DebugOut.Analog[29] = NCSerialDataOkay; |
| dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
| dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1]; |
| dynamicParams.KalmanMaxDrift = fromNaviCtrl.Param.Byte[2]; |
| NCSerialDataOkay = fromNaviCtrl.Param.Byte[3]; |
| break; |
| default: |
| break; |
| } |
| } |
| else // no valid data from NaviCtrl |
| { |
| } else { // no valid data from NaviCtrl |
| // disable GPS control |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| 328,11 → 325,11 |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
| // cyclic commands |
| ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| SPITransferCompleted = 0; // transfer is in progress |
| UpdateSPI_Buffer(); // update data in ToNaviCtrl |
| UpdateSPI_Buffer(); // update data in toNaviCtrl |
| SPI_TxBufferIndex = 1; //proceed with 2nd byte |
| 341,8 → 338,8 |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum |
| SPDR = ToNaviCtrl.Sync1; // send first byte |
| toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum |
| SPDR = toNaviCtrl.Sync1; // send first byte |
| } |
| } |
| 389,35 → 386,26 |
| case SPI_DATA: // data bytes |
| SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
| // if all bytes are received of a packet from the NaviCtrl |
| if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) |
| { // last byte transfered is the checksum of the packet |
| if (rxdata == rxchksum) // checksum matching? |
| { |
| if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) { // last byte transfered is the checksum of the packet |
| if (rxdata == rxchksum) { // checksum matching? |
| // copy SPI_RxBuffer -> FromFlightCtrl |
| uint8_t *ptr = (uint8_t *)&FromNaviCtrl; |
| uint8_t *ptr = (uint8_t *)&fromNaviCtrl; |
| cli(); |
| memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl)); |
| memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl)); |
| sei(); |
| SPI_RxDataValid = 1; |
| //DebugOut.Analog[18]++; |
| } |
| else |
| { // checksum does not match |
| //DebugOut.Analog[17]++; |
| } else { // checksum does not match |
| SPI_RxDataValid = 0; // reset valid flag |
| } |
| SPI_RXState = SPI_SYNC1; // reset state sync |
| } |
| else // not all bytes transfered |
| { |
| rxchksum += rxdata; // update checksum |
| } |
| SPI_RXState = SPI_SYNC1; // reset state sync |
| } else { // not all bytes transfered |
| rxchksum += rxdata; // update checksum |
| } |
| break; |
| }// eof switch(SPI_RXState) |
| // if still some bytes left for transmission to NaviCtrl |
| if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) |
| { |
| if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| 424,7 → 412,7 |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
| ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
| toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
| SPI_TxBufferIndex++; |
| } else { |
| //Transfer of all bytes of the packet to NaviCtrl completed |
| /branches/dongfang_FC_rewrite/spi.h |
|---|
| 13,25 → 13,25 |
| #define SPI_CMD_VERSION 14 |
| typedef struct { |
| uint8_t Sync1; |
| uint8_t Sync2; |
| uint8_t Command; |
| int16_t IntegralNick; |
| int16_t IntegralRoll; |
| int16_t AccNick; |
| int16_t AccRoll; |
| int16_t GyroHeading; |
| int16_t GyroNick; |
| int16_t GyroRoll; |
| int16_t GyroYaw; |
| union { |
| int8_t sByte[12]; |
| uint8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| uint8_t Sync1; |
| uint8_t Sync2; |
| uint8_t Command; |
| int16_t IntegralPitch; |
| int16_t IntegralRoll; |
| int16_t AccPitch; |
| int16_t AccRoll; |
| int16_t GyroHeading; |
| int16_t GyroPitch; |
| int16_t GyroRoll; |
| int16_t GyroYaw; |
| union { |
| int8_t sByte[12]; |
| uint8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) ToNaviCtrl_t; |
| #define SPI_CMD_OSD_DATA 100 |
| 40,38 → 40,38 |
| #define SPI_KALMAN 103 |
| typedef struct { |
| uint8_t Command; |
| int16_t GPSStickNick; |
| int16_t GPSStickRoll; |
| int16_t GPS_Yaw; |
| int16_t CompassHeading; |
| int16_t Status; |
| uint16_t BeepTime; |
| union { |
| int8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| uint8_t Command; |
| int16_t GPSStickPitch; |
| int16_t GPSStickRoll; |
| int16_t GPS_Yaw; |
| int16_t CompassHeading; |
| int16_t Status; |
| uint16_t BeepTime; |
| union { |
| int8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) FromNaviCtrl_t; |
| typedef struct { |
| uint8_t Major; |
| uint8_t Minor; |
| uint8_t Patch; |
| uint8_t Compatible; |
| uint8_t Major; |
| uint8_t Minor; |
| uint8_t Patch; |
| uint8_t Compatible; |
| // unsigned char Hardware; |
| } __attribute__((packed)) SPI_VersionInfo_t; |
| extern ToNaviCtrl_t ToNaviCtrl; |
| extern FromNaviCtrl_t FromNaviCtrl; |
| extern ToNaviCtrl_t toNaviCtrl; |
| extern FromNaviCtrl_t fromNaviCtrl; |
| typedef struct { |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| uint8_t SerialDataOkay; |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| uint8_t SerialDataOkay; |
| } __attribute__((packed)) NCData_t; |
| extern uint8_t NCDataOkay; |
| 80,5 → 80,7 |
| void SPI_MasterInit(void); |
| void SPI_StartTransmitPacket(void); |
| void SPI_TransmitByte(void); |
| // new: |
| // extern void UpdateSPI_Buffer(void); |
| #endif //_SPI_H |
| /branches/dongfang_FC_rewrite/temp |
|---|
| 0,0 → 1,48 |
| H display: |
| request: if remoteKeys displayLine = 0 |
| set request flag. |
| print: |
| if (remotekeys == 1) dec menuitem |
| else if (remotekeys == 2) inc menuitem |
| else if (remotekeys == 3) menuitem <-- 0 |
| response: |
| print; |
| Dear Phat, am I still your beloved after all this? |
| I felt pretty well today and went to work, but I got tired. I am tired of the work too but OK I get paid. Tomorrow I will talk with the doctor again, she will get some more test results. Maybe we will never find out what it is ... I will then just hope it will cure by itself before too long. |
| Tomorrow I will go to a colleague's wedding. She is Swiss, and really nice. I hope I will feel OK and not be sick. I have to get a present for her and I have to be there at 2 in the afternoon. Somebody else I know also said he would love to go (he likes weddings very much) but later he canceled again... so I will go there alone. |
| I am sorry to hear about Pook not enjoying herself so much now... what can I say, things will get better. Just wait. And try yourself. She is such a sweet, fun loving girl. Ask her next time how she and I (!) danced while you were away for 5 minutes at DJ Station. But really, how does one survive in Bangkok without a job? I guess there is no real social security? How does she do? It is quite totally different from me here. You cannot really have a problem here, unless you really have taken care to get one. I never received any social security myself (even though I was without a job for a few months after graduating - I didn't really want to work in that town). |
| I also am not sure if I have the right job or not. Where I am now is a disaster site, I think. Everything is done wrongly and inefficiently, a huge waste of money. But honestly, I don't know how that could be done any better. In IT, you almost always have to work for some "business" people, and almost everything they think they know about IT is misunderstood... so I am working lazily, without pride and thinking it all doesn't matter because it will be rubbish anyway... not very good. Eventually I will be a grumpy old man (if I am not already, ha ha) that nobody wants to meet in the morning... |
| Thank you for your very very nice words about me :) I guess you can say I am not cold like some others. I am a little careful about finding the people I want to be together with, but then I can see a lot af charming and wonderful things in each person. Sometimes some disappointing ones too, but not until later. I would like you to be picky too - and rather just wait longer than to go with the wrong one. You are a really warm and sweet person. You easily deserve someone very very good to you - and also someone that you yourself find really attractive and wonderful. And someone with a personality and an energy level that are agreeable with yours. |
| I also very much enjoyed our happy but short time together. We had a nice way of sleeping together, always holding each other. That is actually the best of all time together, I think. |
| I know it is not very fair of me to fall in love with someone else, what can I say? Sorry, I think is the word. But I really did. But I am happy still to be able to write to you. And that is because I enjoy writing to you. |
| Even despite of that .. it is my bed time now. I sleep 12 hours a day...! It is a waste of precious time (there are so many many things I want to do, all the time! Flying and building my models, swim, do training, do my computer programming and electonics projects, get a Facebook profile, run over the mountains home from office, go out dancing, travel, see old and new friends...) and I just sleep instead. But I cannot really do anything else at the moment. I hope I will have my energy back, and soon.. |
| Also kisses to you and if I may hold you...? |
| Your |
| Soren |
| Dear Phat, |
| Thank you again so much for the nice emails... |
| Today, I have gone flying, and smashed up 2 helicopters. But the damage was only like 50 Baht. I went to the wedding together with 2 colleagues, we planned to come a little late (after church) but we came too late - almost 1 hr. Anyway we were there during all of the reception, in the garden of a beautiful old house. The weather was fantastic, usually it rains a lot in this season but today was sunny and warm, without being too hot. |
| And yes there was at least anohter gay but I dont care really :) And there was a gorgeous and charming young Swiss boy with a ... well not really so attractive girlfriend. He was crazy about her. That was a little funny to watch... |
| I talked with the doctor again today, on the phone. I don't have any hepatitis either, and now she doesn't have any more ideas what it can be. We just have to wait and see if it cures by itself, as viral infections almost always do. This afternoon, I felt like going out tonight, but now I don't know. Feel a little tired again. I can't wait till I can do some sports again! Then I will be better. |
| My life is OK basically, it's just that my work is too boring and I have too little energy now. Will find a solution soon! |
| Now I will go to sleep again. I didn't sleep for so long time out of boredom, just because I was feeling too tired. Better now. |
| Love from |
| Yr Soren |
| /branches/dongfang_FC_rewrite/timer0.c |
|---|
| 54,6 → 54,9 |
| #include "eeprom.h" |
| #include "analog.h" |
| // for degugging! |
| #include "rc.h" |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| 140,7 → 143,9 |
| if(!cnt--) { // every 10th run (9.765kHz/10 = 976Hz) |
| cnt = 9; |
| cnt_1ms^=1; |
| if(!cnt_1ms) runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
| if(!cnt_1ms) { |
| runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
| } |
| CountMilliseconds++; // increment millisecond counter |
| } |
| /branches/dongfang_FC_rewrite/timer2.c |
|---|
| 222,7 → 222,6 |
| } |
| remainingPulseLength += ServoPitchValue - (256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position |
| ServoPitchValue /= MULTIPLIER; |
| // DebugOut.Analog[20] = ServoPitchValue; |
| break; |
| case 2: // Roll Compensation Servo |
| 244,7 → 243,6 |
| } |
| remainingPulseLength += ServoRollValue - (256 / 2) * MULTIPLIER; // shift ServoRollValue to center position |
| ServoRollValue /= MULTIPLIER; |
| //DebugOut.Analog[20] = ServoRollValue; |
| break; |
| default: // other servo channels |
| /branches/dongfang_FC_rewrite/twimaster.c |
|---|
| 66,7 → 66,7 |
| volatile uint16_t I2CTimeout = 100; |
| uint8_t missingMotor = 0; |
| MotorData_t Motor[MAX_MOTORS]; |
| MotorData_t motor[MAX_MOTORS]; |
| volatile uint8_t DACValues[4]; |
| uint8_t DACChannel = 0; |
| 101,10 → 101,10 |
| motor_read = 0; |
| for(i=0; i < MAX_MOTORS; i++) { |
| Motor[i].SetPoint = 0; |
| Motor[i].Present = 0; |
| Motor[i].Error = 0; |
| Motor[i].MaxPWM = 0; |
| motor[i].SetPoint = 0; |
| motor[i].Present = 0; |
| motor[i].Error = 0; |
| motor[i].MaxPWM = 0; |
| } |
| SREG = sreg; |
| 206,12 → 206,12 |
| else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
| break; |
| case 1: // Send Data to Slave |
| I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint |
| I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
| break; |
| case 2: // repeat case 0+1 for all motors |
| if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
| if(!missing_motor) missing_motor = motor_write + 1; |
| if(++Motor[motor_write].Error == 0) Motor[motor_write].Error = 255; // increment error counter and handle overflow |
| if(++motor[motor_write].Error == 0) motor[motor_write].Error = 255; // increment error counter and handle overflow |
| } |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| I2CTimeout = 10; |
| 222,12 → 222,12 |
| case 3: |
| if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
| // no response from the addressed slave received |
| Motor[motor_read].Present = 0; |
| motor[motor_read].Present = 0; |
| motor_read++; // next motor |
| if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| } else { |
| Motor[motor_read].Present = ('1' - '-') + motor_read; |
| motor[motor_read].Present = ('1' - '-') + motor_read; |
| I2C_ReceiveByte(); //Transmit 1st byte |
| } |
| missingMotor = missing_motor; |
| 234,12 → 234,12 |
| missing_motor = 0; |
| break; |
| case 4: //Read 1st byte and transmit 2nd Byte |
| Motor[motor_read].Current = TWDR; |
| motor[motor_read].Current = TWDR; |
| I2C_ReceiveLastByte(); // nack |
| break; |
| case 5: |
| //Read 2nd byte |
| Motor[motor_read].MaxPWM = TWDR; |
| motor[motor_read].MaxPWM = TWDR; |
| motor_read++; // next motor |
| if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| 289,7 → 289,7 |
| printf("\n\rFound BL-Ctrl: "); |
| for(i = 0; i < MAX_MOTORS; i++) { |
| Motor[i].SetPoint = 0; |
| motor[i].SetPoint = 0; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| 300,11 → 300,11 |
| for(i = 0; i < MAX_MOTORS; i++) { |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| _delay_ms(2); |
| if(Motor[i].Present) printf("%d ",i+1); |
| if(motor[i].Present) printf("%d ",i+1); |
| } |
| for(i = 0; i < MAX_MOTORS; i++) { |
| if(!Motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
| Motor[i].Error = 0; |
| if(!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
| motor[i].Error = 0; |
| } |
| } |
| /branches/dongfang_FC_rewrite/twimaster.h |
|---|
| 22,7 → 22,7 |
| } __attribute__((packed)) MotorData_t; |
| #define MAX_MOTORS 12 |
| extern MotorData_t Motor[MAX_MOTORS]; |
| extern MotorData_t motor[MAX_MOTORS]; |
| extern volatile uint16_t I2CTimeout; |
| /branches/dongfang_FC_rewrite/uart0.c |
|---|
| 64,6 → 64,7 |
| #include "attitude.h" |
| #include "rc.h" |
| #include "externalControl.h" |
| #include "output.h" |
| #if defined (USE_MK3MAG) |
| #include "ubx.h" |
| 79,15 → 80,17 |
| #define FALSE 0 |
| #define TRUE 1 |
| //int8_t test __attribute__ ((section (".noinit"))); |
| uint8_t Request_VerInfo = FALSE; |
| uint8_t Request_ExternalControl = FALSE; |
| uint8_t Request_Display = FALSE; |
| uint8_t Request_Display1 = FALSE; |
| uint8_t Request_DebugData = FALSE; |
| uint8_t Request_Data3D = FALSE; |
| uint8_t Request_DebugLabel = 255; |
| uint8_t Request_PPMChannels = FALSE; |
| uint8_t Request_MotorTest = FALSE; |
| uint8_t request_VerInfo = FALSE; |
| uint8_t request_ExternalControl = FALSE; |
| uint8_t request_Display = FALSE; |
| uint8_t request_Display1 = FALSE; |
| uint8_t request_DebugData = FALSE; |
| uint8_t request_Data3D = FALSE; |
| uint8_t request_DebugLabel = 255; |
| uint8_t request_PPMChannels = FALSE; |
| uint8_t request_MotorTest = FALSE; |
| uint8_t request_variables = FALSE; |
| uint8_t DisplayLine = 0; |
| volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
| 131,27 → 134,27 |
| "GyroPitch(AC) ", |
| "GyroRoll(AC) ", |
| "GyroYaw(AC) ", |
| "AccPitch ", |
| "AccRoll ", //10 |
| "CorrSumPitch ", |
| "CorrSumRoll ", |
| "Pressure range ", |
| "Pressure A/D ", |
| "Pressure Value ", //15 |
| "Press. rangewidz", |
| "Press. init A/D ", |
| "FACTOR ", |
| "throttleTerm ", |
| "pitchTerm ", //20 |
| "rollTerm ", |
| "yawTerm ", |
| "P Pitch ", |
| "I Pitch ", |
| "P+D Pitch ", //25 |
| "Pitch acc noise ", |
| "Roll acc noise ", |
| "DynamicPitch ", |
| "DynamicRoll ", |
| "AccPitch (angle)", |
| "AccRoll (angle) ", //10 |
| "UBat ", |
| "Pitch Term ", |
| "Roll Term ", |
| "Yaw Term ", |
| "Throttle Term ", //15 |
| "0th O Corr pitch", |
| "0th O Corr roll ", |
| "DriftCompDelta P", |
| "DriftCompDelta R", |
| "ADPitchGyroOffs ", //20 |
| "ADRollGyroOffs ", |
| "M1 ", |
| "M2 ", |
| "M3 ", |
| "M4 ", //25 |
| "ControlYaw ", |
| "Acc Z ", |
| "DriftCompPitch ", |
| "DriftCompRoll ", |
| "Pitch Noise ", //30 |
| "Roll Noise " |
| }; |
| 333,8 → 336,58 |
| } |
| // -------------------------------------------------------------------------- |
| // application example: |
| // SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
| /* |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t txd_bufferIndex = 0; |
| uint8_t *currentBuffer; |
| uint8_t currentBufferIndex; |
| uint16_t lengthOfCurrentBuffer; |
| uint8_t shift = 0; |
| txd_buffer[txd_bufferIndex++] = '#'; // Start character |
| txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[txd_bufferIndex++] = cmd; // Command |
| va_start(ap, numofbuffers); |
| while(numofbuffers) { |
| currentBuffer = va_arg(ap, uint8_t*); |
| lengthOfCurrentBuffer = va_arg(ap, int); |
| currentBufferIndex = 0; |
| // Encode data: 3 bytes of data are encoded into 4 bytes, |
| // where the 2 most significant bits are both 0. |
| while(currentBufferIndex != lengthOfCurrentBuffer) { |
| if (!shift) txd_buffer[txd_bufferIndex] = 0; |
| txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
| txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
| shift += 2; |
| if (shift == 6) { shift=0; txd_bufferIndex++; } |
| currentBufferIndex++; |
| } |
| } |
| // If the number of data bytes was not divisible by 3, stuff |
| // with 0 pseudodata until length is again divisible by 3. |
| if (shift == 2) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex] &= 0b00110000; |
| txd_buffer[++txd_bufferIndex] = 0; |
| shift = 4; |
| } |
| if (shift == 4) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |
| */ |
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t pt = 0; |
| uint8_t a,b,c; |
| uint8_t ptr = 0; |
| 426,33 → 479,29 |
| // -------------------------------------------------------------------------- |
| void usart0_ProcessRxData(void) { |
| // We control the motorTestActive var from here: Count it down. |
| if (motorTestActive) motorTestActive--; |
| // if data in the rxd buffer are not locked immediately return |
| if(!rxd_buffer_locked) return; |
| uint8_t tempchar1, tempchar2; |
| Decode64(); // decode data block in rxd_buffer |
| switch(rxd_buffer[1] - 'a') { |
| case FC_ADDRESS: |
| switch(rxd_buffer[2]) { |
| #ifdef USE_MK3MAG |
| case 'K':// compass value |
| CompassHeading = ((Heading_t *)pRxData)->Heading; |
| CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| compassHeading = ((Heading_t *)pRxData)->Heading; |
| compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| break; |
| #endif |
| case 't':// motor test |
| if(RxDataLen > 20) // |
| { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest)); |
| } |
| else |
| { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, 4); |
| } |
| //Request_MotorTest = TRUE; |
| case 't': // motor test |
| if(RxDataLen > 20) { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest)); |
| } else { |
| memcpy(&motorTest[0], (uint8_t*)pRxData, 4); |
| } |
| motorTestActive = 255; |
| externalControlActive = 255; |
| break; |
| 463,29 → 512,25 |
| break; |
| case 'm':// "Set Mixer Table |
| if(pRxData[0] == EEMIXER_REVISION) |
| { |
| memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
| MixerTable_WriteToEEProm(); |
| while(!txd_complete); // wait for previous frame to be sent |
| tempchar1 = 1; |
| } |
| else |
| { |
| tempchar1 = 0; |
| } |
| if(pRxData[0] == EEMIXER_REVISION) { |
| memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer)); |
| MixerTable_WriteToEEProm(); |
| while(!txd_complete); // wait for previous frame to be sent |
| tempchar1 = 1; |
| } else { |
| tempchar1 = 0; |
| } |
| SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
| break; |
| case 'p': // get PPM channels |
| Request_PPMChannels = TRUE; |
| request_PPMChannels = TRUE; |
| break; |
| case 'q':// request settings |
| if(pRxData[0] == 0xFF) |
| { |
| pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
| } |
| if(pRxData[0] == 0xFF) { |
| pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
| } |
| // limit settings range |
| if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1 |
| else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
| 527,12 → 572,10 |
| } // case FC_ADDRESS: |
| default: // any Slave Address |
| switch(rxd_buffer[2]) |
| { |
| switch(rxd_buffer[2]) { |
| case 'a':// request for labels of the analog debug outputs |
| Request_DebugLabel = pRxData[0]; |
| if(Request_DebugLabel > 31) Request_DebugLabel = 31; |
| request_DebugLabel = pRxData[0]; |
| if(request_DebugLabel > 31) request_DebugLabel = 31; |
| externalControlActive = 255; |
| break; |
| 546,31 → 589,35 |
| externalControlActive = 255; |
| RemoteKeys |= pRxData[0]; |
| if(RemoteKeys) DisplayLine = 0; |
| Request_Display = TRUE; |
| request_Display = TRUE; |
| break; |
| case 'l':// request for display columns |
| externalControlActive = 255; |
| MenuItem = pRxData[0]; |
| Request_Display1 = TRUE; |
| request_Display1 = TRUE; |
| break; |
| case 'v': // request for version and board release |
| Request_VerInfo = TRUE; |
| request_VerInfo = TRUE; |
| break; |
| case 'x': |
| request_variables = TRUE; |
| break; |
| case 'g':// get external control data |
| Request_ExternalControl = TRUE; |
| request_ExternalControl = TRUE; |
| break; |
| case 'd': // request for the debug data |
| DebugData_Interval = (uint16_t) pRxData[0] * 10; |
| if(DebugData_Interval > 0) Request_DebugData = TRUE; |
| if(DebugData_Interval > 0) request_DebugData = TRUE; |
| break; |
| case 'c': // request for the 3D data |
| Data3D_Interval = (uint16_t) pRxData[0] * 10; |
| if(Data3D_Interval > 0) Request_Data3D = TRUE; |
| if(Data3D_Interval > 0) request_Data3D = TRUE; |
| break; |
| default: |
| 602,30 → 649,30 |
| void usart0_TransmitTxData(void) { |
| if(!txd_complete) return; |
| if(Request_VerInfo && txd_complete) { |
| if(request_VerInfo && txd_complete) { |
| SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
| Request_VerInfo = FALSE; |
| request_VerInfo = FALSE; |
| } |
| if(Request_Display && txd_complete) { |
| if(request_Display && txd_complete) { |
| LCD_PrintMenu(); |
| SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
| DisplayLine++; |
| if(DisplayLine >= 4) DisplayLine = 0; |
| Request_Display = FALSE; |
| request_Display = FALSE; |
| } |
| if(Request_Display1 && txd_complete) { |
| if(request_Display1 && txd_complete) { |
| LCD_PrintMenu(); |
| SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
| Request_Display1 = FALSE; |
| request_Display1 = FALSE; |
| } |
| if(Request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
| if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
| uint8_t label[16]; // local sram buffer |
| memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer |
| SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16); |
| Request_DebugLabel = 0xFF; |
| memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
| SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
| request_DebugLabel = 0xFF; |
| } |
| if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
| 633,47 → 680,52 |
| ConfirmFrame = 0; |
| } |
| if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete) { |
| if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) { |
| SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
| DebugData_Timer = SetDelay(DebugData_Interval); |
| Request_DebugData = FALSE; |
| request_DebugData = FALSE; |
| } |
| if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) { |
| if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) { |
| SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
| Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| Data3D_Timer = SetDelay(Data3D_Interval); |
| Request_Data3D = FALSE; |
| request_Data3D = FALSE; |
| } |
| if(Request_ExternalControl && txd_complete) { |
| if(request_ExternalControl && txd_complete) { |
| SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl)); |
| Request_ExternalControl = FALSE; |
| request_ExternalControl = FALSE; |
| } |
| #ifdef USE_MK3MAG |
| if((CheckDelay(Compass_Timer)) && txd_complete) { |
| ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
| ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg |
| ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
| ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
| ToMk3Mag.CalState = CompassCalState; |
| ToMk3Mag.CalState = compassCalState; |
| SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
| // the last state is 5 and should be send only once to avoid multiple flash writing |
| if(CompassCalState > 4) CompassCalState = 0; |
| if(compassCalState > 4) compassCalState = 0; |
| Compass_Timer = SetDelay(99); |
| } |
| #endif |
| if(Request_MotorTest && txd_complete) { |
| if(request_MotorTest && txd_complete) { |
| SendOutData('T', FC_ADDRESS, 0); |
| Request_MotorTest = FALSE; |
| request_MotorTest = FALSE; |
| } |
| if(Request_PPMChannels && txd_complete) { |
| if(request_PPMChannels && txd_complete) { |
| SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in)); |
| Request_PPMChannels = FALSE; |
| request_PPMChannels = FALSE; |
| } |
| if (request_variables && txd_complete) { |
| SendOutData('X', FC_ADDRESS, 1, (uint8_t *)&variables, sizeof(variables)); |
| request_variables = FALSE; |
| } |
| } |
| /branches/dongfang_FC_rewrite/userparams.txt |
|---|
| 1,3 → 1,5 |
| Userparam 3: Projection throttle effect. |
| Userparam 4: Throttle stick D value. Recommended: 12-15. Good fun to fly with. |
| Userparam 5: Filter control. |
| /branches/dongfang_FC_rewrite/yaw-analysis.txt |
|---|
| 0,0 → 1,353 |
| // gyro readings |
| int16_t GyroNick, GyroRoll, GyroYaw; |
| int16_t TrimNick, TrimRoll; |
| int32_t IntegralGyroYaw = 0; |
| int32_t ReadingIntegralGyroYaw = 0; |
| // compass course |
| int16_t CompassHeading = -1; // negative angle indicates invalid data. |
| int16_t CompassCourse = -1; |
| int16_t CompassOffCourse = 0; |
| uint8_t CompassCalState = 0; |
| uint16_t BadCompassHeading = 500; |
| int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t YawGyroDrift; |
| uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
| int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
| int16_t GPSStickNick = 0, GPSStickRoll = 0; |
| // stick values derived by uart inputs |
| int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
| /************************************************************************/ |
| /* Neutral Readings */ |
| /************************************************************************/ |
| void SetNeutral(uint8_t AccAdjustment) |
| { |
| ... |
| AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
| GyroYaw = 0; |
| CompassCourse = CompassHeading; |
| // Inititialize YawGyroIntegral value with current compass heading |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| YawGyroDrift = 0; |
| // Something completely different: Here's why the turn-over's were vars. |
| TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
| TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
| } |
| /************************************************************************/ |
| /* Averaging Measurement Readings */ |
| /************************************************************************/ |
| void Mean(void) |
| { |
| GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
| // Yaw |
| // calculate yaw gyro integral (~ to rotation angle) |
| YawGyroHeading += GyroYaw; |
| ReadingIntegralGyroYaw += GyroYaw; |
| // Coupling fraction |
| if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
| { |
| .... |
| YawGyroHeading += tmp14; |
| if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
| if(abs(GyroYaw > 64)) |
| { |
| if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
| } |
| } |
| // Yaw |
| // limit YawGyroHeading proportional to 0? to 360? |
| if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap |
| if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR; |
| IntegralGyroYaw = ReadingIntegralGyroYaw; |
| } |
| void SetCompassCalState(void) |
| { |
| static uint8_t stick = 1; |
| // if nick is centered or top set stick to zero |
| if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0; |
| // if nick is down trigger to next cal state |
| if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick) |
| { |
| stick = 1; |
| CompassCalState++; |
| if(CompassCalState < 5) Beep(CompassCalState); |
| else BeepTime = 1000; |
| } |
| } |
| /************************************************************************/ |
| /* MotorControl */ |
| /************************************************************************/ |
| void MotorControl(void) |
| { |
| int16_t h, tmp_int; |
| // Mixer Fractions that are combined for Motor Control |
| int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
| int16_t PDPartYaw; |
| static int32_t SetPointYaw = 0; |
| static uint16_t UpdateCompassCourse = 0; |
| Mean(); |
| GRN_ON; |
| if(RC_Quality > 140) |
| { |
| if(ModelIsFlying < 256) |
| { |
| StickYaw = 0; |
| if(ModelIsFlying == 250) |
| { |
| UpdateCompassCourse = 1; |
| ReadingIntegralGyroYaw = 0; |
| SetPointYaw = 0; |
| } |
| } |
| else MKFlags |= (MKFLAG_FLY); // set fly flag |
| ...... |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| // if roll stick is centered and nick stick is down |
| if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
| { |
| CompassCalState = 1; |
| BeepTime = 1000; |
| } |
| (R/C data): |
| // mapping of yaw |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| if (StickYaw > 2) StickYaw-= 2; |
| else if (StickYaw< -2) StickYaw += 2; |
| else StickYaw = 0; |
| } |
| if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
| { |
| StickYaw += ExternControl.Yaw; |
| // disable I part of gyro control feedback |
| if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0; |
| // MeasurementCounter is incremented in the isr of analog.c |
| if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
| { |
| if(ParamSet.DriftComp) |
| { |
| if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
| if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
| } |
| YawGyroDrift = 0; |
| // Yawing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(abs(StickYaw) > 15 ) // yaw stick is activated |
| { |
| BadCompassHeading = 1000; |
| if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) |
| { |
| UpdateCompassCourse = 1; |
| } |
| } |
| // exponential stick sensitivity in yawring rate |
| tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx? |
| tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
| SetPointYaw = tmp_int; |
| // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
| ReadingIntegralGyroYaw -= tmp_int; |
| // limit the effect |
| CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Compass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // compass code is used if Compass option is selected |
| if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
| { |
| int16_t w, v, r,correction, error; |
| if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
| { |
| SetCompassCalState(); |
| #ifdef USE_KILLAGREG |
| MM3_Calibrate(); |
| #endif |
| } |
| else |
| { |
| #ifdef USE_KILLAGREG |
| static uint8_t updCompass = 0; |
| if (!updCompass--) |
| { |
| updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| MM3_Heading(); |
| } |
| #endif |
| // get maximum attitude angle |
| w = abs(IntegralGyroNick / 512); |
| v = abs(IntegralGyroRoll / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
| if(abs(GyroYaw) > 128) // spinning fast |
| { |
| error = 0; |
| } |
| if(!BadCompassHeading && w < 25) |
| { |
| YawGyroDrift += error; |
| if(UpdateCompassCourse) |
| { |
| BeepTime = 200; |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
| UpdateCompassCourse = 0; |
| } |
| } |
| YawGyroHeading += (error * 8) / correction; |
| w = (w * FCParam.CompassYawEffect) / 32; |
| w = FCParam.CompassYawEffect - w; |
| if(w >= 0) |
| { |
| if(!BadCompassHeading) |
| { |
| v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
| // calc course deviation |
| r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * FCParam.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| ReadingIntegralGyroYaw += v; |
| } |
| else |
| { // wait a while |
| BadCompassHeading--; |
| } |
| } |
| else |
| { // ignore compass at extreme attitudes for a while |
| BadCompassHeading = 500; |
| } |
| } |
| } |
| #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
| PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| // limit YawMixFraction |
| if(GasMixFraction > MIN_YAWGAS) |
| { |
| CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2)); |
| } |
| else |
| { |
| CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| tmp_int = ParamSet.GasMax * CONTROL_SCALING; |
| CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
| --------------- |
| anal-lyse: |
| YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; |
| OK |
| SetPointYaw->setPointYaw: |
| - static |
| - Set to 0 at take off |
| - Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw) |
| OK |
| PDPartYaw: |
| - nonstatic nonglobal |
| - = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) |
| / |
| (256L / CONTROL_SCALING) |
| + (int32_t)(IntegralGyroYaw * GyroYawIFactor) |
| / |
| (2 * (44000 / CONTROL_SCALING)); |
| OK |
| GyroYaw: |
| - global |
| - = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro) |
| OK |
| YawGyroHeading->yawGyroHeading: |
| - GyroYaw is summed to it at each iteration |
| - It is wrapped around at <0 and >=360. |
| - It is used -- where???? To adjust CompassCourse... |
| OK |
| IntegralGyroYaw->yawAngle: Deviation of heading from desired??? |
| - GyroYaw is summed to it at each iteration |
| - SetPointYaw is subtracted from it (!) at each iteration. |
| - It is NOT wrapped, but just limited to +/- 50000 |
| - It is corrected / pulled in axis coupling and by the compass. |
| OK (Except that with the compass) |
| BadCompassHeading: Need to update the "CompassCourse". |
| CompassHeading: Opdateret fra mm3mag. |
| CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag. |
| UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR |