/branches/V0.70d CRK HexaLotte/FlightCtrl.aps |
---|
0,0 → 1,0 |
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>21-Mar-2008 18:51:49</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>15-May-2007 11:20:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\Flight-Ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>E:\Daten\Bastelprojekte\Mikrokopter\Software\Soucen\FlightCtrl\V0.68d Code Redesign killagreg\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega644.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><SOURCEFILE>ubx.c</SOURCEFILE><SOURCEFILE>cmps03.c</SOURCEFILE><SOURCEFILE>fifo.c</SOURCEFILE><SOURCEFILE>mm3.c</SOURCEFILE><SOURCEFILE>mymath.c</SOURCEFILE><SOURCEFILE>spi.c</SOURCEFILE><SOURCEFILE>timer2.c</SOURCEFILE><SOURCEFILE>uart1.c</SOURCEFILE><SOURCEFILE>led.c</SOURCEFILE><HEADERFILE>uart.h</HEADERFILE><HEADERFILE>_Settings.h</HEADERFILE><HEADERFILE>analog.h</HEADERFILE><HEADERFILE>fc.h</HEADERFILE><HEADERFILE>gps.h</HEADERFILE><HEADERFILE>main.h</HEADERFILE><HEADERFILE>menu.h</HEADERFILE><HEADERFILE>old_macros.h</HEADERFILE><HEADERFILE>printf_P.h</HEADERFILE><HEADERFILE>rc.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE><HEADERFILE>cmps03.h</HEADERFILE><HEADERFILE>eeprom.h</HEADERFILE><HEADERFILE>fifo.h</HEADERFILE><HEADERFILE>led.h</HEADERFILE><HEADERFILE>mm3.h</HEADERFILE><HEADERFILE>mymath.h</HEADERFILE><HEADERFILE>spi.h</HEADERFILE><HEADERFILE>timer2.h</HEADERFILE><HEADERFILE>uart1.h</HEADERFILE><HEADERFILE>ubx.h</HEADERFILE><OTHERFILE>makefile</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>YES</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE>makefile</EXTERNALMAKEFILE><PART>atmega644</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>Flight-Ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS><OPTION><FILE>GPS.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>analog.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>cmps03.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>eeprom.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fifo.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>led.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>menu.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>mm3.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>mymath.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>printf_P.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>rc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>spi.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer0.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer2.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>twimaster.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart1.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>ubx.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS><LIB>libc.a</LIB><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -Wstrict-prototypes -std=gnu99 -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Programme\Winavr\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Programme\Winavr\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>main.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>uart.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>menu.c</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>timer0.c</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>fc.c</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>fc.h</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>menu.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>TWIMASTER.C</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>twimaster.h</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>uart.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>_Settings.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>analog.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>gps.h</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>main.h</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>old_macros.h</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>printf_P.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>rc.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>timer0.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>makefile</FileName><Status>1</Status></File00018></Files><Workspace><File00000><Position>251 96 720 458</Position><LineCol>0 0</LineCol></File00000><File00001><Position>273 118 734 450</Position><LineCol>0 0</LineCol></File00001><File00002><Position>295 140 756 472</Position><LineCol>0 0</LineCol></File00002><File00003><Position>317 162 778 494</Position><LineCol>0 0</LineCol></File00003><File00004><Position>339 184 800 516</Position><LineCol>0 0</LineCol></File00004><File00005><Position>361 206 822 538</Position><LineCol>0 0</LineCol></File00005><File00006><Position>383 228 844 560</Position><LineCol>0 0</LineCol></File00006><File00007><Position>405 250 866 582</Position><LineCol>0 0</LineCol></File00007><File00008><Position>251 96 712 428</Position><LineCol>0 0</LineCol></File00008><File00009><Position>273 118 734 450</Position><LineCol>0 0</LineCol></File00009><File00010><Position>295 140 756 472</Position><LineCol>0 0</LineCol></File00010><File00011><Position>317 162 778 494</Position><LineCol>0 0</LineCol></File00011><File00012><Position>339 184 800 516</Position><LineCol>0 0</LineCol></File00012><File00013><Position>361 206 822 538</Position><LineCol>0 0</LineCol></File00013><File00014><Position>383 228 844 560</Position><LineCol>0 0</LineCol></File00014><File00015><Position>405 250 866 582</Position><LineCol>0 0</LineCol></File00015><File00016><Position>251 96 712 428</Position><LineCol>0 0</LineCol></File00016><File00017><Position>273 118 734 450</Position><LineCol>0 0</LineCol></File00017><File00018><Position>295 140 756 472</Position><LineCol>0 0</LineCol></File00018></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio> |
/branches/V0.70d CRK HexaLotte/Hex-Files/BootLoader_MEGA644_20MHZ_V0_1.hex |
---|
0,0 → 1,64 |
:10F8000011241FBECFEFD0E1DEBFCDBF11E0A0E0DD |
:10F81000B1E0E0EEFBEF02C005900D92A030B10721 |
:10F82000D9F712E0A0E0B1E001C01D92A230B1070B |
:10F83000E1F70C943B7C0C941D7C0895982F8091EB |
:10F84000C00085FFFCCF9093C60008958091C00052 |
:10F850008823E4F78091C600992708958DE40E94DB |
:10F860001E7C8BE40E941E7C82E40E941E7C8CE441 |
:10F870000E941E7C0895CFEFD0E1DEBFCDBFE0E057 |
:10F88000F0E014915F01772474BE98E10FB6F8940C |
:10F8900090936000109260000FBE7092C5008AE2E3 |
:10F8A0008093C4008091C00082608093C0009093D8 |
:10F8B000C10086E08093C200EF01072D8091C00057 |
:10F8C00087FF0DC0013031F48091C6008A3AB1F152 |
:10F8D00000E005C08091C6008B3109F401E0CE0143 |
:10F8E00021968436910528F11F3F41F0112331F014 |
:10F8F000E0910001F091010109951BC01092C50033 |
:10F900008AE28093C4008AE00E941E7C8DE00E94FF |
:10F910001E7C86E50E941E7C80E30E941E7C8EE297 |
:10F920000E941E7C81E30E941E7C8AE30E941E7C52 |
:10F9300005C080E593EC0197F1F7C0CF0E942E7CC3 |
:10F940000E94267C813611F489E547C1813471F427 |
:10F950000E94267CA82EBB24BA2CAA240E94267CB6 |
:10F960009927A82AB92AB694A79406C1823629F401 |
:10F9700089E50E941E7C81E00BC1823409F083C0BE |
:10F980000E94267C9927D82FCC270E94267C992775 |
:10F99000C82BD92B0E94267C082F12E0812E11E063 |
:10F9A000912EEE24FF2464010894811C911CEC1616 |
:10F9B000FD0618F40E94267C01C08FEFD6018C93BF |
:10F9C0000894E11CF11CBFEFEB16F10461F358F34E |
:10F9D000E4E77E1609F0DEC0063409F03DC0FFEF13 |
:10F9E000AF16F7EFBF0608F033C0DE01A5016627AA |
:10F9F0007727440F551F661F771F6A017B01C2E0FE |
:10FA0000D1E001E08991992729913327322F2227CC |
:10FA1000822B932B0C01FA0100935700E8951124D7 |
:10FA20004E5F5F4F6F4F7F4F129761F785E0F60192 |
:10FA300080935700E89507B600FCFDCF81E18093E5 |
:10FA40005700E89576956795579547955A0194C064 |
:10FA500080E00E941E7C90C0053409F08DC0F50145 |
:10FA60009E01A2E0B1E0E1BD8F2F992782BD8D916B |
:10FA700080BD3196FA9AF99AF999FECF215030401B |
:10FA800091F75F0179C0873609F047C00E94267C54 |
:10FA90009927D82ECC240E94267C9927C82AD92AB7 |
:10FAA0000E94267C863411F5E6016501EE24FF24D0 |
:10FAB000CC0CDD1CEE1CFF1CF60105911491802F6F |
:10FAC0000E941E7C812F99270E941E7C82E090E07C |
:10FAD000A0E0B0E0C80ED91EEA1EFB1E229761F717 |
:10FAE000F694E794D794C79456012ACF853409F049 |
:10FAF00027CF7501E1BC8F2D992782BDF89A089414 |
:10FB0000E11CF11C80B50E941E7C0894C108D1083C |
:10FB1000C114D10479F7570113CF853601F594E765 |
:10FB2000791651F580E090E0A0E0B0E023E0FC0120 |
:10FB300020935700E89507B600FCFDCF80509F4FFB |
:10FB4000AF4FBF4F8F3FE7EF9E07E0E0AE07E0E02B |
:10FB5000BE0768F381E180935700E8950DC08534B6 |
:10FB600069F488E190E02CE00FB6F894A8958093B2 |
:10FB700060000FBE209360008DE02FC08035E1F360 |
:10FB80008C34D1F3803711F483E527C0843721F416 |
:10FB90000E941E7C80E021C0843521F40E94267CD6 |
:10FBA000782EEACF8B3109F4C9CE8A3A09F4C6CE51 |
:10FBB000863529F480E30E941E7C81E30EC08337E2 |
:10FBC00041F489E00E941E7C86E90E941E7C8EE141 |
:10FBD00004C08B3109F4B4CE8FE30E941E7CB0CEFA |
:040000030000F80001 |
:00000001FF |
/branches/V0.70d CRK HexaLotte/Hex-Files/Conrad LEA-4H Config-4Hz.txt |
---|
0,0 → 1,58 |
MON-VER - 0A 04 46 00 35 2E 30 30 20 20 20 20 4A 75 6C 20 31 37 20 32 30 30 36 20 31 35 3A 30 38 3A 31 30 00 01 30 30 30 34 30 30 30 31 00 00 4D 34 48 31 2E 31 43 20 4A 75 6C 20 31 37 20 32 30 30 36 20 31 36 3A 34 32 3A 33 30 00 00 |
CFG-ANT - 06 13 04 00 0B 00 0F 38 |
CFG-DAT - 06 06 02 00 00 00 |
CFG-FXN - 06 0E 24 00 12 00 00 00 C0 D4 01 00 C0 D4 01 00 C0 27 09 00 C0 27 09 00 A0 8C 00 00 40 77 1B 00 00 00 00 00 00 00 00 00 |
CFG-INF - 06 02 08 00 00 00 00 00 00 87 00 00 |
CFG-INF - 06 02 08 00 01 00 00 00 00 00 00 87 |
CFG-INF - 06 02 08 00 03 00 00 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 02 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 04 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 06 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 08 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 11 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 12 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 20 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 21 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 22 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 30 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 31 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 32 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 10 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 11 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 20 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 02 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 06 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 07 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 08 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 09 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 30 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 31 00 00 00 00 |
CFG-MSG - 06 01 06 00 0D 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 0D 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 00 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 01 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 02 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 03 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 04 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 05 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 06 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 07 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 08 00 00 00 01 |
CFG-MSG - 06 01 06 00 F1 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 04 00 00 00 00 |
CFG-NAV2 - 06 1A 28 00 03 00 00 00 03 03 10 02 50 C3 00 00 0F 0A 05 3C 00 01 00 00 FA 00 FA 00 64 00 2C 01 00 00 00 00 00 00 00 00 00 00 00 00 |
CFG-NMEA - 06 17 04 00 00 23 00 02 |
CFG-PRT - 06 00 14 00 01 00 00 00 D0 08 08 00 00 E1 00 00 01 00 01 00 00 00 00 00 |
CFG-PRT - 06 00 14 00 02 00 00 00 D0 08 08 00 00 E1 00 00 01 00 01 00 00 00 00 00 |
CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 03 00 03 00 00 00 00 00 |
CFG-RATE - 06 08 06 00 FA 00 01 00 00 00 |
CFG-RXM - 06 11 02 00 03 00 |
CFG-SBAS - 06 16 08 00 00 00 01 00 00 00 00 00 |
CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 34 03 00 00 00 00 |
/branches/V0.70d CRK HexaLotte/Hex-Files/Conrad LEA-4H Config-5Hz.txt |
---|
0,0 → 1,58 |
MON-VER - 0A 04 46 00 35 2E 30 30 20 20 20 20 4A 75 6C 20 31 37 20 32 30 30 36 20 31 35 3A 30 38 3A 31 30 00 01 30 30 30 34 30 30 30 31 00 00 4D 34 48 31 2E 31 43 20 4A 75 6C 20 31 37 20 32 30 30 36 20 31 36 3A 34 32 3A 33 30 00 00 |
CFG-ANT - 06 13 04 00 0B 00 0F 38 |
CFG-DAT - 06 06 02 00 00 00 |
CFG-FXN - 06 0E 24 00 12 00 00 00 C0 D4 01 00 C0 D4 01 00 C0 27 09 00 C0 27 09 00 A0 8C 00 00 40 77 1B 00 00 00 00 00 00 00 00 00 |
CFG-INF - 06 02 08 00 00 00 00 00 00 87 00 00 |
CFG-INF - 06 02 08 00 01 00 00 00 00 00 00 87 |
CFG-INF - 06 02 08 00 03 00 00 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 02 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 04 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 06 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 08 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 11 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 12 00 01 00 00 |
CFG-MSG - 06 01 06 00 01 20 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 21 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 22 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 30 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 31 00 00 00 00 |
CFG-MSG - 06 01 06 00 01 32 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 10 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 11 00 00 00 00 |
CFG-MSG - 06 01 06 00 02 20 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 02 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 06 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 07 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 08 00 00 00 00 |
CFG-MSG - 06 01 06 00 0A 09 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 30 00 00 00 00 |
CFG-MSG - 06 01 06 00 0B 31 00 00 00 00 |
CFG-MSG - 06 01 06 00 0D 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 0D 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 00 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 01 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 02 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 03 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 04 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 05 00 00 00 01 |
CFG-MSG - 06 01 06 00 F0 06 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 07 00 00 00 00 |
CFG-MSG - 06 01 06 00 F0 08 00 00 00 01 |
CFG-MSG - 06 01 06 00 F1 00 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 01 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 03 00 00 00 00 |
CFG-MSG - 06 01 06 00 F1 04 00 00 00 00 |
CFG-NAV2 - 06 1A 28 00 03 00 00 00 03 03 10 02 50 C3 00 00 0F 0A 05 3C 00 01 00 00 FA 00 FA 00 64 00 2C 01 00 00 00 00 00 00 00 00 00 00 00 00 |
CFG-NMEA - 06 17 04 00 00 23 00 02 |
CFG-PRT - 06 00 14 00 01 00 00 00 D0 08 08 00 00 E1 00 00 01 00 01 00 00 00 00 00 |
CFG-PRT - 06 00 14 00 02 00 00 00 D0 08 08 00 00 E1 00 00 01 00 01 00 00 00 00 00 |
CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 03 00 03 00 00 00 00 00 |
CFG-RATE - 06 08 06 00 64 00 02 00 00 00 |
CFG-RXM - 06 11 02 00 03 00 |
CFG-SBAS - 06 16 08 00 00 00 01 00 00 00 00 00 |
CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 34 03 00 00 00 00 |
/branches/V0.70d CRK HexaLotte/Hex-Files/Readme.txt |
---|
0,0 → 1,145 |
V0.70b Ausgangsversion. |
G.Stobrawa 18.07.2008: |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |
- PPM24 Support für bis zu 12 RC-Kanäle. |
- Support für Kompass CMPS01, MK3MAG oder MM3 |
- 2. Uart wird nun unterstützt (MCU = atmega644p im Makefile) |
- Ausertung des UBX-Protocols an 1. oder 2. Uart |
- einige kleinere Bugfixes |
- GPS-Hold-Funktion hinzugefügt |
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht) |
- Poti3 steuert die GPS Funktionen (Poti3 < 70:GPS inaktiv, 70<=Poti3<160: GPS Hold, 160<=Poti3: GPS Home) |
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter |
- Hardware Configuration: |
- Die PWM des MK3MAG/CMPS03 wird wie bisher standard über den Port PC4 (Pin5 an SV1 der FC) eingelesen. |
- Der MM3 wird wie folgt verbunden. |
FC 1.0/1.1/1.2 Level Shifter MM3 |
SCK (Pin1 SV5) ---> SCK (Pin1) |
MISO (Pin3 SV5) <--- MISO (Pin2) |
MOSI (Pin5 SV5) ---> MOSI (Pin3) |
GND (Pin6 SV5) ---> GND (Pin7 / Pin14) |
PC4 (Pin5 SV1) ---> SS (Pin4) |
PC5 (Pin6 SV1) ---> RESET (Pin6) |
Zusätzlich benötigt der MM3 noch eine 3V oder 3V3 Versorgung an VDD (Pin12). |
- Für das UBLOX-Modul muss noch GPS-GND mit FC-GND (Pin7 SV1) und |
die GPS-TXD Leitung mit FC-RXD (Pin1 SV1) verbunden werden, |
wenn man die FC 1.0 mit dem Atmega644 verwendet. |
Für die FC 1.1/1.2 mit Atmega644p-Bestückung benötigt man FC-RXD1 (Pin3 SV1). |
Zusätzlich benötigt das UBLOX-Modul noch eine 5V-Versorgung die ggf. von |
der FC (an Pin2 SV1) abgegriffen werden kann. |
Wenn die FC gültige Messages vom GPS empfängt, blinkt die rote LED mit 4 Hz. |
- Konfiguration des MK. |
Es sollte ein Haken bei GPS und Kompass gesetzt sein. Wenn nur GPS Aktiviert ist, wird intern immer auch der |
Kompass aktiviert, da er für den GPS-Regler unbedingt notwendig ist. |
- Unter Sonstiges: Kompass-Wirkung etwa auf 50 bis 70. |
- User Parameters: |
Parameter 3 --> Calibration factor for transforming Gyro Integrals to angular degrees (~170) |
Parameter 4 --> Angle between the MM3 Board (Arrow) and the MK head (typical ~180) |
- Zusätzliche akustische Signale: |
Signale die eine Fehlfunktion anzeigen: |
Dauerton: Eine GPS-Funktion ist aktiviert und es werden keine oder |
unvollständige GPS-Daten via UART empfangen. |
Hier ist zu berücksichtigen, dass die folgenden UBX Messages am UBLOX |
aktiviert wurden: NAV-POSLLH, NAV-SOL, NAV-VELNED. |
Fehlt eine dieser Messages wird breits der Dauerton gesetzt. |
5Hz Piepen: Ist eine der GPS Funktion aktiviert, zeigt dass den Empfang valider UBX-Daten, |
wenn noch kein 3D-Satfix vorliegt. Die Pausen zwischen den 4Hz Pieps werden |
immer länger je mehr Satelitten das GPS empfängt. |
Liegt ein 3D Fix vor, so verstummt der Pieper dann endgültig egal wie viele Satelitten |
beteiligt sind (es müssen dafür aber mindesten 4 sein). |
Ist die Comming Home Funktion aktiv (Poti3>160) und konnte keine Home-Position beim |
Einschalten der Motoren eingelernt werden, so ertönt ebenfalls ein 4Hz-Piepen. |
10Hz Piepen: Die Kommunikation zum Kompassmodul ist gestört. (Funktioniert bei beiden Kompassmodulen) |
Der CompassValue in den Debugs vom Koptertool zeigt dann -1 an. |
Signale die eine Aktion bestätigen. |
1s Piepen: Ertönt dieses Signal nach dem Einschalten der Motoren wird die erfolgreiche |
Übernahme der Home-Position bestätigt. |
- Inbetriebnahme: |
Nach dem Flashen des Codes sollte man die Kalibrierung für den MK3MAG/MM3 wiederholen, da diese Daten |
an einer anderen Position im EEProm der FC abgelegt und wieder gelesen werden. |
Das Vorgehen erfolgt beim MM3 und MK3MAG exakt gleich wie hier beschrieben. |
http://www.mikrokopter.de/ucwiki/MK3Mag?highlight=%28mk3mag%29 |
Zur Bestimmung des User Parameters 3 (Umrechnungsfaktor zwischen dem Gyrointegral und dem zugehörigen Neigungswinkel) |
hat sich folgendes Vorgehen bewehrt. |
- Man bestimmt den Wert des Roll- und Nick-Integrals für einen Neigungswinkel von 45° |
über die Ausgaben des Koptertools. |
- Aus diesem Wert kann man den benötigten UserParam3 berechnen. |
UserParam3 = (Nick-Integral(45°)+Roll-Integral(45°))/2*GyroACCFaktor/45°/8. (typisch 170) |
- Nachdem dieser Wert über die Settings des Koptertools im MK abgepeichert ist, sollte sich der CompassValue bei |
Verkippungen nur unwesentlich verändern. |
- Preflight GPS Test: |
Ist der Kopter eingeschaltet, so kann man den GPS-Empfang überprüfen, in dem man Poti3 > 70 setzt. |
Erhält man ein Dauerpiepen besteht keine Kommunikation zum GPS oder eine der benötigten UBX-Messseages fehlt. |
Blinkt die rote LED der FC, so werden grundsätzlich valide Daten vom GPS empfangen. Abhilfe schafft dann die |
Einstellung des GPS-Moduls via USB und u-Center sodass die UBX Messages NAV-POSLLH, NAV-SOL, NAV-VELNED |
mit 57600 baud auf zum Target 1 (RS232) gesendet werden. Es empfiehlt sich alle anderen Sendungen inkl. NMEA |
abzuschalten, da diese sonst durch den UBX-Parser auf der FC laufen und sinnlose Rechenzeit beanspruchen. |
Am einfachsten geht das mit dem Konfigurationsfile "Conrad LEA-4H Config.txt". Dazu verbindet man das |
UBLOX-Modul via USB mit dem PC und started das u-Center. Dan wählt man im Menü: |
"Tools->GPS Configuration" dieses File aus und klickt auf den Button "File >> GPS". |
Hat man diese Hürde genommen (Dauerpiepsen ist Weg), wird wahrscheinlich ein 5Hz piepen zu hören sein. |
Dieses zeigt den korrekten Empfang aller UBX-Messages an. Je mehr Satelitten empfangen werden, desto länger werden |
die Pausen zwischen den Pieps, bis sie schleißlich ganz verschwinden, was einen 3D-Satfix signalisiert. |
Die GPS-Funktionen können ggf. nun wieder abgeschaltet werden (Poti3<70). |
- Setzen der Home Position: |
Startet man die Motoren, so ertönt in diesem Moment eine 1s-langer Piep, der die erfolgreiche Übernahme |
der Home-Position bestätigt. Dies passiert immer, auch wenn keine GPS-Funktion aktiviert wurde (Poti3<70). |
Es kann jedoch sein, dass das Signal nicht ertönt. In diesem Fall konnte keine Home Position |
abgespeichert werden, da zu diesem Zeitpunkt kein 3D-Fix vorlag. Aktiviert man dann später während des Fluges Comming Home (Poti3>160) wird dieser Zustand mit einem 5Hz Piepen angezeigt. Der Kopter versucht dann |
wenigstens ein Position Hold. Schaltet man die Motoren ab, so wird auch die ggf. |
gespeicherte Home-Position gelöscht. |
- Dynamishes Position Hold: |
Ist 70 < Poti3 < 160 so ist die dynamische Position Hold Funktion aktiv. Solange sich der Nick- & Roll-Stick |
in Zentralposiotion befinden (genauer Auschlag < 20 Counts) wird die laterale (XY)-Position durch das GPS- |
Signal geregelt. Dabei wird versucht die Abweichung der aktuellen GPS-Position von der zum Zeitpunkt |
der letzen Nick/Roll-Stickbewegung gespeicherten Position zu minimieren. (Siehe dazu auch D- & P-Parameter |
des GPS-Reglers). Dadurch kann man den MK zu einer bestimmten Position steuern und behält diese bei. |
Es kann vorkommen, das bei extremer Timmerstellung an Nick und Roll der Funke der |
zugehörige Stickwert bereits eine Auslenkung > 20 zeigt. In diesem Fall wird das Position Hold |
leider nicht aktiviert. Durch die automatische Abschaltung des GPS-Reglers für den Fall einer manuellen |
Bedienung kann man jederzeit in das Flugverhalten eingreifen. |
- Comming Home: |
Ist Poti3 > 160 so ist die Comming Home Funktion aktiv. Das verhalten ist analog dem Position Hold, |
jedoch ist das Regelziel nicht die zuletzt gespeichwerte GPS-Position sondern die zum Zeitpunkt des |
Einschaltens der MOtoren gespeicherte Home Position. Eine manualle Bedienung von Nick- und Roll-Stick |
unterbricht auch hier sofort den GPS-Regler und die Steuerung erfolgt manuell. |
/branches/V0.70d CRK HexaLotte/Hex-Files/WasIstWas.txt |
---|
0,0 → 1,25 |
+++++++++++++++++++++++++ |
+ Flight-Ctrl: |
+++++++++++++++++++++++++ |
BootLoader_MEGA644_20MHZ_V0_1.hex |
Der Bootloader wird per ISP eingespielt |
Der Bootloader nur dann eingespielt werden, wenn noch nie ein Bootloader eingespielt wurde! |
Danach können Softwareupdates seriell eingespielt werden. |
Aktuelle Firmware |
Wird per serielle Schnittstelle (durch den Bootloader) eingespielt |
Flight-Ctrl SW >= 0.69 benötigt das Kopter-Tool 1.50 |
Flight-Ctrl_MEGA644_KILLAGREG_V0_69k.hex für Atmega644 mit Extension Board für MM3 und Conrad-GPS at Uart1 |
Flight-Ctrl_MEGA644_NAVICTRL_V0_69k.hex für Atmega644 mit NaviCtrl |
Flight-Ctrl_MEGA644_MK3MAG_V0_69k.hex für Atmega644 mit Support für den CMPS03/MK3MAG und Conrad-GPS at Uart 1 |
Flight-Ctrl_MEGA644p_KILLAGREG_V0_69k.hex für Atmega644p mit Extension Board für MM3 und Conrad-GPS at Uart 2 |
Flight-Ctrl_MEGA644p_NAVICTRL_V0_69k.hex für Atmega644p mit NaviCtrl |
Flight-Ctrl_MEGA644p_MK3MAG_V0_69k.hex für Atmega644p mit Support für den CMPS03/MK3MAG und Conrad-GPS at Uart 2 |
Die Firmware läuft sowohl auf der FC 1.0 als auch auf der FC 1.1/1.2 |
/branches/V0.70d CRK HexaLotte/Kopter-Tool/MikroKopter-Tool.exe |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/branches/V0.70d CRK HexaLotte/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/V0.70d CRK HexaLotte/_Settings.h |
---|
0,0 → 1,33 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Abstimmung |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define ACC_AMPLIFY 12 |
#define FACTOR_I 0.0001 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debug-Interface |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define SIO_DEBUG 1 // Soll der Debugger aktiviert sein? |
#define MIN_DEBUG_INTERVALL 250 // in diesem Intervall werden Degugdaten ohne Aufforderung gesendet |
// +++++++++++++++++++++++++++++++ |
// + Getestete Settings: |
// +++++++++++++++++++++++++++++++ |
// Setting: Kamera |
// Stick_P:3 |
// Stick_D:0 |
// Gyro_P: 175 |
// Gyro_I: 175 |
// Ki_Anteil: 10 |
// +++++++++++++++++++++++++++++++ |
// + Getestete Settings: |
// +++++++++++++++++++++++++++++++ |
// Setting: Normal |
// Stick_P:2 |
// Stick_D:8 |
// Gyro_P: 80 |
// Gyro_I: 150 |
// Ki_Anteil: 5 |
/branches/V0.70d CRK HexaLotte/analog.c |
---|
0,0 → 1,235 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "analog.h" |
#include "main.h" |
#include "timer0.h" |
#include "fc.h" |
#include "printf_P.h" |
#include "eeprom.h" |
#include "twimaster.h" |
volatile int16_t Current_AccZ = 0; |
volatile int16_t UBat = 100; |
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115; |
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0; |
volatile int32_t AirPressure = 32000; |
volatile uint8_t average_pressure = 0; |
volatile int16_t StartAirPressure; |
volatile uint16_t ReadingAirPressure = 1023; |
uint8_t PressureSensorOffset; |
volatile int16_t HeightD = 0; |
volatile uint16_t MeasurementCounter = 0; |
/*****************************************************/ |
/* Initialize Analog Digital Converter */ |
/*****************************************************/ |
void ADC_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) | 0x00; |
//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 = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); |
//Set ADC Control and Status Register B |
//Trigger Source to Free Running Mode |
ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
// Enable AD conversion |
ADC_Enable(); |
// restore global interrupt flags |
SREG = sreg; |
} |
void SearchAirPressureOffset(void) |
{ |
uint8_t off; |
off = GetParamByte(PID_PRESSURE_OFFSET); |
if(off > 20) off -= 10; |
OCR0A = off; |
Delay_ms_Mess(100); |
if(ReadingAirPressure < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms_Mess(50); |
printf("."); |
if(ReadingAirPressure < 900) break; |
} |
SetParamByte(PID_PRESSURE_OFFSET, off); |
PressureSensorOffset = off; |
Delay_ms_Mess(300); |
} |
void SearchGyroOffset(void) |
{ |
uint8_t i, ready = 0; |
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
for(i = 140; i != 0; i--) |
{ |
if(ready == 3 && i > 10) i = 9; |
ready = 0; |
if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++; |
if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++; |
if(AdValueGyrYaw < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw > 1030) AnalogOffsetYaw++ ; else ready++; |
twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset |
I2C_Start(); // initiate data transmission |
if(AnalogOffsetNick < 10) { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;}; |
if(AnalogOffsetRoll < 10) { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;}; |
if(AnalogOffsetYaw < 10) { GyroDefectYaw = 1; AnalogOffsetYaw = 10;}; if(AnalogOffsetYaw > 245) { GyroDefectYaw = 1; AnalogOffsetYaw = 245;}; |
while(twi_state); // wait for end of data transmission |
average_pressure = 0; |
ADC_Enable(); |
while(average_pressure == 0); |
if(i < 10) Delay_ms_Mess(10); |
} |
Delay_ms_Mess(70); |
} |
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
// runs at 156.25 kHz or 6.4 µs |
// if after (70.4µs) all 11 states are processed the interrupt is disabled |
// and the update of further ads is stopped |
// The routine changes the ADC input muxer running |
// thru the state machine by the following order. |
// state 0: ch0 (yaw gyro) |
// state 1: ch1 (roll gyro) |
// state 2: ch2 (nick gyro) |
// state 3: ch4 (battery voltage -> UBat) |
// state 4: ch6 (acc y -> Current_AccY) |
// state 5: ch7 (acc x -> Current_AccX) |
// state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw) |
// state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll) |
// state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick) |
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading) |
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure) |
ISR(ADC_vect) |
{ |
static uint8_t adc_channel = 0, state = 0; |
static uint16_t yaw1, roll1, nick1; |
static int16_t tmpAirPressure = 0; |
// disable further AD conversion |
ADC_Disable(); |
// state machine |
switch(state++) |
{ |
case 0: |
yaw1 = ADC; // get Gyro Yaw Voltage 1st sample |
adc_channel = 1; // set next channel to ADC1 = ROLL GYRO |
MeasurementCounter++; // increment total measurement counter |
break; |
case 1: |
roll1 = ADC; // get Gyro Roll Voltage 1st sample |
adc_channel = 2; // set next channel to ADC2 = NICK GYRO |
break; |
case 2: |
nick1 = ADC; // get Gyro Nick Voltage 1st sample |
adc_channel = 4; // set next channel to ADC4 = UBAT |
break; |
case 3: |
// get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3 |
UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value |
adc_channel = 6; // set next channel to ADC6 = ACC_Y |
break; |
case 4: |
AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction |
adc_channel = 7; // set next channel to ADC7 = ACC_X |
break; |
case 5: |
AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction |
adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
break; |
case 6: |
// average over two samples to create current AdValueGyrYaw |
if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
break; |
case 7: |
// average over two samples to create current ADValueGyrRoll |
if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2; |
else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
adc_channel = 2; // set next channel to ADC2 = NICK GYRO |
break; |
case 8: |
// average over two samples to create current ADValueNick |
if(BoardRelease == 10) AdValueGyrNick = (ADC + nick1) / 2; |
else AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1 |
adc_channel = 5; // set next channel to ADC5 = ACC_Z |
break; |
case 9: |
// get z acceleration |
AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction |
AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4; |
if(AdValueAccTop > 1) |
{ |
if(NeutralAccZ < 750) |
{ |
NeutralAccZ += 0.02; |
if(Model_Is_Flying < 500) NeutralAccZ += 0.1; |
} |
} |
else if(AdValueAccTop < -1) |
{ |
if(NeutralAccZ > 550) |
{ |
NeutralAccZ-= 0.02; |
if(Model_Is_Flying < 500) NeutralAccZ -= 0.1; |
} |
} |
Current_AccZ = ADC; |
Reading_Integral_Top += AdValueAccTop; // Integrieren |
Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen |
adc_channel = 3; // set next channel to ADC3 = air pressure |
break; |
case 10: |
tmpAirPressure += ADC; // sum vadc values |
if(++average_pressure >= 5) // if 5 values are summerized for averaging |
{ |
ReadingAirPressure = ADC; // update measured air pressure |
HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(StartAirPressure - tmpAirPressure - ReadingHeight))/8; // D-Part = CurrentValue - OldValue |
AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
ReadingHeight = StartAirPressure - AirPressure; |
average_pressure = 0; // reset air pressure measurement counter |
tmpAirPressure = 0; |
} |
adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
state = 0; // reset state machine |
break; |
default: |
adc_channel = 0; |
state = 0; |
break; |
} |
// set adc muxer to next adc_channel |
ADMUX = (ADMUX & 0xE0) | adc_channel; |
// after full cycle stop further interrupts |
if(state != 0) ADC_Enable(); |
} |
/branches/V0.70d CRK HexaLotte/analog.h |
---|
0,0 → 1,30 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
#include <inttypes.h> |
extern volatile int16_t UBat; |
extern volatile int16_t AdValueGyrNick, AdValueGyrRoll, AdValueGyrYaw; |
extern uint8_t AnalogOffsetNick, AnalogOffsetRoll, AnalogOffsetYaw; |
extern volatile int16_t AdValueAccRoll, AdValueAccNick, AdValueAccTop; |
extern volatile int16_t Current_AccZ; |
extern volatile int32_t AirPressure; |
extern volatile uint16_t MeasurementCounter; |
extern uint8_t PressureSensorOffset; |
extern volatile int16_t HeightD; |
extern volatile uint16_t ReadingAirPressure; |
extern volatile int16_t StartAirPressure; |
void SearchAirPressureOffset(void); |
void SearchGyroOffset(void); |
void ADC_Init(void); |
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define ADC_Disable() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define ADC_Enable() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
#endif //_ANALOG_H |
/branches/V0.70d CRK HexaLotte/eeprom.c |
---|
0,0 → 1,360 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Konstanten |
// + 0-250 -> normale Werte |
// + 251 -> Poti1 |
// + 252 -> Poti2 |
// + 253 -> Poti3 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#include <avr/eeprom.h> |
#include <string.h> |
#include "eeprom.h" |
#include "printf_P.h" |
#include "led.h" |
// byte array in eeprom |
uint8_t EEPromArray[E2END+1] EEMEM; |
paramset_t ParamSet; |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void ParamSet_DefaultSet1(void) // sport |
{ |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.Height_MinGas = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 4; // Wert : 0-50 |
ParamSet.Stick_P = 15; // Wert : 1-24 |
ParamSet.Stick_D = 30; // Wert : 0-250 |
ParamSet.Yaw_P = 12; // Wert : 1-20 |
ParamSet.Gas_Min = 8; // Wert : 0-32 |
ParamSet.Gas_Max = 230; // Wert : 33-250 |
ParamSet.GyroAccFactor = 30; // Wert : 1-64 |
ParamSet.CompassYawEffect = 128; // Wert : 0-250 |
ParamSet.Gyro_P = 80; // Wert : 0-250 |
ParamSet.Gyro_I = 150; // Wert : 0-250 |
ParamSet.LowVoltageWarning = 94; // Wert : 0-250 |
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
ParamSet.EmergencyGasDuration = 30; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
ParamSet.UfoArrangement = 0; // X oder + Formation |
ParamSet.I_Factor = 32; |
ParamSet.UserParam1 = 0; //zur freien Verwendung |
ParamSet.UserParam2 = 0; //zur freien Verwendung |
ParamSet.UserParam3 = 0; //zur freien Verwendung |
ParamSet.UserParam4 = 0; //zur freien Verwendung |
ParamSet.UserParam5 = 0; // zur freien Verwendung |
ParamSet.UserParam6 = 0; // zur freien Verwendung |
ParamSet.UserParam7 = 0; // zur freien Verwendung |
ParamSet.UserParam8 = 0; // zur freien Verwendung |
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
ParamSet.LoopHysteresis = 50; |
ParamSet.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
ParamSet.Yaw_PosFeedback = 90; |
ParamSet.Yaw_NegFeedback = 5; |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
ParamSet.GyroAccTrim = 16; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 100; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 15; |
ParamSet.J17Timing = 15; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
memcpy(ParamSet.Name, "Sport\0",6); |
} |
/***************************************************/ |
/* Default Values for parameter set 2 */ |
/***************************************************/ |
void ParamSet_DefaultSet2(void) // normal |
{ |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.Height_MinGas = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 3; // Wert : 0-50 |
ParamSet.Stick_P = 12; // Wert : 1-24 |
ParamSet.Stick_D = 16; // Wert : 0-250 |
ParamSet.Yaw_P = 6; // Wert : 1-20 |
ParamSet.Gas_Min = 8; // Wert : 0-32 |
ParamSet.Gas_Max = 230; // Wert : 33-250 |
ParamSet.GyroAccFactor = 30; // Wert : 1-64 |
ParamSet.CompassYawEffect = 128; // Wert : 0-250 |
ParamSet.Gyro_P = 80; // Wert : 0-250 |
ParamSet.Gyro_I = 120; // Wert : 0-250 |
ParamSet.LowVoltageWarning = 94; // Wert : 0-250 |
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
ParamSet.EmergencyGasDuration = 30; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
ParamSet.UfoArrangement = 0; // X oder + Formation |
ParamSet.I_Factor = 32; |
ParamSet.UserParam1 = 0; // zur freien Verwendung |
ParamSet.UserParam2 = 0; // zur freien Verwendung |
ParamSet.UserParam3 = 0; // zur freien Verwendung |
ParamSet.UserParam4 = 0; // zur freien Verwendung |
ParamSet.UserParam5 = 0; // zur freien Verwendung |
ParamSet.UserParam6 = 0; // zur freien Verwendung |
ParamSet.UserParam7 = 0; // zur freien Verwendung |
ParamSet.UserParam8 = 0; // zur freien Verwendung |
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
ParamSet.LoopHysteresis = 50; |
ParamSet.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
ParamSet.Yaw_PosFeedback = 90; // Faktor, mit dem Yaw die Achsen Roll und Nick verkoppelt |
ParamSet.Yaw_NegFeedback = 5; |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
ParamSet.GyroAccTrim = 32; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 75; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 20; |
ParamSet.J17Timing = 20; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
memcpy(ParamSet.Name, "Normal\0", 7); |
} |
/***************************************************/ |
/* Default Values for parameter set 3 */ |
/***************************************************/ |
void ParamSet_DefaultSet3(void) // beginner |
{ |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.Height_MinGas = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 3; // Wert : 0-50 |
ParamSet.Stick_P = 8; // Wert : 1-24 |
ParamSet.Stick_D = 16; // Wert : 0-250 |
ParamSet.Yaw_P = 6; // Wert : 1-20 |
ParamSet.Gas_Min = 8; // Wert : 0-32 |
ParamSet.Gas_Max = 230; // Wert : 33-250 |
ParamSet.GyroAccFactor = 30; // Wert : 1-64 |
ParamSet.CompassYawEffect = 128; // Wert : 0-250 |
ParamSet.Gyro_P = 100; // Wert : 0-250 |
ParamSet.Gyro_I = 120; // Wert : 0-250 |
ParamSet.LowVoltageWarning = 94; // Wert : 0-250 |
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
ParamSet.EmergencyGasDuration = 20; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
ParamSet.UfoArrangement = 0; // X oder + Formation |
ParamSet.I_Factor = 16; |
ParamSet.UserParam1 = 0; // zur freien Verwendung |
ParamSet.UserParam2 = 0; // zur freien Verwendung |
ParamSet.UserParam3 = 0; // zur freien Verwendung |
ParamSet.UserParam4 = 0; // zur freien Verwendung |
ParamSet.UserParam5 = 0; // zur freien Verwendung |
ParamSet.UserParam6 = 0; // zur freien Verwendung |
ParamSet.UserParam7 = 0; // zur freien Verwendung |
ParamSet.UserParam8 = 0; // zur freien Verwendung |
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
ParamSet.ServoNickRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
ParamSet.LoopHysteresis = 50; |
ParamSet.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
ParamSet.Yaw_PosFeedback = 90; // Faktor, mit dem Yaw die Achsen Roll und Nick verkoppelt |
ParamSet.Yaw_NegFeedback = 5; |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
ParamSet.GyroAccTrim = 32; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 50; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 30; |
ParamSet.J17Timing = 30; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
memcpy(ParamSet.Name, "Beginner\0", 9); |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as byte */ |
/***************************************************/ |
uint8_t GetParamByte(uint8_t param_id) |
{ |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
/* Write Parameter to EEPROM as byte */ |
/***************************************************/ |
void SetParamByte(uint8_t param_id, uint8_t value) |
{ |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
uint16_t GetParamWord(uint8_t param_id) |
{ |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
/* Write Parameter to EEPROM as word */ |
/***************************************************/ |
void SetParamWord(uint8_t param_id, uint16_t value) |
{ |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
// number [0..5] |
void ParamSet_ReadFromEEProm(uint8_t setnumber) |
{ |
if (setnumber > 5) setnumber = 5; |
eeprom_read_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN); |
LED_Init(); |
} |
/***************************************************/ |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
// number [0..5] |
void ParamSet_WriteToEEProm(uint8_t setnumber) |
{ |
if(setnumber > 5) setnumber = 5; |
eeprom_write_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN); |
// set this parameter set to active set |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
LED_Init(); |
} |
/***************************************************/ |
/* Get active parameter set */ |
/***************************************************/ |
uint8_t GetActiveParamSet(void) |
{ |
uint8_t setnumber; |
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]); |
if(setnumber > 5) |
{ |
setnumber = 2; |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
} |
return(setnumber); |
} |
/***************************************************/ |
/* Set active parameter set */ |
/***************************************************/ |
void SetActiveParamSet(uint8_t setnumber) |
{ |
if(setnumber > 5) setnumber = 5; |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
} |
/***************************************************/ |
/* Initialize EEPROM Parameter Sets */ |
/***************************************************/ |
void ParamSet_Init(void) |
{ |
// version check |
if(eeprom_read_byte(&EEPromArray[PID_VERSION]) != EEPARAM_VERSION) |
{ |
// if version check faild |
printf("\n\rInit. EEPROM: Generating Default-Parameter..."); |
ParamSet_DefaultSet1(); // Fill ParamSet Structure to default parameter set 1 (Sport) |
// fill all 5 parameter settings with set 1 except otherwise defined |
for (unsigned char i=0;i<6;i++) |
{ |
if(i==2) ParamSet_DefaultSet2(); // Kamera |
if(i==3) ParamSet_DefaultSet3(); // Beginner |
if(i>3) ParamSet_DefaultSet2(); // Kamera |
ParamSet_WriteToEEProm(i); |
} |
// default-Setting is parameter set 3 |
SetParamByte(PID_ACTIVE_SET, 3); |
// update version info |
SetParamByte(PID_VERSION, EEPARAM_VERSION); |
} |
// read active parameter set to ParamSet stucture |
ParamSet_ReadFromEEProm(GetParamByte(PID_ACTIVE_SET)); |
printf("\n\rUsing Parameter Set %d", GetParamByte(PID_ACTIVE_SET)); |
} |
/branches/V0.70d CRK HexaLotte/eeprom.h |
---|
0,0 → 1,136 |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <inttypes.h> |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_VERSION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_NICK 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_Z 8 // word |
#ifdef USE_KILLAGREG |
#define PID_MM3_X_OFF 10 // byte |
#define PID_MM3_Y_OFF 11 // byte |
#define PID_MM3_Z_OFF 12 // byte |
#define PID_MM3_X_RANGE 13 // word |
#define PID_MM3_Y_RANGE 15 // word |
#define PID_MM3_Z_RANGE 17 // word |
#endif |
#define EEPROM_ADR_PARAMSET_BEGIN 100 |
// bit mask for mk_param_struct.GlobalConfig |
#define CFG_HEIGHT_CONTROL 0x01 |
#define CFG_HEIGHT_SWITCH 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_COMPASS_ACTIVE 0x08 |
#define CFG_COMPASS_FIX 0x10 |
#define CFG_GPS_ACTIVE 0x20 |
#define CFG_AXIS_COUPLING_ACTIVE 0x40 |
#define CFG_ROTARY_RATE_LIMITER 0x80 |
// bit mask for mk_param_struct.LoopConfig |
#define CFG_LOOP_UP 0x01 |
#define CFG_LOOP_DOWN 0x02 |
#define CFG_LOOP_LEFT 0x04 |
#define CFG_LOOP_RIGHT 0x08 |
// defines for lookup mk_param_struct.ChannelAssignment |
#define CH_NICK 0 |
#define CH_ROLL 1 |
#define CH_GAS 2 |
#define CH_YAW 3 |
#define CH_POTI1 4 |
#define CH_POTI2 5 |
#define CH_POTI3 6 |
#define CH_POTI4 7 |
#define EEPARAM_VERSION 71 // is count up, if EE_Paramater stucture has changed (compatibility) |
// 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 Height_MinGas; // Wert : 0-100 |
uint8_t Height_D; // Wert : 0-250 |
uint8_t MaxHeight; // Wert : 0-32 |
uint8_t Height_P; // Wert : 0-32 |
uint8_t Height_Gain; // Wert : 0-50 |
uint8_t Height_ACC_Effect; // Wert : 0-250 |
uint8_t Stick_P; // Wert : 1-6 |
uint8_t Stick_D; // Wert : 0-64 |
uint8_t Yaw_P; // Wert : 1-20 |
uint8_t Gas_Min; // Wert : 0-32 |
uint8_t Gas_Max; // Wert : 33-250 |
uint8_t GyroAccFactor; // Wert : 1-64 |
uint8_t CompassYawEffect; // Wert : 0-32 |
uint8_t Gyro_P; // Wert : 10-250 |
uint8_t Gyro_I; // Wert : 0-250 |
uint8_t LowVoltageWarning; // Wert : 0-250 |
uint8_t EmergencyGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
uint8_t EmergencyGasDuration; // Wert : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
uint8_t UfoArrangement; // x oder + Formation |
uint8_t I_Factor; // Wert : 0-250 |
uint8_t UserParam1; // Wert : 0-250 |
uint8_t UserParam2; // Wert : 0-250 |
uint8_t UserParam3; // Wert : 0-250 |
uint8_t UserParam4; // Wert : 0-250 |
uint8_t ServoNickControl; // Wert : 0-250 // Stellung des Servos |
uint8_t ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
uint8_t ServoNickMin; // Wert : 0-250 // Anschlag |
uint8_t ServoNickMax; // Wert : 0-250 // Anschlag |
uint8_t ServoNickRefresh; // |
uint8_t LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
uint8_t LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
uint8_t LoopHysteresis; // Wert: 0-250 Hysterese für Stickausschlag |
uint8_t Yaw_PosFeedback; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
uint8_t Yaw_NegFeedback; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick Gegenkoppelt (NickRollGegenkopplung) |
uint8_t AngleTurnOverNick; // Wert: 0-250 180°-Punkt |
uint8_t AngleTurnOverRoll; // Wert: 0-250 180°-Punkt |
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
uint8_t DriftComp; |
uint8_t DynamicStability; |
uint8_t UserParam5; // Wert : 0-250 |
uint8_t UserParam6; // Wert : 0-250 |
uint8_t UserParam7; // Wert : 0-250 |
uint8_t UserParam8; // Wert : 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; |
uint8_t NaviGpsP; |
uint8_t NaviGpsI; |
uint8_t NaviGpsD; |
uint8_t NaviGpsACC; |
uint8_t NaviGpsMinSat; |
uint8_t NaviStickThreshold; |
uint8_t ExternalControl; // for serial Control |
uint8_t LoopConfig; // see upper defines for bitcoding |
uint8_t ServoNickCompInvert; // Wert : 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 ParamSet; |
extern void ParamSet_Init(void); |
extern void ParamSet_ReadFromEEProm(uint8_t setnumber); |
extern void ParamSet_WriteToEEProm(uint8_t setnumber); |
extern uint8_t GetActiveParamSet(void); |
extern void SetActiveParamSet(uint8_t setnumber); |
extern uint8_t GetParamByte(uint8_t param_id); |
extern void SetParamByte(uint8_t param_id, uint8_t value); |
extern uint16_t GetParamWord(uint8_t param_id); |
extern void SetParamWord(uint8_t param_id, uint16_t value); |
#endif //_EEPROM_H |
/branches/V0.70d CRK HexaLotte/fc.c |
---|
0,0 → 1,1435 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "eeprom.h" |
#include "timer0.h" |
#include "_Settings.h" |
#include "analog.h" |
#include "fc.h" |
#include "uart.h" |
#include "rc.h" |
#include "twimaster.h" |
#include "timer2.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#include "gps.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#include "gps.h" |
#endif |
#include "led.h" |
// gyro readings |
int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw; |
// gyro neutral readings |
int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0; |
int16_t StartNeutralRoll = 0, StartNeutralNick = 0; |
// mean accelerations |
int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop; |
// neutral acceleration readings |
volatile int16_t NeutralAccX=0, NeutralAccY=0; |
volatile float NeutralAccZ = 0; |
// attitude gyro integrals |
int32_t IntegralNick = 0,IntegralNick2 = 0; |
int32_t IntegralRoll = 0,IntegralRoll2 = 0; |
int32_t IntegralYaw = 0; |
int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0; |
int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0; |
int32_t Reading_IntegralGyroYaw = 0; |
int32_t MeanIntegralNick; |
int32_t MeanIntegralRoll; |
// attitude acceleration integrals |
int32_t IntegralAccNick = 0, IntegralAccRoll = 0; |
volatile int32_t Reading_Integral_Top = 0; |
// compass course |
volatile int16_t CompassHeading = -1; // negative angle indicates invalid data. |
volatile int16_t CompassCourse = -1; |
volatile int16_t CompassOffCourse = 0; |
volatile uint8_t CompassCalState = 0; |
uint8_t FunnelCourse = 0; |
uint16_t BadCompassHeading = 500; |
int32_t YawGyroHeading; |
int16_t YawGyroDrift; |
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0; |
// MK flags |
uint16_t Model_Is_Flying = 0; |
volatile uint8_t MKFlags = 0; |
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L; |
float Gyro_P_Factor; |
float Gyro_I_Factor; |
int16_t DiffNick, DiffRoll; |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
// setpoints for motors |
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
// stick values derived by rc channels readings |
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
int16_t GPS_Nick = 0, GPS_Roll = 0; |
int16_t MaxStickNick = 0, MaxStickRoll = 0; |
// stick values derived by uart inputs |
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
int16_t ReadingHeight = 0; |
int16_t SetPointHeight = 0; |
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0; |
float Ki = FACTOR_I; |
uint8_t Looping_Nick = 0, Looping_Roll = 0; |
uint8_t Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0; |
fc_param_t FCParam = {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100}; |
/************************************************************************/ |
/* Creates numbeeps beeps at the speaker */ |
/************************************************************************/ |
void Beep(uint8_t numbeeps) |
{ |
while(numbeeps--) |
{ |
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren! |
BeepTime = 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 funktion if motors are running |
} |
} |
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(void) |
{ |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralYaw = 0; |
FCParam.Yaw_PosFeedback = 0; |
FCParam.Yaw_NegFeedback = 0; |
CalibMean(); |
Delay_ms_Mess(100); |
CalibMean(); |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated? |
{ |
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
} |
AdNeutralNick = AdValueGyrNick; |
AdNeutralRoll = AdValueGyrRoll; |
AdNeutralYaw = AdValueGyrYaw; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
if(GetParamWord(PID_ACC_NICK) > 1023) |
{ |
NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mean_AccNick) / ACC_AMPLIFY; |
NeutralAccZ = Current_AccZ; |
} |
else |
{ |
NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z); |
} |
Reading_IntegralGyroNick = 0; |
Reading_IntegralGyroNick2 = 0; |
Reading_IntegralGyroRoll = 0; |
Reading_IntegralGyroRoll2 = 0; |
Reading_IntegralGyroYaw = 0; |
Reading_GyroNick = 0; |
Reading_GyroRoll = 0; |
Reading_GyroYaw = 0; |
Delay_ms_Mess(100); |
StartAirPressure = AirPressure; |
HeightD = 0; |
Reading_Integral_Top = 0; |
CompassCourse = CompassHeading; |
BeepTime = 50; |
TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
ExternHeightValue = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR; |
YawGyroDrift = 0; |
MKFlags |= MKFLAG_CALIBRATE; |
} |
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
{ |
static int32_t tmpl,tmpl2; |
// Get offset corrected gyro readings (~ to angular velocity) |
Reading_GyroYaw = AdNeutralYaw - AdValueGyrYaw; |
Reading_GyroRoll = AdValueGyrRoll - AdNeutralRoll; |
Reading_GyroNick = AdValueGyrNick - AdNeutralNick; |
// Acceleration Sensor |
// sliding average sensor readings |
Mean_AccNick = ((int32_t)Mean_AccNick * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 2L; |
Mean_AccRoll = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L; |
Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L; |
// sum sensor readings for later averaging |
IntegralAccNick += ACC_AMPLIFY * AdValueAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
NaviAccNick += AdValueAccNick; |
NaviAccRoll += AdValueAccRoll; |
NaviCntAcc++; |
// Yaw |
// calculate yaw gyro integral (~ to rotation angle) |
Reading_IntegralGyroYaw += Reading_GyroYaw; |
YawGyroHeading += Reading_GyroYaw; |
if(YawGyroHeading >= (360L * YAW_GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * YAW_GYRO_DEG_FACTOR; // 360° Wrap |
if(YawGyroHeading < 0) YawGyroHeading += 360L * YAW_GYRO_DEG_FACTOR; |
// Coupling fraction |
if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L; |
tmpl *= FCParam.Yaw_PosFeedback; |
tmpl /= 4096L; |
tmpl2 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L; |
tmpl2 *= FCParam.Yaw_PosFeedback; |
tmpl2 /= 4096L; |
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
} |
else tmpl = tmpl2 = 0; |
// Roll |
Reading_GyroRoll += tmpl; |
Reading_GyroRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; |
Reading_IntegralGyroRoll2 += Reading_GyroRoll; |
Reading_IntegralGyroRoll += Reading_GyroRoll - AttitudeCorrectionRoll; |
if(Reading_IntegralGyroRoll > TurnOver180Roll) |
{ |
Reading_IntegralGyroRoll = -(TurnOver180Roll - 10000L); |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
} |
if(Reading_IntegralGyroRoll < -TurnOver180Roll) |
{ |
Reading_IntegralGyroRoll = (TurnOver180Roll - 10000L); |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
} |
if(AdValueGyrRoll < 15) Reading_GyroRoll = -1000; |
if(AdValueGyrRoll < 7) Reading_GyroRoll = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyrRoll > 1010) Reading_GyroRoll = +1000; |
if(AdValueGyrRoll > 1017) Reading_GyroRoll = +2000; |
} |
else |
{ |
if(AdValueGyrRoll > 2020) Reading_GyroRoll = +1000; |
if(AdValueGyrRoll > 2034) Reading_GyroRoll = +2000; |
} |
// Nick |
Reading_GyroNick -= tmpl2; |
Reading_GyroNick -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
Reading_IntegralGyroNick2 += Reading_GyroNick; |
Reading_IntegralGyroNick += Reading_GyroNick - AttitudeCorrectionNick; |
if(Reading_IntegralGyroNick > TurnOver180Nick) |
{ |
Reading_IntegralGyroNick = -(TurnOver180Nick - 25000L); |
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick; |
} |
if(Reading_IntegralGyroNick < -TurnOver180Nick) |
{ |
Reading_IntegralGyroNick = (TurnOver180Nick - 25000L); |
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick; |
} |
if(AdValueGyrNick < 15) Reading_GyroNick = -1000; |
if(AdValueGyrNick < 7) Reading_GyroNick = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyrNick > 1010) Reading_GyroNick = +1000; |
if(AdValueGyrNick > 1017) Reading_GyroNick = +2000; |
} |
else |
{ |
if(AdValueGyrNick > 2020) Reading_GyroNick = +1000; |
if(AdValueGyrNick > 2034) Reading_GyroNick = +2000; |
} |
// start ADC again to capture measurement values for the next loop |
ADC_Enable(); |
IntegralYaw = Reading_IntegralGyroYaw; |
IntegralNick = Reading_IntegralGyroNick; |
IntegralRoll = Reading_IntegralGyroRoll; |
IntegralNick2 = Reading_IntegralGyroNick2; |
IntegralRoll2 = Reading_IntegralGyroRoll2; |
if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Nick && !Looping_Roll) |
{ |
if(Reading_GyroNick > 200) Reading_GyroNick += 4 * (Reading_GyroNick - 200); |
else if(Reading_GyroNick < -200) Reading_GyroNick += 4 * (Reading_GyroNick + 200); |
if(Reading_GyroRoll > 200) Reading_GyroRoll += 4 * (Reading_GyroRoll - 200); |
else if(Reading_GyroRoll < -200) Reading_GyroRoll += 4 * (Reading_GyroRoll + 200); |
} |
} |
/************************************************************************/ |
/* Averaging Measurement Readings for Calibration */ |
/************************************************************************/ |
void CalibMean(void) |
{ |
if(BoardRelease >= 13) SearchGyroOffset(); |
// stop ADC to avoid changing values during calculation |
ADC_Disable(); |
Reading_GyroNick = AdValueGyrNick; |
Reading_GyroRoll = AdValueGyrRoll; |
Reading_GyroYaw = AdValueGyrYaw; |
Mean_AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick; |
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
Mean_AccTop = (int32_t)AdValueAccTop; |
// start ADC (enables internal trigger so that the ISR in analog.c |
// updates the readings once) |
ADC_Enable(); |
TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
} |
/************************************************************************/ |
/* Transmit Motor Data via I2C */ |
/************************************************************************/ |
void SendMotorData(void) |
{ |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) |
{ |
Motor_Rear = 0; |
Motor_Front = 0; |
Motor_Right = 0; |
Motor_Left = 0; |
if(MotorTest[0]) Motor_Front = MotorTest[0]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off |
} |
DebugOut.Analog[12] = Motor_Front; |
DebugOut.Analog[13] = Motor_Rear; |
DebugOut.Analog[14] = Motor_Left; |
DebugOut.Analog[15] = Motor_Right; |
//Start I2C Interrupt Mode |
twi_state = TWI_STATE_MOTOR_TX; |
I2C_Start(); |
} |
/************************************************************************/ |
/* Maps the parameter to poti values */ |
/************************************************************************/ |
void ParameterMapping(void) |
{ |
if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok |
// else the last updated values are used |
{ |
//update poti values by rc-signals |
#define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
#define CHK_POTI(b,a) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a;} |
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight); |
CHK_POTI_MM(FCParam.Height_D,ParamSet.Height_D,0,100); |
CHK_POTI_MM(FCParam.Height_P,ParamSet.Height_P,0,100); |
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect); |
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect); |
CHK_POTI_MM(FCParam.Gyro_P,ParamSet.Gyro_P,10,255); |
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I); |
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor); |
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1); |
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2); |
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3); |
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4); |
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5); |
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6); |
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7); |
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8); |
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl); |
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit); |
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback); |
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability); |
CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255); |
CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255); |
CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl); |
CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain); |
CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP); |
CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI); |
CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD); |
CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC); |
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl); |
Ki = (float) FCParam.I_Factor * FACTOR_I; |
} |
} |
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 MotorValue, pd_result, h, tmp_int; |
int16_t YawMixFraction, GasMixFraction; |
static int32_t SumNick = 0, SumRoll = 0; |
static int32_t SetPointYaw = 0; |
static int32_t IntegralErrorNick = 0; |
static int32_t IntegralErrorRoll = 0; |
static uint16_t RcLostTimer; |
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
static uint8_t HeightControlActive = 0; |
static int16_t HeightControlGas = 0; |
static int8_t TimerDebugOut = 0; |
static uint16_t UpdateCompassCourse = 0; |
static int32_t CorrectionNick, CorrectionRoll; |
Mean(); |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// determine gas value |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixFraction = StickGas; |
if(GasMixFraction < ParamSet.Gas_Min + 10) GasMixFraction = ParamSet.Gas_Min + 10; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality < 120) // the rc-frame signal is not reveived or noisy |
{ |
if(!PcAccess) // if also no PC-Access via UART |
{ |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0x0C00; |
} |
} |
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
else // rc lost countdown finished |
{ |
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData() |
} |
RED_ON; // set red led |
if(Model_Is_Flying > 1000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing |
// set neutral rc inputs |
PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData() |
} // eof RC_Quality < 120 |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is good |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality > 140) |
{ |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
// reset emergency timer |
RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++; |
} |
if(Model_Is_Flying < 256) |
{ |
SumNick = 0; |
SumRoll = 0; |
StickYaw = 0; |
if(Model_Is_Flying == 250) |
{ |
UpdateCompassCourse = 1; |
Reading_IntegralGyroYaw = 0; |
SetPointYaw = 0; |
} |
} |
else MKFlags |= (MKFLAG_FLY); // set fly flag |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
//PPM24-Extension |
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
//limit poti values |
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
//PPM24-Extension |
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
// if motors are off and the gas stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
// and if the yaw stick is in the leftmost position |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calibrate the neutral readings of all attitude sensors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
{ |
// gas/yaw joystick is top left |
// _________ |
// |x | |
// | | |
// | | |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
Model_Is_Flying = 0; |
// check roll/nick stick position |
// if nick stick is top or roll stick is left or right --> change parameter setting |
// according to roll/nick stick position |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
{ |
uint8_t setting = 1; // default |
// nick/roll joystick |
// _________ |
// |2 3 4| |
// | | |
// |1 5| |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
// roll stick leftmost and nick stick centered --> setting 1 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1; |
// roll stick leftmost and nick stick topmost --> setting 2 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2; |
// roll stick centered an nick stick topmost --> setting 3 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3; |
// roll stick rightmost and nick stick topmost --> setting 4 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4; |
// roll stick rightmost and nick stick centered --> setting 5 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5; |
// update active parameter set in eeprom |
SetActiveParamSet(setting); |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Beep(GetActiveParamSet()); |
} |
else |
{ |
if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)) |
{ |
// if roll stick is centered and nick stick is down |
if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 20 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
{ |
// nick/roll joystick |
// _________ |
// | | |
// | | |
// | | |
// | | |
// | x | |
// ¯¯¯¯¯¯¯¯¯ |
// enable calibration state of compass |
CompassCalState = 1; |
BeepTime = 1000; |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Beep(GetActiveParamSet()); |
} |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Beep(GetActiveParamSet()); |
} |
} |
} |
} |
// and if the yaw stick is in the rightmost position |
// save the ACC neutral setting to eeprom |
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
SetParamWord(PID_ACC_NICK, 0xFFFF); // make value invalid |
Model_Is_Flying = 0; |
SetNeutral(); |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
Beep(GetActiveParamSet()); |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// gas stick is down |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// and yaw stick is rightmost --> start motors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 200; // do not repeat if once executed |
Model_Is_Flying = 1; |
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START |
SetPointYaw = 0; |
Reading_IntegralGyroYaw = 0; |
Reading_IntegralGyroNick = 0; |
Reading_IntegralGyroRoll = 0; |
Reading_IntegralGyroNick2 = IntegralNick; |
Reading_IntegralGyroRoll2 = IntegralRoll; |
SumNick = 0; |
SumRoll = 0; |
} |
} |
else delay_startmotors = 0; // reset delay timer if sticks are not in this position |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// and yaw stick is leftmost --> stop motors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
{ |
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_stopmotors = 200; // do not repeat if once executed |
Model_Is_Flying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
} |
// remapping of paameters only if the signal rc-sigbnal conditions are good |
} // eof RC_Quality > 150 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// new values from RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC |
{ |
int tmp_int; |
ParameterMapping(); // remapping params (online poti replacement) |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4; |
StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D; |
StickNick -= (GPS_Nick); |
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
StickRoll -= (GPS_Roll); |
// direct mapping of yaw and gas |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
// update gyro control loop factors |
Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / (256.0 / STICK_GAIN); |
Gyro_I_Factor = ((float) FCParam.Gyro_I) / (44000 / STICK_GAIN); |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Digital Control via DubWise |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define KEY_VALUE (FCParam.ExternalControl * 4) // step width |
if(DubWiseKeys[1]) BeepTime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; |
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; |
else tmp_int = 0; |
ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8; |
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; |
else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; |
else tmp_int = 0; |
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
if(DubWiseKeys[0] & 2) ExternHeightValue++; |
if(DubWiseKeys[0] & 16) ExternHeightValue--; |
StickNick += (STICK_GAIN * ExternStickNick) / 8; |
StickRoll += (STICK_GAIN * ExternStickRoll) / 8; |
StickYaw += (STICK_GAIN * ExternStickYaw); |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analog control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
{ |
StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P; |
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
StickYaw += ExternControl.Yaw; |
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
if(StickGas < 0) StickGas = 0; |
// disable I part of gyro control feedback |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
// avoid negative scaling factors |
if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
if(Gyro_I_Factor < 0) Gyro_I_Factor = 0; |
// update max stick positions for nick and roll |
if(abs(StickNick / STICK_GAIN) > MaxStickNick) |
{ |
MaxStickNick = abs(StickNick)/STICK_GAIN; |
if(MaxStickNick > 100) MaxStickNick = 100; |
} |
else MaxStickNick--; |
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll) |
{ |
MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
if(MaxStickRoll > 100) MaxStickRoll = 100; |
} |
else MaxStickRoll--; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
else |
{ |
if(Looping_Left) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
else |
{ |
if(Looping_Right) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
else |
{ |
if(Looping_Top) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
else |
{ |
if(Looping_Down) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
} |
} |
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
if(Looping_Top || Looping_Down) {Looping_Nick = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Nick = 0; |
} // End of new RC-Values or Emergency Landing |
if(Looping_Roll || Looping_Nick) |
{ |
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// set all inputs to save values |
if(MKFlags & MKFLAG_EMERGENCY_LANDING) |
{ |
StickYaw = 0; |
StickNick = 0; |
StickRoll = 0; |
Gyro_P_Factor = (float) 100 / (256.0 / STICK_GAIN); |
Gyro_I_Factor = (float) 120 / (44000 / STICK_GAIN); |
Looping_Roll = 0; |
Looping_Nick = 0; |
MaxStickNick = 0; |
MaxStickRoll = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Trim Gyro-Integrals to ACC-Signals |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define BALANCE_NUMBER 256L |
// sum for averaging |
MeanIntegralNick += IntegralNick; |
MeanIntegralRoll += IntegralRoll; |
if(Looping_Nick || Looping_Roll) // if looping in any direction |
{ |
// reset averaging for acc and gyro integral as well as gyro integral acc correction |
MeasurementCounter = 0; |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
MeanIntegralNick = 0; |
MeanIntegralRoll = 0; |
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick; |
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll; |
AttitudeCorrectionNick = 0; |
AttitudeCorrectionRoll = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction |
{ |
int32_t tmp_long, tmp_long2; |
// determine the deviation of gyro integral from averaged acceleration sensor |
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick); |
tmp_long /= 16; |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll); |
tmp_long2 /= 16; |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
#define BALANCE 32 |
// limit correction effect |
if(tmp_long > BALANCE) tmp_long = BALANCE; |
if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE; |
// correct current readings |
Reading_IntegralGyroNick -= tmp_long; |
Reading_IntegralGyroRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int16_t cnt = 0; |
static int8_t last_n_p, last_n_n, last_r_p, last_r_n; |
static int32_t MeanIntegralNick_old, MeanIntegralRoll_old; |
// if not lopping in any direction (this should be alwais the case, |
// because the Measurement counter is reset to 0 if looping in any direction is active.) |
if(!Looping_Nick && !Looping_Roll && !FunnelCourse) |
{ |
// Calculate mean value of the gyro integrals |
MeanIntegralNick /= BALANCE_NUMBER; |
MeanIntegralRoll /= BALANCE_NUMBER; |
// Calculate mean of the acceleration values |
IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER; |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick); |
CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim; |
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll); |
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim; |
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER; |
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) |
{ |
AttitudeCorrectionNick /= 2; |
AttitudeCorrectionRoll /= 2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor) |
IntegralErrorNick = IntegralNick2 - IntegralNick; |
Reading_IntegralGyroNick2 -= IntegralErrorNick; |
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor) |
IntegralErrorRoll = IntegralRoll2 - IntegralRoll; |
Reading_IntegralGyroRoll2 -= IntegralErrorRoll; |
if(YawGyroDrift > BALANCE_NUMBER/2) AdNeutralYaw++; |
if(YawGyroDrift < -BALANCE_NUMBER/2) AdNeutralYaw--; |
YawGyroDrift = 0; |
/* |
DebugOut.Analog[17] = IntegralAccNick / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
DebugOut.Analog[19] = IntegralErrorNick;// / 26; |
DebugOut.Analog[20] = IntegralErrorRoll;// / 26; |
DebugOut.Analog[21] = MeanIntegralNick / 26; |
DebugOut.Analog[22] = MeanIntegralRoll / 26; |
//DebugOut.Analog[28] = CorrectionNick; |
DebugOut.Analog[29] = CorrectionRoll; |
DebugOut.Analog[30] = AttitudeCorrectionRoll * 10; |
*/ |
#define ERROR_LIMIT (BALANCE_NUMBER * 4) |
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16) |
#define MOVEMENT_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorNick) / 4096; |
CorrectionNick = 0; |
if(labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) |
{ |
if(IntegralErrorNick > ERROR_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2; |
CorrectionNick = IntegralErrorNick / 8; |
if(CorrectionNick > 5000) CorrectionNick = 5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_p = 1; |
} |
else last_n_p = 0; |
if(IntegralErrorNick < -ERROR_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2; |
CorrectionNick = IntegralErrorNick / 8; |
if(CorrectionNick < -5000) CorrectionNick = -5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_n = 1; |
} |
else last_n_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
// correct Gyro Offsets |
if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt; |
if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt; |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralErrorNick) / 4096; |
CorrectionRoll = 0; |
if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) |
{ |
if(IntegralErrorRoll > ERROR_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll > 5000) CorrectionRoll = 5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_p = 1; |
} |
else last_r_p = 0; |
if(IntegralErrorRoll < -ERROR_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2; |
CorrectionRoll = IntegralErrorRoll / 8; |
if(CorrectionRoll < -5000) CorrectionRoll = -5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_n = 1; |
} |
else last_r_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
// correct Gyro Offsets |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt; |
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt; |
/* |
DebugOut.Analog[27] = CorrectionRoll; |
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
*/ |
} |
else // looping is active |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
FunnelCourse = 0; |
} |
// if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction |
if(!Gyro_I_Factor) |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralNick_old = MeanIntegralNick; |
MeanIntegralRoll_old = MeanIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// reset variables used for averaging |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
MeanIntegralNick = 0; |
MeanIntegralRoll = 0; |
MeasurementCounter = 0; |
} // end of averaging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
SetPointYaw = tmp_int; |
// trimm drift of Reading_IntegralGyroYaw with SetPointYaw(StickYaw) |
Reading_IntegralGyroYaw -= tmp_int; |
// limit the effect |
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000; |
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// compass code is used if Compass option is selected |
if((ParamSet.GlobalConfig & CFG_COMPASS_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(IntegralNick / 512); |
v = abs(IntegralRoll / 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 / YAW_GYRO_DEG_FACTOR)) % 360) - 180; |
if(!BadCompassHeading && w < 25) |
{ |
YawGyroDrift += error; |
if(UpdateCompassCourse) |
{ |
BeepTime = 200; |
CompassCourse = (YawGyroHeading / YAW_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 / YAW_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; |
Reading_IntegralGyroYaw += v; |
} |
else |
{ // wait a while |
BadCompassHeading--; |
} |
} |
else |
{ // ignore compass at extreme attitudes for a while |
BadCompassHeading = 500; |
} |
} |
} |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
{ |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} |
else |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
#endif |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = IntegralNick / ParamSet.GyroAccFactor; |
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFactor; |
DebugOut.Analog[2] = Mean_AccNick; |
DebugOut.Analog[3] = Mean_AccRoll; |
DebugOut.Analog[4] = Reading_GyroYaw; |
DebugOut.Analog[5] = ReadingHeight; |
DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
//DebugOut.Analog[16] = Mean_AccTop; |
DebugOut.Analog[20] = ServoValue; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |
DebugOut.Analog[19] = motor_rx[3]; |
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; |
DebugOut.Analog[20] /= 14; |
DebugOut.Analog[21] = motor_rx[4]; |
DebugOut.Analog[22] = motor_rx[5]; |
DebugOut.Analog[23] = motor_rx[6]; |
DebugOut.Analog[24] = motor_rx[7]; |
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
DebugOut.Analog[9] = Reading_GyroNick; |
DebugOut.Analog[9] = SetPointHeight; |
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128; |
DebugOut.Analog[10] = FCParam.Gyro_I; |
DebugOut.Analog[10] = ParamSet.Gyro_I; |
DebugOut.Analog[9] = CompassOffCourse; |
DebugOut.Analog[10] = GasMixFraction; |
DebugOut.Analog[3] = HeightD * 32; |
DebugOut.Analog[4] = HeightControlGas; |
*/ |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Looping_Nick) Reading_GyroNick = Reading_GyroNick * Gyro_P_Factor; |
else Reading_GyroNick = IntegralNick * Gyro_I_Factor + Reading_GyroNick * Gyro_P_Factor; |
if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor; |
else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor; |
Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
DebugOut.Analog[21] = Reading_GyroNick; |
DebugOut.Analog[22] = Reading_GyroRoll; |
// limit control feedback |
#define MAX_SENSOR (4096 * STICK_GAIN) |
if(Reading_GyroNick > MAX_SENSOR) Reading_GyroNick = MAX_SENSOR; |
if(Reading_GyroNick < -MAX_SENSOR) Reading_GyroNick = -MAX_SENSOR; |
if(Reading_GyroRoll > MAX_SENSOR) Reading_GyroRoll = MAX_SENSOR; |
if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR; |
if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Height Control |
// The height control algorithm reduces the gas but does not increase the gas. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixFraction *= STICK_GAIN; |
// If height control is activated and no emergency landing is active |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) ) |
{ |
int tmp_int; |
// if height control is activated by an rc channel |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
if(FCParam.MaxHeight < 50) |
{ |
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading |
HeightControlActive = 0; // disable height control |
} |
else HeightControlActive = 1; // enable height control |
} |
else // no switchable height control |
{ |
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20; |
HeightControlActive = 1; |
} |
// get current height |
h = ReadingHeight; |
// if current height is above the setpoint reduce gas |
if((h > SetPointHeight) && HeightControlActive) |
{ |
// GasMixFraction - HightDeviation * P - HeightChange * D - ACCTop * DACC |
// height difference -> P control part |
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / (16 / STICK_GAIN); |
h = GasMixFraction - h; // reduce gas |
// height gradient --> D control part |
//h -= (HeightD * FCParam.Height_D) / (8 / STICK_GAIN); // D control part |
h -= (HeightD) / (8 / STICK_GAIN); // D control part |
// acceleration sensor effect |
tmp_int = ((Reading_Integral_Top / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
if(tmp_int > 70 * STICK_GAIN) tmp_int = 70 * STICK_GAIN; |
else if(tmp_int < -(70 * STICK_GAIN)) tmp_int = -(70 * STICK_GAIN); |
h -= tmp_int; |
// update height control gas |
HeightControlGas = (HeightControlGas*15 + h) / 16; |
// limit gas reduction |
if(HeightControlGas < ParamSet.Height_MinGas * STICK_GAIN) |
{ |
if(GasMixFraction >= ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = ParamSet.Height_MinGas * STICK_GAIN; |
// allows landing also if gas stick is reduced below min gas on height control |
if(GasMixFraction < ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = GasMixFraction; |
} |
// limit gas to stick setting |
if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction; |
GasMixFraction = HeightControlGas; |
} |
} |
// limit gas to parameter setting |
if(GasMixFraction > (ParamSet.Gas_Max - 20) * STICK_GAIN) GasMixFraction = (ParamSet.Gas_Max - 20) * STICK_GAIN; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mixer and PI-Controller |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMixFraction; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yaw-Fraction |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
YawMixFraction = Reading_GyroYaw - SetPointYaw * STICK_GAIN; // yaw controller |
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value |
// limit YawMixFraction |
if(GasMixFraction > MIN_YAWGAS) |
{ |
if(YawMixFraction > (GasMixFraction / 2)) YawMixFraction = GasMixFraction / 2; |
if(YawMixFraction < -(GasMixFraction / 2)) YawMixFraction = -(GasMixFraction / 2); |
} |
else |
{ |
if(YawMixFraction > (MIN_YAWGAS / 2)) YawMixFraction = MIN_YAWGAS / 2; |
if(YawMixFraction < -(MIN_YAWGAS / 2)) YawMixFraction = -(MIN_YAWGAS / 2); |
} |
tmp_int = ParamSet.Gas_Max * STICK_GAIN; |
if(YawMixFraction > ((tmp_int - GasMixFraction))) YawMixFraction = ((tmp_int - GasMixFraction)); |
if(YawMixFraction < -((tmp_int - GasMixFraction))) YawMixFraction = -((tmp_int - GasMixFraction)); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = Reading_GyroNick - StickNick; // get difference |
if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control |
else SumNick += DiffNick; // I-part for head holding |
if(SumNick > (STICK_GAIN * 16000L)) SumNick = (STICK_GAIN * 16000L); |
if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L); |
pd_result = DiffNick + Ki * SumNick; // PI-controller for nick |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
// Motor Front |
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Front = MotorValue; |
// Motor Rear |
MotorValue = GasMixFraction - pd_result + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Rear = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = Reading_GyroRoll - StickRoll; // get difference |
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control |
else SumRoll += DiffRoll; // I-part for head holding |
if(SumRoll > (STICK_GAIN * 16000L)) SumRoll = (STICK_GAIN * 16000L); |
if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L); |
pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
// Motor Left |
MotorValue = GasMixFraction + pd_result - YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Left = MotorValue; |
// Motor Right |
MotorValue = GasMixFraction - pd_result - YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Right = MotorValue; |
} |
/branches/V0.70d CRK HexaLotte/fc.h |
---|
0,0 → 1,129 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
#ifndef _FC_H |
#define _FC_H |
#include <inttypes.h> |
#define YAW_GYRO_DEG_FACTOR 1550L // Factor between Yaw Gyro Integral and HeadingAngle in deg |
#define STICK_GAIN 4 |
typedef struct |
{ |
uint8_t Height_D; |
uint8_t MaxHeight; |
uint8_t Height_P; |
uint8_t Height_ACC_Effect; |
uint8_t CompassYawEffect; |
uint8_t Gyro_P; |
uint8_t Gyro_I; |
uint8_t Yaw_P; |
uint8_t I_Factor; |
uint8_t UserParam1; |
uint8_t UserParam2; |
uint8_t UserParam3; |
uint8_t UserParam4; |
uint8_t UserParam5; |
uint8_t UserParam6; |
uint8_t UserParam7; |
uint8_t UserParam8; |
uint8_t ServoNickControl; |
uint8_t LoopGasLimit; |
uint8_t Yaw_PosFeedback; |
uint8_t Yaw_NegFeedback; |
uint8_t DynamicStability; |
uint8_t ExternalControl; |
uint8_t J16Timing; |
uint8_t J17Timing; |
uint8_t NaviGpsModeControl; |
uint8_t NaviGpsGain; |
uint8_t NaviGpsP; |
uint8_t NaviGpsI; |
uint8_t NaviGpsD; |
uint8_t NaviGpsACC; |
} fc_param_t; |
extern fc_param_t FCParam; |
// attitude |
extern int32_t IntegralNick, IntegralRoll, IntegralYaw; |
extern int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw; |
// offsets |
extern int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw; |
extern volatile int16_t NeutralAccX, NeutralAccY; |
extern volatile float NeutralAccZ; |
extern volatile int32_t Reading_Integral_Top; // calculated in analog.c |
// compass navigation |
extern volatile int16_t CompassHeading; |
extern volatile int16_t CompassCourse; |
extern volatile int16_t CompassOffCourse; |
extern volatile uint8_t CompassCalState; |
extern int32_t YawGyroHeading; |
extern int16_t YawGyroHeadingInDeg; |
// hight control |
extern int ReadingHeight; |
extern int SetPointHeight; |
// mean accelerations |
extern int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop; |
// acceleration send to navi board |
extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
// looping params |
extern long TurnOver180Nick, TurnOver180Roll; |
// external control |
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
void MotorControl(void); |
void SendMotorData(void); |
void CalibMean(void); |
void Mean(void); |
void SetNeutral(void); |
void Beep(uint8_t numbeeps); |
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
// setpoints for motors |
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr |
// current stick values |
extern int16_t StickNick; |
extern int16_t StickRoll; |
extern int16_t StickYaw; |
extern int16_t GPS_Nick; |
extern int16_t GPS_Roll; |
// current stick elongations |
extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
extern uint16_t Model_Is_Flying; |
// MKFlags |
#define MKFLAG_MOTOR_RUN 0x01 |
#define MKFLAG_FLY 0x02 |
#define MKFLAG_CALIBRATE 0x04 |
#define MKFLAG_START 0x08 |
#define MKFLAG_EMERGENCY_LANDING 0x10 |
#define MKFLAG_RESERVE1 0x20 |
#define MKFLAG_RESERVE2 0x40 |
#define MKFLAG_RESERVE3 0x80 |
volatile extern uint8_t MKFlags; |
#endif //_FC_H |
/branches/V0.70d CRK HexaLotte/fifo.c |
---|
0,0 → 1,28 |
#include "fifo.h" |
void fifo_init (fifo_t *f, uint8_t *buffer, const uint8_t size) |
{ |
f->count = 0; |
f->pread = f->pwrite = buffer; |
f->read2end = f->write2end = f->size = size; |
} |
uint8_t fifo_put (fifo_t *f, const uint8_t data) |
{ |
return _inline_fifo_put (f, data); |
} |
uint8_t fifo_get_wait (fifo_t *f) |
{ |
while (!f->count); |
return _inline_fifo_get (f); |
} |
int16_t fifo_get_nowait (fifo_t *f) |
{ |
if (!f->count) return -1; |
return (int16_t) _inline_fifo_get (f); |
} |
/branches/V0.70d CRK HexaLotte/fifo.h |
---|
0,0 → 1,98 |
#ifndef _FIFO_H_ |
#define _FIFO_H_ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
// the fifo object |
typedef struct |
{ |
uint8_t volatile count; // # number of characters in FIFO |
uint8_t size; // buffer size |
uint8_t *pread; // read pointer |
uint8_t *pwrite; // write pointer |
uint8_t read2end, write2end; // number of characters for buffer overflow for read/write pointers |
} fifo_t; |
/* |
The initialization of the FIFO sets the read/write pointers etc.. |
The FIFO uses the buffer 'buf' which byte length must 'size'. |
*/ |
extern void fifo_init (fifo_t*, uint8_t* buf, const uint8_t size); |
/* |
Puts a byte into the FIFO. Returns 1 on success ans 0 in case of FIFO overflow. |
*/ |
extern uint8_t fifo_put (fifo_t*, const uint8_t data); |
/* |
Get the next byte out of the FIFO. If the FIFO is empty the function blocks |
until the next byte is put into the FIFO. |
*/ |
extern uint8_t fifo_get_wait (fifo_t*); |
/* |
Get the next byte from the FIFO as int. Returns -1 if the FIFO is empty. |
*/ |
extern int16_t fifo_get_nowait (fifo_t*); |
/* |
The same like fifo_put |
*/ |
static inline uint8_t _inline_fifo_put (fifo_t *f, const uint8_t data) |
{ |
if (f->count >= f->size) |
return 0; |
uint8_t * pwrite = f->pwrite; |
*(pwrite++) = data; |
uint8_t write2end = f->write2end; |
if (--write2end == 0) |
{ |
write2end = f->size; |
pwrite -= write2end; |
} |
f->write2end = write2end; |
f->pwrite = pwrite; |
uint8_t sreg = SREG; |
cli(); |
f->count++; |
SREG = sreg; |
return 1; |
} |
/* |
Get the next byte from FIFO. Before this functionis called |
it must be checked that there is a byte in the FIFO to get. |
*/ |
static inline uint8_t _inline_fifo_get (fifo_t *f) |
{ |
uint8_t *pread = f->pread; |
uint8_t data = *(pread++); |
uint8_t read2end = f->read2end; |
if (--read2end == 0) |
{ |
read2end = f->size; |
pread -= read2end; |
} |
f->pread = pread; |
f->read2end = read2end; |
uint8_t sreg = SREG; |
cli(); |
f->count--; |
SREG = sreg; |
return data; |
} |
#endif /* _FIFO_H_ */ |
/branches/V0.70d CRK HexaLotte/flight.pnproj |
---|
0,0 → 1,0 |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="fc.c"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="GPS.c"></File><File path="gps.h"></File><File path="License.txt"></File><File path="eeprom.c"></File><File path="spi.h"></File><File path="spi.c"></File></Project> |
/branches/V0.70d CRK HexaLotte/flight.pnps |
---|
0,0 → 1,0 |
<pd><ViewState><e p="Flight-Ctrl" x="true"></e></ViewState></pd> |
/branches/V0.70d CRK HexaLotte/gps.c |
---|
0,0 → 1,413 |
#include <inttypes.h> |
#include <stdlib.h> |
#include "fc.h" |
#include "ubx.h" |
#include "mymath.h" |
#include "timer0.h" |
#include "uart.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 3000 |
#define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
#define GPS_I_LIMIT 156 // limit for the position error integral |
#define GPS_D_LIMIT 625 |
#define GPS_P_LIMIT 312 |
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 (FCParam.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
else if(FCParam.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 >= ParamSet.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[ParamSet.ChannelAssignment[CH_NICK]]) < ParamSet.NaviStickThreshold) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < ParamSet.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) |
{ |
GPS_Nick = 0; |
GPS_Roll = 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; |
} |
GPSPosDevIntegral_North += GPSPosDev_North; |
GPSPosDevIntegral_East += GPSPosDev_East; |
GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
//Calculate PID-components of the controller |
// D-Part |
D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/256; |
D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/256; |
GPS_LimitXY(&D_North, &D_East, GPS_D_LIMIT); |
// P-Part |
P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/1024; |
P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/1024; |
GPS_LimitXY(&P_North, &P_East, GPS_P_LIMIT); |
// I-Part |
I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192; |
GPS_LimitXY(&I_North, &I_East, GPS_I_LIMIT); |
// combine P & I & D-Part |
PID_North = P_North + I_North + D_North; |
PID_East = P_East + I_East + D_East; |
// scale combination with gain. |
PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / 100; |
PID_East = (PID_East * (int32_t)FCParam.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 GPS_Nick and GPS_Roll variable is contrarily to the stick values |
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
// GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
PID_Nick = (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2; |
PID_Roll = (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2; |
// limit resulting GPS control vector |
GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
GPS_Nick = (int16_t)PID_Nick; |
GPS_Roll = (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 < ParamSet.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/V0.70d CRK HexaLotte/gps.h |
---|
0,0 → 1,9 |
#ifndef _GPS_H |
#define _GPS_H |
#include <inttypes.h> |
extern void GPS_Main(void); |
#endif //_GPS_H |
/branches/V0.70d CRK HexaLotte/led.c |
---|
0,0 → 1,65 |
#include <inttypes.h> |
#include "led.h" |
#include "fc.h" |
#include "eeprom.h" |
uint8_t J16Blinkcount = 0, J16Mask = 1; |
uint8_t J17Blinkcount = 0, J17Mask = 1; |
// initializes the LED control outputs J16, J17 |
void LED_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; |
} |
// called in main loop every 2ms |
void LED_Update(void) |
{ |
static int8_t delay = 0; |
if(!delay--) // 10 ms intervall |
{ |
delay = 4; |
if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing > 230)) |
{ |
if(ParamSet.J16Bitmask & 128) J16_ON; |
else J16_OFF; |
} |
else if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing < 10)) |
{ |
if(ParamSet.J16Bitmask & 128) J16_OFF; |
else J16_ON; |
} |
else if(!J16Blinkcount--) |
{ |
J16Blinkcount = FCParam.J16Timing - 1; |
if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2; |
if(J16Mask & ParamSet.J16Bitmask) J16_ON; else J16_OFF; |
} |
if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing > 230)) |
{ |
if(ParamSet.J17Bitmask & 128) J17_ON; |
else J17_OFF; |
} |
else if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing < 10)) |
{ |
if(ParamSet.J17Bitmask & 128) J17_OFF; |
else J17_ON; |
} |
else if(!J17Blinkcount--) |
{ |
J17Blinkcount = FCParam.J17Timing - 1; |
if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2; |
if(J17Mask & ParamSet.J17Bitmask) J17_ON; else J17_OFF; |
} |
} |
} |
/branches/V0.70d CRK HexaLotte/led.h |
---|
0,0 → 1,19 |
#ifndef _LED_H |
#define _LED_H |
#include <avr/io.h> |
#define J16_ON PORTC |= (1<<PORTC2) |
#define J16_OFF PORTC &= ~(1<<PORTC2) |
#define J16_TOGGLE PORTC ^= (1<<PORTC2) |
#define J17_ON PORTC |= (1<<PORTC3) |
#define J17_OFF PORTC &= ~(1<<PORTC3) |
#define J17_TOGGLE PORTC ^= (1<<PORTC3) |
void LED_Init(void); |
void LED_Update(void); |
#endif //_LED_H |
/branches/V0.70d CRK HexaLotte/main.c |
---|
0,0 → 1,314 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "timer0.h" |
#include "timer2.h" |
#include "uart.h" |
#if defined (__AVR_ATmega644P__) |
#include "uart1.h" |
#endif |
#include "led.h" |
#include "menu.h" |
#include "fc.h" |
#include "rc.h" |
#include "analog.h" |
#include "printf_P.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#include "twimaster.h" |
#include "eeprom.h" |
#include "_Settings.h" |
uint8_t BoardRelease = 10; |
//############################################################################ |
//Hauptprogramm |
int main (void) |
//############################################################################ |
{ |
unsigned int timer; |
// disable interrupts global |
cli(); |
// get board release |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // make some delay |
if(PINB & (1<<PINB0)) |
{ |
if(PINB & (1<<PINB1)) BoardRelease = 13; |
else BoardRelease = 11; |
} |
else BoardRelease = 10; |
// set LED ports as output |
DDRB |= (1<<DDB1)|(1<<DDB0); |
RED_ON; |
GRN_OFF; |
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
BeepTime = 2000; |
PPM_in[CH_GAS] = 0; |
StickYaw = 0; |
StickRoll = 0; |
StickNick = 0; |
RED_OFF; |
// initalize modules |
//LED_Init(); Is done within ParamSet_Init() below |
TIMER0_Init(); |
TIMER2_Init(); |
USART0_Init(); |
#if defined (__AVR_ATmega644P__) |
if (BoardRelease == 11) USART1_Init(); |
#endif |
RC_Init(); |
ADC_Init(); |
I2C_Init(); |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
#ifdef USE_KILLAGREG |
MM3_Init(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Init(); |
#endif |
// enable interrupts global |
sei(); |
VersionInfo.Major = VERSION_MAJOR; |
VersionInfo.Minor = VERSION_MINOR; |
VersionInfo.PCCompatible = VERSION_COMPATIBLE; |
VersionInfo.Hardware = 1; // Flight Control |
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",BoardRelease/10,BoardRelease%10, VERSION_MAJOR, VERSION_MINOR,VERSION_INDEX + 'a'); |
printf("\n\r=============================="); |
GRN_ON; |
// Parameter set handling |
ParamSet_Init(); |
if(GetParamWord(PID_ACC_NICK) > 1023) |
{ |
printf("\n\rACC not calibrated!"); |
} |
//wait for a short time (otherwise the RC channel check won't work below) |
timer = SetDelay(500); |
while(!CheckDelay(timer)); |
if(ParamSet.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"); |
#endif |
#ifdef USE_KILLAGREG |
printf("\n\rSupport for MicroMag3 Compass"); |
#endif |
#ifdef USE_MK3MAG |
printf("\n\rSupport for MK3MAG Compass"); |
#endif |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
#if defined (__AVR_ATmega644P__) |
if(BoardRelease == 10) |
{ |
printf("\n\rSupport for GPS at 1st UART"); |
} |
else |
{ |
printf("\n\rSupport for GPS at 2nd UART"); |
} |
#else // (__AVR_ATmega644__) |
printf("\n\rSupport for GPS at 1st UART"); |
#endif |
#endif |
SetNeutral(); |
RED_OFF; |
BeepTime = 2000; |
ExternControl.Digital[0] = 0x55; |
printf("\n\rControl: "); |
if (ParamSet.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Neutral"); |
printf("\n\n\r"); |
LCD_Clear(); |
I2CTimeout = 5000; |
while (1) |
{ |
if(UpdateMotor) // control interval |
{ |
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by isr of timer0 |
//PORTD |= (1<<PORTD4); |
MotorControl(); |
//PORTD &= ~(1<<PORTD4); |
SendMotorData(); |
RED_OFF; |
if(PcAccess) PcAccess--; |
else |
{ |
DubWiseKeys[0] = 0; |
DubWiseKeys[1] = 0; |
ExternControl.Config = 0; |
ExternStickNick= 0; |
ExternStickRoll = 0; |
ExternStickYaw = 0; |
} |
if(!I2CTimeout) |
{ |
I2CTimeout = 5; |
I2C_Reset(); |
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
BeepTime = 10000; // 1 second |
BeepModulation = 0x0080; |
} |
} |
else |
{ |
I2CTimeout--; |
RED_OFF; |
} |
if(SIO_DEBUG && (!UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) )) |
{ |
USART0_TransmitTxData(); |
USART0_ProcessRxData(); |
} |
else USART0_ProcessRxData(); |
if(CheckDelay(timer)) |
{ |
if(UBat < ParamSet.LowVoltageWarning) |
{ |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 6000; // 0.6 seconds |
BeepModulation = 0x0300; |
} |
} |
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = SetDelay(20); // every 20 ms |
} |
LED_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/V0.70d CRK HexaLotte/main.h |
---|
0,0 → 1,36 |
#ifndef _MAIN_H |
#define _MAIN_H |
#include <avr/io.h> |
//set crystal frequency here |
#if defined (__AVR_ATmega644__) |
#define SYSCLK 20000000L //crystal freqency in Hz |
#endif |
#if defined (__AVR_ATmega644P__) |
#define SYSCLK 20000000L //crystal freqency in Hz |
#endif |
#define F_CPU SYSCLK |
// neue Hardware |
#define RED_OFF {if(BoardRelease == 10) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
#define RED_ON {if(BoardRelease == 10) 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) |
#include <inttypes.h> |
extern uint8_t BoardRelease; |
#endif //_MAIN_H |
/branches/V0.70d CRK HexaLotte/makefile |
---|
0,0 → 1,456 |
#-------------------------------------------------------------------- |
# MCU name |
#MCU = atmega644 |
MCU = atmega644p |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 70 |
VERSION_INDEX = 3 |
VERSION_COMPATIBLE = 8 # PC-Kompatibilität |
#------------------------------------------------------------------- |
#OPTIONS |
# Use one of the extensions for a gps solution |
EXT = KILLAGREG |
#EXT = NAVICTRL |
#EXT = MK3MAG |
#------------------------------------------------------------------- |
ifeq ($(MCU), atmega644) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
#FUSE_SETTINGS = -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
# -u bei neuen Controllern wieder einspielen |
HEX_NAME = MEGA644_$(EXT) |
endif |
ifeq ($(MCU), atmega644p) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644p_$(EXT) |
endif |
ifeq ($(F_CPU), 16000000) |
QUARZ = 16MHZ |
endif |
ifeq ($(F_CPU), 20000000) |
QUARZ = 20MHZ |
endif |
# Output format. (can be srec, ihex, binary) |
FORMAT = ihex |
# Target file name (without extension). |
ifeq ($(VERSION_INDEX), 0) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)a |
endif |
ifeq ($(VERSION_INDEX), 1) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)b |
endif |
ifeq ($(VERSION_INDEX), 2) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c |
endif |
ifeq ($(VERSION_INDEX), 3) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d |
endif |
ifeq ($(VERSION_INDEX), 4) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e |
endif |
ifeq ($(VERSION_INDEX), 5) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f |
endif |
ifeq ($(VERSION_INDEX), 6) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g |
endif |
ifeq ($(VERSION_INDEX), 7) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h |
endif |
ifeq ($(VERSION_INDEX), 8) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i |
endif |
ifeq ($(VERSION_INDEX), 9) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j |
endif |
ifeq ($(VERSION_INDEX), 10) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k |
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 = s |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c led.c |
SRC += twimaster.c rc.c fc.c eeprom.c fifo.c |
ifeq ($(MCU), atmega644p) |
SRC += uart1.c |
endif |
ifeq ($(EXT), KILLAGREG) |
SRC += mm3.c mymath.c gps.c ubx.c |
endif |
ifeq ($(EXT), MK3MAG) |
SRC += mk3mag.c mymath.c gps.c ubx.c |
endif |
ifeq ($(EXT), NAVICTRL) |
SRC += spi.c |
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 \ |
-Wa,-adhlns=$(<:.c=.lst) \ |
$(patsubst %,-I%,$(EXTRAINCDIRS)) |
# Set a "language standard" compiler flag. |
# Unremark just one line below to set the language standard to use. |
# gnu99 = C99 + GNU extensions. See GCC manual for more information. |
#CFLAGS += -std=c89 |
#CFLAGS += -std=gnu89 |
#CFLAGS += -std=c99 |
CFLAGS += -std=gnu99 |
CFLAGS += -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_COMPATIBLE=$(VERSION_COMPATIBLE) -DVERSION_INDEX=$(VERSION_INDEX) |
ifeq ($(EXT), KILLAGREG) |
CFLAGS += -DUSE_KILLAGREG |
endif |
ifeq ($(EXT), MK3MAG) |
CFLAGS += -DUSE_MK3MAG |
endif |
ifeq ($(EXT), NAVICTRL) |
CFLAGS += -DUSE_NAVICTRL |
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/V0.70d CRK HexaLotte/menu.c |
---|
0,0 → 1,257 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <inttypes.h> |
#include "main.h" |
#include "eeprom.h" |
#include "timer2.h" |
#include "fc.h" |
#include "rc.h" |
#include "uart.h" |
#include "printf_P.h" |
#include "analog.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
#include "ubx.h" |
#endif |
#include "_Settings.h" |
#define ARRAYSIZE 10 |
uint8_t Array[ARRAYSIZE] = {1,2,3,4,5,6,7,8,9,10}; |
#define DISPLAYBUFFSIZE 80 |
int8_t DisplayBuff[DISPLAYBUFFSIZE] = "Hello World"; |
uint8_t DispPtr = 0; |
uint8_t RemoteButtons = 0; |
#define KEY1 0x01 |
#define KEY2 0x02 |
#define KEY3 0x04 |
#define KEY4 0x08 |
#define KEY5 0x10 |
/************************************/ |
/* 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 (!defined (USE_KILLAGREG) && !defined (USE_MK3MAG)) |
static uint8_t MaxMenuItem = 11; |
#else |
#ifdef USE_MK3MAG |
static uint8_t MaxMenuItem = 12; |
#endif |
#ifdef USE_KILLAGREG |
static uint8_t MaxMenuItem = 14; |
#endif |
#endif |
static uint8_t MenuItem=0; |
// if KEY1 is activated goto previous menu item |
if(RemoteButtons & KEY1) |
{ |
if(MenuItem) MenuItem--; |
else MenuItem = MaxMenuItem; |
LCD_Clear(); |
RemotePollDisplayLine = -1; |
} |
// if KEY2 is activated goto next menu item |
if(RemoteButtons & KEY2) |
{ |
if (MenuItem == MaxMenuItem) MenuItem = 0; |
else MenuItem++; |
LCD_Clear(); |
RemotePollDisplayLine = -1; |
} |
// if KEY1 and KEY2 is activated goto initial menu item |
if((RemoteButtons & KEY1) && (RemoteButtons & KEY2)) MenuItem = 0; |
// 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_INDEX+'a'); |
LCD_printfxy(0,2,"Setting: %d ", GetActiveParamSet()); |
LCD_printfxy(0,3,"(c) Holger Buss"); |
break; |
case 1:// Height Control Menu Item |
if(ParamSet.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",ReadingAirPressure); |
LCD_printfxy(0,3,"Offset :%5i",PressureSensorOffset); |
} |
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",IntegralNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024); |
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[ParamSet.ChannelAssignment[CH_NICK]],PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]],PPM_in[ParamSet.ChannelAssignment[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]],PPM_in[ParamSet.ChannelAssignment[CH_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]],PPM_in[ParamSet.ChannelAssignment[CH_POTI4]]); |
break; |
case 5:// Gyro Sensor Menu Item |
LCD_printfxy(0,0,"Gyro - Sensor"); |
switch(BoardRelease) |
{ |
case 10: |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll); |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw); |
break; |
case 11: |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick/2); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2); |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw/2); |
break; |
case 12: |
default: |
LCD_printfxy(0,1,"Nick %4i (%3i)(%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick/2, AnalogOffsetNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)(%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2, AnalogOffsetRoll); |
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw/2 , AnalogOffsetYaw ); |
break; |
} |
break; |
case 6:// Acceleration Sensor Menu Item |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick, NeutralAccX); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll, NeutralAccY); |
LCD_printfxy(0,3,"Height %4i (%3i)",Mean_AccTop, (int)NeutralAccZ); |
break; |
case 7:// Accumulator Voltage / Remote Control Level |
LCD_printfxy(0,1,"Voltage: %5i",UBat); |
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" ,Poti1,Poti5); //PPM24-Extesion |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion |
break; |
case 10:// Servo Menu Item |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl); |
LCD_printfxy(0,2,"Position: %3i",ServoValue); |
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax); |
break; |
case 11://Extern Control |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick, ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",ExternControl.Gas, ExternControl.Yaw); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Height, ExternControl.Config); |
break; |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
case 12://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.%.3d%.3d 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.%.3d%.3d deg",i1, i2, i3); |
i1 = (int16_t)(GPSInfo.altitude/1000L); |
i2 = abs((int16_t)(GPSInfo.altitude%1000L)); |
LCD_printfxy(0,3,"Alt: %d.%.3d m",i1, i2); |
} |
break; |
#endif |
#ifdef USE_KILLAGREG |
case 13:// MM3 Kompass |
LCD_printfxy(0,0,"MM3 Offset"); |
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off); |
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off); |
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off); |
break; |
case 14://MM3 Range |
LCD_printfxy(0,0,"MM3 Range"); |
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range); |
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range); |
LCD_printfxy(0,3,"Z_Range: %4i",MM3_calib.Z_range); |
break; |
#endif |
default: MaxMenuItem = MenuItem - 1; |
MenuItem = 0; |
break; |
} |
RemoteButtons = 0; |
} |
/branches/V0.70d CRK HexaLotte/menu.h |
---|
0,0 → 1,16 |
#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 RemoteButtons; |
#endif //_MENU_H |
/branches/V0.70d CRK HexaLotte/mk3mag.c |
---|
0,0 → 1,81 |
#include <avr/io.h> |
#include <stdlib.h> |
#include <inttypes.h> |
#include "timer0.h" |
#include "fc.h" |
#include "rc.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/V0.70d CRK HexaLotte/mk3mag.h |
---|
0,0 → 1,21 |
#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/V0.70d CRK HexaLotte/mm3.c |
---|
0,0 → 1,476 |
/* |
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 "main.h" |
#include "mymath.h" |
#include "fc.h" |
#include "timer0.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "printf_P.h" |
#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 |
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); |
#ifdef USE_WALTER_EXT // walthers board |
// Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET |
DDRB |= (1<<DDB2); |
DDRC |= (1<<DDC6); |
// set pins permanent to low |
PORTB &= ~((1<<PORTB2)); |
PORTC &= ~((1<<PORTC6)); |
#else // killagregs board |
// Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
DDRC |= (1<<DDC4)|(1<<DDC5); |
// set pins permanent to low |
PORTC &= ~((1<<PORTC4)|(1<<PORTC5)); |
#endif |
// 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: |
#ifdef USE_WALTER_EXT // walthers board |
PORTC &= ~(1<<PORTC6); // select slave |
PORTB |= (1<<PORTB2); // PB2 to High, MM3 Reset |
#else |
PORTC &= ~(1<<PORTC4); // select slave |
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
#endif |
MM3.STATE = MM3_STATE_START_TRANSFER; |
return; |
case MM3_STATE_START_TRANSFER: |
#ifdef USE_WALTER_EXT // walthers board |
PORTB &= ~(1<<PORTB2); // PB2 auf Low (was 102.4 µs at high level) |
#else |
PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level) |
#endif |
// 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; |
} |
} |
#ifdef USE_WALTER_EXT // walthers board |
PORTC |= (1<<PORTC6); // deselect slave |
#else |
PORTC |= (1<<PORTC4); // deselect slave |
#endif |
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[ParamSet.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[ParamSet.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; |
uint16_t div_factor; |
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)ParamSet.UserParam4 + 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 |
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
// calculate sinus cosinus of nick and tilt angle |
angle = (IntegralNick/div_factor); |
sin_nick = (int32_t)(c_sin_8192(angle)); |
cos_nick = (int32_t)(c_cos_8192(angle)); |
angle = (IntegralRoll/div_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/V0.70d CRK HexaLotte/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/V0.70d CRK HexaLotte/mymath.c |
---|
0,0 → 1,100 |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include "mymath.h" |
// discrete mathematics |
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
int16_t c_sin_8192(int16_t angle) |
{ |
int8_t m,n; |
int16_t sinus; |
// avoid negative angles |
if (angle < 0) |
{ |
m = -1; |
angle = abs(angle); |
} |
else m = +1; |
// fold angle to intervall 0 to 359 |
angle %= 360; |
// check quadrant |
if (angle <= 90) n=1; // first quadrant |
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
else {angle = 360 - angle; n = -1;} //fourth quadrant |
// get lookup value |
sinus = pgm_read_word(&pgm_sinlookup[angle]); |
// calculate sinus value |
return (sinus * m * n); |
} |
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
int16_t c_cos_8192(int16_t angle) |
{ |
return (c_sin_8192(90 - angle)); |
} |
// Arcustangens returns degree in a range of +/. 180 deg |
const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
int16_t c_atan2(int16_t y, int16_t x) |
{ |
int16_t index, angle; |
int8_t m; |
if (!x && !y) return 0; //atan2(0, 0) is undefined |
if (y < 0) m = -1; |
else m = 1; |
if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
if (index < 0) index = -index; |
if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
else if (index > 7334) angle = 90; // limit is 90 deg |
else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
else if (index > 1465) angle = 88; |
else if (index > 1046) angle = 87; |
else if (index > 813) angle = 86; |
else if (index > 664) angle = 85; |
else if (index > 561) angle = 84; |
else if (index > 486) angle = 83; |
else if (index > 428) angle = 82; |
else if (index > 382) angle = 81; |
else angle = 80; // (index>345) |
if (x > 0) return (angle * m); // 1st and 4th quadrant |
else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
} |
// integer square root |
uint32_t c_sqrt(uint32_t number) |
{ |
if(!number) return 0; |
uint32_t s1, s2; |
uint8_t iter = 0; |
// initialization of iteration |
s2 = number; |
do // iterative formula to solve x^2 - n = 0 |
{ |
s1 = s2; |
s2 = number / s1; |
s2 += s1; |
s2 /= 2; |
iter++; |
//if(iter > 40) break; |
}while( ( (s1-s2) > 1) && (iter < 40)); |
return s2; |
} |
/branches/V0.70d CRK HexaLotte/mymath.h |
---|
0,0 → 1,11 |
#ifndef _MYMATH_H |
#define _MYMATH_H |
#include <inttypes.h> |
extern int16_t c_sin_8192(int16_t angle); |
extern int16_t c_cos_8192(int16_t angle); |
extern int16_t c_atan2(int16_t y, int16_t x); |
extern uint32_t c_sqrt(uint32_t number); |
#endif // _MYMATH_H |
/branches/V0.70d CRK HexaLotte/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/V0.70d CRK HexaLotte/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 "uart.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/V0.70d CRK HexaLotte/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/V0.70d CRK HexaLotte/rc.c |
---|
0,0 → 1,180 |
/*####################################################################################### |
Decodieren eines RC Summen Signals |
#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "rc.h" |
#include "main.h" |
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame |
volatile int16_t PPM_diff[15]; |
volatile uint8_t NewPpmData = 1; |
volatile int16_t RC_Quality = 0; |
volatile uint8_t NewRCFrames = 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); |
// low level |
PORTD &= ~((1<<PORTD5)|(1<<PORTD4)); |
// PD3 can't be used in FC 1.1 if 2nd UART is activated |
// because TXD1 is at that port |
if(BoardRelease == 10) |
{ |
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)); |
TIMSK1 |= (1<<ICIE1)|(1<<TOIE1); |
RC_Quality = 0; |
SREG = sreg; |
} |
// happens every 0.209712 s. |
// check for at least one new frame per timer overflow (timeout) |
ISR(TIMER1_OVF_vect) |
{ |
if (NewRCFrames == 0) RC_Quality -= RC_Quality/8; |
NewRCFrames = 0; |
} |
/********************************************************************/ |
/* 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 |
NewRCFrames++; |
} |
// synchronize channel index |
index = 1; |
} |
else // within the PPM frame |
{ |
if(index < 14) // 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 -= 466; // 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; |
} |
// 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 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 |
if(index == 5) PORTD |= (1<<PORTD5); else PORTD &= ~(1<<PORTD5); |
if(index == 6) PORTD |= (1<<PORTD4); else PORTD &= ~(1<<PORTD4); |
if(BoardRelease == 10) |
{ |
if(index == 7) PORTD |= (1<<PORTD3); else PORTD &= ~(1<<PORTD3); |
} |
} |
} |
if(RC_Quality) RC_Quality--; |
} |
/branches/V0.70d CRK HexaLotte/rc.h |
---|
0,0 → 1,11 |
#ifndef _RC_H |
#define _RC_H |
#include <inttypes.h> |
extern void RC_Init (void); |
extern volatile int16_t PPM_in[15]; // the RC-Signal |
extern volatile int16_t PPM_diff[15]; // the differentiated RC-Signal |
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) |
#endif //_RC_H |
/branches/V0.70d CRK HexaLotte/spi.c |
---|
0,0 → 1,328 |
// ######################## SPI - FlightCtrl ################### |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <string.h> |
#include <stdlib.h> |
#include "_Settings.h" |
#include "spi.h" |
#include "fc.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "uart.h" |
#include "timer0.h" |
#include "analog.h" |
#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; |
// 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; |
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS }; |
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.IntegralNick = 0; |
ToNaviCtrl.IntegralRoll = 0; |
SPI_RxDataValid = 0; |
} |
/**********************************************************/ |
/* Update Data transferd by the SPI from/to NaviCtrl */ |
/**********************************************************/ |
void UpdateSPI_Buffer(void) |
{ |
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.IntegralNick = (int16_t) (IntegralNick / 108); |
ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108); |
ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
ToNaviCtrl.GyroNick = Reading_GyroNick; |
ToNaviCtrl.GyroRoll = Reading_GyroRoll; |
ToNaviCtrl.GyroYaw = Reading_GyroYaw; |
ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
switch(ToNaviCtrl.Command) |
{ |
case SPI_CMD_USER: |
ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1; |
ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2; |
ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3; |
ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4; |
ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5; |
ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6; |
ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7; |
ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8; |
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] = ParamSet.LowVoltageWarning; |
ToNaviCtrl.Param.Byte[11] = GetActiveParamSet(); |
break; |
case SPI_CMD_PARAMETER1: |
ToNaviCtrl.Param.Byte[0] = FCParam.NaviGpsModeControl; // Parameters for the Naviboard |
ToNaviCtrl.Param.Byte[1] = FCParam.NaviGpsGain; |
ToNaviCtrl.Param.Byte[2] = FCParam.NaviGpsP; |
ToNaviCtrl.Param.Byte[3] = FCParam.NaviGpsI; |
ToNaviCtrl.Param.Byte[4] = FCParam.NaviGpsD; |
ToNaviCtrl.Param.Byte[5] = FCParam.NaviGpsACC; |
ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat; |
ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold; |
ToNaviCtrl.Param.Byte[8] = 15; // MaxRadius |
break; |
case SPI_CMD_STICK: |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1; |
ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2; |
ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3; |
ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4; |
ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
break; |
case SPI_CMD_CAL_COMPASS: |
if(CompassCalState > 5) |
{ |
CompassCalState = 0; |
ToNaviCtrl.Param.Byte[0] = 5; |
} |
else |
{ |
ToNaviCtrl.Param.Byte[0] = CompassCalState; |
} |
break; |
} |
sei(); // enable all interrupts |
// analyze content of packet from NaviCtrl if valid |
if (SPI_RxDataValid) |
{ |
// update gps controls |
if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
{ |
GPS_Nick = FromNaviCtrl.GPS_Nick; |
GPS_Roll = FromNaviCtrl.GPS_Roll; |
} |
// 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_CMD_OSD_DATA: |
// ToFlightCtrl.Param.Byte[0] = OsdBar; |
// ToFlightCtrl.Param.Int[1] = Distance; |
break; |
case SPI_CMD_GPS_POS: |
// ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude; |
// ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude; |
break; |
case SPI_CMD_GPS_TARGET: |
// ToFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude; |
// ToFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude; |
break; |
default: |
break; |
} |
} |
else // no valid data from NaviCtrl |
{ |
// disable GPS control |
GPS_Nick = 0; |
GPS_Roll = 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; |
DebugOut.Analog[18]++; |
} |
else |
{ // checksum does not match |
DebugOut.Analog[17]++; |
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/V0.70d CRK HexaLotte/spi.h |
---|
0,0 → 1,117 |
// ######################## SPI - FlightCtrl ################### |
#ifndef _SPI_H |
#define _SPI_H |
//#include <util/delay.h> |
#include <inttypes.h> |
#define SPI_PROTOCOL_COMP 1 |
//----------------------------------------- |
#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 SPE |
#define SPE SPE0 |
#endif |
#ifndef MSTR |
#define MSTR MSTR0 |
#endif |
#ifndef SPR1 |
#define SPR1 SPR01 |
#endif |
#ifndef SPR0 |
#define SPR0 SPR00 |
#endif |
#ifndef SPIE |
#define SPIE SPIE0 |
#endif |
#ifndef SPDR |
#define SPDR SPDR0 |
#endif |
#ifndef SPIF |
#define SPIF SPIF0 |
#endif |
#ifndef SPSR |
#define SPSR SPSR0 |
#endif |
// ------------------------- |
#define SLAVE_SELECT_DDR_PORT DDRC |
#define SLAVE_SELECT_PORT PORTC |
#define SPI_SLAVE_SELECT PC5 |
#define SPI_CMD_USER 10 |
#define SPI_CMD_STICK 11 |
#define SPI_CMD_CAL_COMPASS 12 |
#define SPI_CMD_PARAMETER1 13 |
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; |
} ToNaviCtrl_t; |
#define SPI_CMD_OSD_DATA 100 |
#define SPI_CMD_GPS_POS 101 |
#define SPI_CMD_GPS_TARGET 102 |
typedef struct |
{ |
uint8_t Command; |
int16_t GPS_Nick; |
int16_t GPS_Roll; |
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; |
} FromNaviCtrl_t; |
extern ToNaviCtrl_t ToNaviCtrl; |
extern FromNaviCtrl_t FromNaviCtrl; |
void SPI_MasterInit(void); |
void SPI_StartTransmitPacket(void); |
void SPI_TransmitByte(void); |
#endif //_SPI_H |
/branches/V0.70d CRK HexaLotte/timer0.c |
---|
0,0 → 1,192 |
#include <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "analog.h" |
#include "main.h" |
#include "fc.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
volatile uint16_t CountMilliseconds = 0; |
volatile uint8_t UpdateMotor = 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 aoffset for the pressure sensor |
DDRB |= (1<<DDB4)|(1<<DDB3); |
PORTB &= ~((1<<PORTB4)|(1<<PORTB3)); |
if(BoardRelease == 10) |
{ |
DDRD |= (1<<DDD2); |
PORTD &= ~(1<<PORTD2); |
} |
else |
{ |
DDRC |= (1<<DDC7); |
PORTC &= ~(1<<PORTC7); |
} |
// 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 devider 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) // 9.765 kHz |
{ |
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++; |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; // every 2nd run (976Hz/2 = 488 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 |
} |
// update compass value if this option is enabled in the settings |
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) |
{ |
#ifdef USE_KILLAGREG |
MM3_Update(); // read out mm3 board |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Update(); // read out mk3mag pwm |
#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) |
{ |
unsigned int t_stop; |
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)) ADC_Enable(); |
} |
/branches/V0.70d CRK HexaLotte/timer0.h |
---|
0,0 → 1,21 |
#ifndef _TIMER0_H |
#define _TIMER0_H |
#include <inttypes.h> |
extern volatile uint16_t CountMilliseconds; |
extern volatile uint8_t UpdateMotor; |
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/V0.70d CRK HexaLotte/timer2.c |
---|
0,0 → 1,152 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "fc.h" |
#include "eeprom.h" |
#include "uart.h" |
volatile uint16_t ServoValue = 0; |
/*****************************************************/ |
/* Initialize Timer 2 */ |
/*****************************************************/ |
// The timer 2 is used to generate the PWM at PD7 (J7) |
// to control a camera servo for nick compensation. |
void TIMER2_Init(void) |
{ |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for nick servo |
DDRD |=(1<<DDD7); |
PORTD &= ~(1<<PORTD7); // set PD7 to low |
// Timer/Counter 2 Control Register A |
// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, 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/64 = 20MHz / 64 = 312.5 kHz |
// The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
// hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
// divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
TCCR2B |= (1<<CS22); |
// Initialize the Timer/Counter 2 Register |
TCNT2 = 0; |
// Initialize the Output Compare Register A used for PWM generation on port PD7. |
OCR2A = 10; |
// Timer/Counter 2 Interrupt Mask Register |
// Enable timer output compare match A Interrupt only |
TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
TIMSK2 |= (1<<OCIE2A); |
SREG = sreg; |
} |
/*****************************************************/ |
/* Control Servo Position */ |
/*****************************************************/ |
ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
{ |
static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match) |
static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl |
static uint16_t ServoState = 40; // cycle down counter for this ISR |
#define MULTIPLIER 4 |
switch(ServoState) |
{ |
case 4: |
// recalculate new ServoValue |
ServoValue = 0x0030; // Offset (part 1) |
FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
ServoValue += FilterServo; // add filtered static offset |
if(ParamSet.ServoNickCompInvert & 0x01) |
{ // inverting movement of servo |
ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
} |
else |
{ // non inverting movement of servo |
ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
} |
// limit servo value to its parameter range definition |
if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
{ |
ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
} |
else |
if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
{ |
ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
} |
DebugOut.Analog[20] = ServoValue; |
// determine prepulse width (remaining part of ServoValue/Timer Cycle) |
if ((ServoValue % 255) < 45) |
{ // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
// so balance with postpulse width |
ServoValue += 77; |
PostPulse = 0x60 - 77; |
} |
else |
{ |
PostPulse = 0x60; |
} |
// set output compare register to 255 - prepulse width |
OCR2A = 255 - (ServoValue % 256); |
// connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
break; |
case 3: |
case 2: |
case 1: |
if(ServoValue > 255) // is larger than a full timer 2 cycle |
{ |
PORTD |= (1<<PORTD7); // set PD7 to high |
TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
ServoValue -= 255; // substract full timer cycle |
} |
else // the post pule must be generated |
{ |
TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
OCR2A = PostPulse; // Offset Part2 |
ServoState = 1; // jump to ServoState 0 with next ISR call |
} |
break; |
case 0: |
ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState |
PORTD &= ~(1<<PORTD7); // set PD7 to low |
TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
break; |
default: |
// do nothing |
break; |
} |
ServoState--; |
} |
/branches/V0.70d CRK HexaLotte/timer2.h |
---|
0,0 → 1,13 |
#ifndef _TIMER2_H |
#define _TIMER2_H |
#include <inttypes.h> |
extern volatile uint16_t ServoValue; |
void TIMER2_Init(void); |
#endif //_TIMER2_H |
/branches/V0.70d CRK HexaLotte/twimaster.c |
---|
0,0 → 1,268 |
/*############################################################################ |
############################################################################*/ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "main.h" |
#include "twimaster.h" |
#include "fc.h" |
#include "analog.h" |
volatile uint8_t twi_state = 0; |
volatile uint8_t motor_write = 0; |
volatile uint8_t motor_read = 0; |
volatile uint8_t dac_channel = 0; |
volatile uint8_t motor_rx[8]; |
volatile uint16_t I2CTimeout = 100; |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define TWSR_STATUS_MASK 0xF8 |
// for Master Transmitter Mode |
#define I2C_STATUS_START 0x08 |
#define I2C_STATUS_REPEATSTART 0x10 |
#define I2C_STATUS_TX_SLA_ACK 0x18 |
#define I2C_STATUS_SLAW_NOACK 0x20 |
#define I2C_STATUS_TX_DATA_ACK 0x28 |
#define I2C_STATUS_TX_DATA_NOTACK 0x30 |
#define I2C_STATUS_RX_DATA_ACK 0x50 |
#define I2C_STATUS_RX_DATA_NOTACK 0x58 |
/**************************************************/ |
/* Initialize I2C (TWI) */ |
/**************************************************/ |
void I2C_Init(void) |
{ |
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 = 0; |
motor_write = 0; |
motor_read = 0; |
SREG = sreg; |
} |
/****************************************/ |
/* Start I2C */ |
/****************************************/ |
void I2C_Start(void) |
{ |
// 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(void) |
{ |
// 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 = 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(); |
I2C_WriteByte(0); |
} |
/****************************************/ |
/* I2C ISR */ |
/****************************************/ |
ISR (TWI_vect) |
{ |
switch (twi_state++) // First i2c_start from SendMotorData() |
{ |
// Master Transmit |
case 0: // Send SLA-W |
I2C_WriteByte(0x52 + (motor_write * 2) ); |
break; |
case 1: // Send Data to Slave |
switch(motor_write) |
{ |
case 0: |
I2C_WriteByte(Motor_Front); |
break; |
case 1: |
I2C_WriteByte(Motor_Rear); |
break; |
case 2: |
I2C_WriteByte(Motor_Right); |
break; |
case 3: |
I2C_WriteByte(Motor_Left); |
break; |
} |
break; |
case 2: // repeat case 0+1 for all motors |
I2C_Stop(); |
if (motor_write < 3) |
{ |
motor_write++; // jump to next motor |
twi_state = 0; // and repeat from state 0 |
} |
else |
{ // data to last motor send |
motor_write = 0; // reset motor write counter |
} |
I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive |
break; |
// Master Receive |
case 3: // Send SLA-R |
I2C_WriteByte(0x53 + (motor_read * 2) ); |
break; |
case 4: |
//Transmit 1st byte |
I2C_ReceiveByte(); |
break; |
case 5: //Read 1st byte and transmit 2nd Byte |
motor_rx[motor_read] = TWDR; |
I2C_ReceiveLastByte(); |
break; |
case 6: |
//Read 2nd byte |
motor_rx[motor_read + 4] = TWDR; |
motor_read++; |
if (motor_read > 3) motor_read = 0; |
I2C_Stop(); |
twi_state = 0; |
I2CTimeout = 10; |
break; |
// Gyro-Offsets |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
case 8: |
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
case 9: |
switch(dac_channel) |
{ |
case 0: |
I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
break; |
case 1: |
I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
break; |
case 2: |
I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
break; |
} |
break; |
case 10: |
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
break; |
case 11: |
I2C_Stop(); |
I2CTimeout = 10; |
// repeat case 7...10 until all DAC Channels are updated |
if(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
twi_state = 7; // and repeat from state 7 |
I2C_Start(); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
twi_state = 0; // reset twi_state |
} |
break; |
default: |
I2C_Stop(); |
twi_state = 0; |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
} |
/branches/V0.70d CRK HexaLotte/twimaster.h |
---|
0,0 → 1,20 |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
+ |
+#include <inttypes.h> |
+ |
+#define TWI_STATE_MOTOR_TX 0 |
+#define TWI_STATE_GYRO_OFFSET_TX 7 |
+ |
+extern volatile uint8_t twi_state; |
+extern volatile uint8_t motor_rx[8]; |
+extern volatile uint16_t I2CTimeout; |
+ |
+extern void I2C_Init (void); // Initialize I2C |
+extern void I2C_Start(void); // Start I2C |
+extern void I2C_Stop (void); // Stop I2C |
+extern void I2C_Reset(void); // Reset I2C |
+ |
+#endif |
/branches/V0.70d CRK HexaLotte/uart.c |
---|
0,0 → 1,518 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/wdt.h> |
#include "eeprom.h" |
#include "main.h" |
#include "menu.h" |
#include "timer0.h" |
#include "uart.h" |
#include "fc.h" |
#include "_Settings.h" |
#include "rc.h" |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
#include "ubx.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#define FALSE 0 |
#define TRUE 1 |
//int8_t test __attribute__ ((section (".noinit"))); |
uint8_t RequestVerInfo = FALSE; |
uint8_t RequestExternalControl = FALSE; |
uint8_t RequestDisplay = FALSE; |
uint8_t RequestDebugData = FALSE; |
uint8_t RequestDebugLabel = 255; |
uint8_t RequestChannelOnly = FALSE; |
uint8_t RemotePollDisplayLine = 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; |
uint8_t PcAccess = 100; |
uint8_t MotorTest[4] = {0,0,0,0}; |
uint8_t DubWiseKeys[4] = {0,0,0,0}; |
uint8_t MySlaveAddr = 0; |
uint8_t ConfirmFrame; |
DebugOut_t DebugOut; |
ExternControl_t ExternControl; |
VersionInfo_t VersionInfo; |
int16_t Debug_Timer; |
#ifdef USE_MK3MAG |
int16_t Compass_Timer; |
#endif |
const uint8_t ANALOG_LABEL[32][16] = |
{ |
//1234567890123456 |
"IntegralNick ", //0 |
"IntegralRoll ", |
"AccNick ", |
"AccRoll ", |
"GyroYaw ", |
"ReadingHeight ", //5 |
"AccZ ", |
"Gas ", |
"CompassHeading ", |
"Voltage ", |
"Receiver Level ", //10 |
"YawGyroHeading ", |
"Motor_Front ", |
"Motor_Rear ", |
"Motor_Right ", |
"Motor_Left ", //15 |
" ", |
"SPI Error ", |
"SPI Ok ", |
" ", |
"Servo ", //20 |
"Nick ", |
"Roll ", |
" ", |
" ", |
" ", //25 |
" ", |
" ", |
" ", |
" ", |
"GPS_Nick ", //30 |
"GPS_Roll " |
}; |
/****************************************************************/ |
/* 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); |
rxd_buffer_locked = FALSE; |
txd_complete = TRUE; |
Debug_Timer = SetDelay(200); |
#ifdef USE_MK3MAG |
Compass_Timer = SetDelay(220); |
#endif |
// 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; |
c = UDR0; // catch the received byte |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
// If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart. |
// The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected. |
#if defined (__AVR_ATmega644P__) |
if(BoardRelease == 10) ubx_parser(c); |
#else |
ubx_parser(c); |
#endif |
#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_locked = TRUE; // lock the rxd buffer |
ReceivedBytes = ptr_rxd_buffer; // store number of received bytes |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
// 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) |
} |
// -------------------------------------------------------------------------- |
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len) |
{ |
uint16_t pt = 0; |
uint8_t a,b,c; |
uint8_t ptr = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = module; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
while(len) |
{ |
if(len) { a = snd[ptr++]; len--;} else a = 0; |
if(len) { b = snd[ptr++]; len--;} else b = 0; |
if(len) { c = snd[ptr++]; len--;} else c = 0; |
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); |
} |
AddCRC(pt); // add checksum after data block and initates the transmission |
} |
// -------------------------------------------------------------------------- |
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max) |
{ |
uint8_t a,b,c,d; |
uint8_t ptr = 0; |
uint8_t x,y,z; |
while(len) |
{ |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
if(ptrIn > max - 2) break; |
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if(len--) ptrOut[ptr++] = x; else break; |
if(len--) ptrOut[ptr++] = y; else break; |
if(len--) ptrOut[ptr++] = z; else break; |
} |
} |
// -------------------------------------------------------------------------- |
void USART0_ProcessRxData(void) |
{ |
// if data in the rxd buffer are not locked immediately return |
if(!rxd_buffer_locked) return; |
#if !defined (USE_KILLAGREG) && !defined (USE_NAVICTRL) |
uint16_t tmp_int_arr1[1]; // local int buffer |
#endif |
uint8_t tmp_char_arr2[2]; // local char buffer |
switch(rxd_buffer[2]) |
{ |
#ifdef USE_MK3MAG |
case 'K':// Compass value |
Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes); |
CompassHeading = tmp_int_arr1[0]; |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
break; |
#endif |
case 'a':// Labels of the Analog Debug outputs |
Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes); |
RequestDebugLabel = tmp_char_arr2[0]; |
PcAccess = 255; |
break; |
case 'b': // extern control |
Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes); |
RemoteButtons |= ExternControl.RemoteButtons; |
ConfirmFrame = ExternControl.Frame; |
PcAccess = 255; |
break; |
case 'c': // extern control with debug request |
Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes); |
RemoteButtons |= ExternControl.RemoteButtons; |
ConfirmFrame = ExternControl.Frame; |
RequestDebugData = TRUE; |
PcAccess = 255; |
break; |
case 'h':// x-1 display columns |
Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
RemoteButtons |= tmp_char_arr2[0]; |
if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE; |
else RequestChannelOnly = FALSE; // keine Displaydaten |
RequestDisplay = TRUE; |
break; |
case 't':// motor test |
Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes); |
PcAccess = 255; |
break; |
case 'k':// keys from DubWise |
Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes); |
ConfirmFrame = DubWiseKeys[3]; |
PcAccess = 255; |
break; |
case 'v': // get version and board release |
RequestVerInfo = TRUE; |
break; |
case 'g':// get external control data |
RequestExternalControl = TRUE; |
break; |
case 'q':// get settings |
Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes); |
while(!txd_complete); |
if(tmp_char_arr2[0] != 0xff) |
{ |
if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5 |
// load requested parameter set |
ParamSet_ReadFromEEProm(tmp_char_arr2[0]); |
SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
} |
else // send active parameter set |
SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN); |
break; |
case 'l': |
case 'm': |
case 'n': |
case 'o': |
case 'p': // save parameterset |
Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes); |
ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1); |
TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
Beep(GetActiveParamSet()); |
break; |
} |
// unlock the rxd buffer after processing |
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); |
//Ausgabe des Zeichens |
UDR0 = c; |
return (0); |
} |
//--------------------------------------------------------------------------------------------- |
void USART0_TransmitTxData(void) |
{ |
if(!txd_complete) return; |
if(RequestExternalControl && txd_complete) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
{ |
SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl)); |
RequestExternalControl = FALSE; |
} |
#ifdef USE_MK3MAG |
if((CheckDelay(Compass_Timer)) && txd_complete) |
{ |
ToMk3Mag.Attitude[0] = (int16_t) (IntegralNick / 108); // approx. 0,1 Deg |
ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108); // approx. 0,1 Deg |
ToMk3Mag.UserParam[0] = FCParam.UserParam1; |
ToMk3Mag.UserParam[1] = FCParam.UserParam2; |
ToMk3Mag.CalState = CompassCalState; |
SendOutData('w',MySlaveAddr,(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((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete) |
{ |
SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut)); |
RequestDebugData = FALSE; |
Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL); |
} |
if(RequestDebugLabel != 255) // Texte für die Analogdaten |
{ |
SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16); |
RequestDebugLabel = 255; |
} |
if(ConfirmFrame && txd_complete) // Datensatz ohne CRC bestätigen |
{ |
txd_buffer[0] = '#'; |
txd_buffer[1] = ConfirmFrame; |
txd_buffer[2] = '\r'; |
txd_complete = 0; |
ConfirmFrame = 0; |
UDR0 = txd_buffer[0]; |
} |
if(RequestDisplay && txd_complete) |
{ |
LCD_PrintMenu(); |
RequestDisplay = FALSE; |
if(++RemotePollDisplayLine == 4 || RequestChannelOnly) |
{ |
SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
RemotePollDisplayLine = -1; |
} |
else SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20); // DisplayZeile übertragen |
} |
if(RequestVerInfo && txd_complete) |
{ |
SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo)); |
RequestVerInfo = FALSE; |
} |
} |
/branches/V0.70d CRK HexaLotte/uart.h |
---|
0,0 → 1,64 |
#ifndef _UART_H |
#define _UART_H |
#define TXD_BUFFER_LEN 150 |
#define RXD_BUFFER_LEN 150 |
#define DUB_KEY_UP 4 |
#define DUB_KEY_DOWN 8 |
#define DUB_KEY_LEFT 16 |
#define DUB_KEY_RIGHT 32 |
#define DUB_KEY_FIRE 64 |
#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 PcAccess; |
extern uint8_t RemotePollDisplayLine; |
extern uint8_t MotorTest[4]; |
extern uint8_t DubWiseKeys[4]; |
typedef struct |
{ |
uint8_t Digital[2]; |
uint16_t Analog[32]; // Debugvalues |
} DebugOut_t; |
extern DebugOut_t DebugOut; |
typedef struct |
{ |
uint8_t Digital[2]; |
uint8_t RemoteButtons; |
int8_t Nick; |
int8_t Roll; |
int8_t Yaw; |
uint8_t Gas; |
int8_t Height; |
uint8_t free; |
uint8_t Frame; |
uint8_t Config; |
} ExternControl_t; |
extern ExternControl_t ExternControl; |
typedef struct |
{ |
uint8_t Major; |
uint8_t Minor; |
uint8_t PCCompatible; |
uint8_t Hardware; |
uint8_t Reserved[6]; |
} VersionInfo_t; |
extern VersionInfo_t VersionInfo; |
#endif //_UART_H |
/branches/V0.70d CRK HexaLotte/uart1.c |
---|
0,0 → 1,148 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "main.h" |
#include "uart1.h" |
#include "fifo.h" |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
#include "ubx.h" |
#endif |
// FIFO-objects and buffers for input and output |
//#define BUFSIZE_IN 0x96 |
//volatile uint8_t inbuf[BUFSIZE_IN]; |
//fifo_t infifo; |
#define BUFSIZE_OUT 0x96 |
volatile uint8_t outbuf[BUFSIZE_OUT]; |
fifo_t outfifo; |
/****************************************************************/ |
/* 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; |
// inint FIFO buffer |
//fifo_init (&infifo, inbuf, BUFSIZE_IN); |
//fifo_init (&outfifo, outbuf, BUFSIZE_OUT); |
} |
/*int16_t USART1_putc (const uint8_t c) |
{ |
int16_t ret = fifo_put (&outfifo, c); |
// create an data register empty interrupt |
UCSR1B |= (1 << UDRIE1); |
return ret; |
} |
*/ |
/*int16_t USART1_getc_nowait () |
{ |
return fifo_get_nowait (&infifo); |
} |
uint8_t USART1_getc_wait () |
{ |
return fifo_get_wait (&infifo); |
} |
*/ |
/****************************************************************/ |
/* USART1 data register empty ISR */ |
/****************************************************************/ |
/*ISR(USART1_UDRE_vect) |
{ |
// Move a character from the output buffer to the data register. |
// When the character was processed the next interrupt is generated. |
// If the output buffer is empty the DRE-interrupt is disabled. |
if (outfifo.count > 0) |
UDR1 = _inline_fifo_get (&outfifo); |
else |
UCSR1B &= ~(1 << UDRIE1); |
} |
*/ |
/****************************************************************/ |
/* 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_KILLAGREG) || defined (USE_MK3MAG) |
if (BoardRelease == 11) ubx_parser(c); // and put it into the ubx protocol parser |
#endif |
} |
/branches/V0.70d CRK HexaLotte/uart1.h |
---|
0,0 → 1,25 |
#ifndef _UART1_H |
#define _UART1_H |
#define USART1_BAUD 57600 |
/* |
Initialize the USART und activate the receiver and transmitter |
as well as the receive-interrupt. The IO-FIFOs are initialized. |
The global interrupt-enable-flag (I-Bit in SREG) is not changed |
*/ |
extern void USART1_Init (void); |
/* |
The character c is stored in the output buffer. If the character was pushed sucessfully to |
the output buffer then the return value is 1. In case of an output buffer overflow the return value is 0. |
The isr is activated, which will send the data from the outbut buffer to the UART. |
*/ |
extern int USART1_putc (const uint8_t c); |
/* |
extern uint8_t USART1_getc_wait(void); |
extern int16_t USART1_getc_nowait(void); |
*/ |
#endif //_UART1_H |
/branches/V0.70d CRK HexaLotte/ubx.c |
---|
0,0 → 1,239 |
#include <inttypes.h> |
#include "ubx.h" |
#include "main.h" |
#include <avr/io.h> |
#include "uart.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/V0.70d CRK HexaLotte/ubx.h |
---|
0,0 → 1,59 |
#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 |
// this function should be called within the UART RX ISR |
extern void ubx_parser(uint8_t c); |
#endif //_UBX_H |
/branches/V0.70d CRK HexaLotte/version.txt |
---|
0,0 → 1,181 |
------- |
V0.53 27.04.2007 H.Buss |
- erste öffentliche Version |
V0.53b 29.04.2007 H.Buss |
- der FAKTOR_I war versehentlich auf Null, dann liegt der MikroKopter nicht so hart in der Luft |
V0.53c 29.04.2007 H.Buss |
- es gib ein Menü, in dem die Werte der Kanäle nach Nick, Roll, Gas,... sortiert sind. |
Die angezeigten Werte waren nicht die Werte der Funke |
V0.54 01.05.2007 H.Buss |
- die Paramtersätze können jetzt vor dem Start ausgewählt werden |
Dazu wird beim Kalibrieren der Messwerte (Gashebel oben links) der Nick-Rollhebel abgefragt: |
2 3 4 |
1 x 5 |
- - - |
Bedeutet: Nick-Rollhebel Links Mitte = Setting:1 Links Oben = Setting:2 usw. |
- der Faktor_I für den Hauptregler ist hinzugekommen. Im Heading-Hold-Modus sollte er vergössert werden, was Stabilität bringt |
V0.55 14.05.2007 H.Buss |
- es können nun Servos an J3,J4,J5 mit den Kanälen 5-7 gesteuert werden |
V0.56 14.05.2007 H.Buss |
- es gab Probleme mit Funken, die mehr als 8 Kanäle haben, wenn mehrere Kanäle dann auf Null waren |
- Funken, die nicht bis +-120 aussteuern können, sollten jetzt auch gehen |
V0.57 24.05.2007 H.Buss |
- Der Höhenregler kann nun auch mittels Schalter bedient werden |
- Bug im Gier-Algorithmus behoben; Schnelles Gieren fürhrte dazu, dass der MK zu weit gedreht hat |
- Kompass-Einfluss dämpfen bei Neigung |
- Man kann zwischen Kompass FIX (Richtung beim Kalibrieren) und Variabel (einstellbar per Gier) wählen |
- Der Motortest vom Kopter-Tool geht jetzt |
- Man kann den Parametersätzen einen Namen geben |
- Das Kamerasetting ist unter Setting 2 defaultmässig integriert |
V0.58 30.05.2007 H.Buss |
- Der Höhenregler-Algorithmus wird nun umgangen, wenn der Höhenreglerschalter aus ist |
V0.60 17.08.2007 H.Buss |
- "Schwindel-Bug" behoben |
- Die Poti-Werte werden jetzt auf Unterlauf (<0) überprüft |
- Poti4 zugefügt |
- Es werden jetzt 8 Kanäle ausgewertet |
- Kamera-Servo (an J7) |
- Die Settings müssen überschrieben werden |
V0.61 - V0.63 H.Buss 27.09.2007 |
- Poti 4 und Kanal 8 werden im Menü angezeigt |
- ein paar Kleinigkeiten bei den DefaultKonstanten2 bereinigt |
- Analog.c: Aktuell_ax korrigiert |
- auf 32 Debug-Kanäle erweitert |
- Loopings sind jetzt möglich und einzeln im KopterTool freischaltbar |
- leichte Anpassungen im Gier - Geschwindigkeit und Drift |
- die Hardwareversion V1.1 wird erkannt und das Programm stellt sich auf die geänderte Gyroverstärkung und die geänderten Portpins ein |
- die Software startet nach dem Einschalten schneller, weil der Luftdruckoffset schneller gefunden wird |
- die PPM-Ausgänge liegen wieder an den Pins an |
- Details an der Sensordatenverarbeitung -> es fliegt sich geringfügig anders |
- der MK ist bei wenig Gas nicht mehr so giftig -> soll das Landen vereinfachen |
- I2C-Bus läuft jetzt sicher nach einer Störung wieder an |
- Sticksignale werden präziser ausgewertet |
- Stick-Kanäle werden ans Kopter-Tool übertragen |
- Es muss die Version V1.47 des Kopter-Tool verwendet werden |
- Die Settings werden auf Default zurückgesetzt |
- am Piepen kann man die Fehlerart unterscheiden |
1. einzelnes Piepen beim Einschalten und Kalibrieren |
2. langsames Intervall mindestens 1 Sek -> Empfangsausfall |
3. schnelleres Intervall mindestens 1 Sek -> Akku |
4. sehr schnelles Intervall mindestens 1 Sek -> Kommunikation zu den Reglern gestört |
V0.64 H.Buss 30.09.2007 |
- beim Gieren wurden die Achsen nicht hart genug geregelt |
V0.65a H.Buss 15.10.2007 |
- Integral im Mischer wieder integriert |
- Feinabstimmung im ACC/Gyro Abgleich -> 1/32 & 100 |
- ACC/Gyro Abgleich auch bei HH |
V0.66a H.Buss 3.11.2007 |
- Messwertverarbeitung aus dem Analog-Interrupt entfernt |
- Analogmessung hängt jetzt am FC-Timing |
- Looping-Stick-Hysterese eingebaut |
- Looping-180°-Umschlag einstellbar |
- Achsenkopplung: Gierbewegung verkoppelt Nick und Roll |
- Lageregelung nach ACC-Sensor verbessert |
- zusätzlicher I-Anteil in der Lageregelung verbessert die Neutrallage |
- Gyrodriftkompensation überarbeitet |
- Bug in der Gier-Stick-Berechnung behoben |
- Gyro-Messung auf 1kHz beschleunigt |
V0.67a H.Buss 16.11.2007 |
- der Hauptregler-I-Anteil wirkt jetzt nur noch auf den Winkel (ausser im HH-Mode) |
- Gyro-Acc-Abgleich jetzt wieder in jedem Zyklus |
- Feinabstimmung |
- Beim HH-Modus gab es noch Bugs |
V0.67e H.Buss 29.11.2007 |
- Parameter: Dynamic Stability und Driftfaktor eingeführt |
- Die Namen der Analogwerte werden jetzt zum Koptertool übertragen |
- Kompatibilität zum Koptertool erhöht |
V0.67f H.Buss 04.12.2007 |
- Das Integral des Hauptreglers wird jetzt linear entladen und nicht mehr proportional |
- Schub für Gier wird jetzt auf den Gaswert begrenzt, dadurch steigt der MK nicht mehr beim Gieren. Gier ist allerdings nicht mehr so agressiv |
- Die ACC-Nullwerte können jetzt dauerhaft im EEPROM gespeichert werden (Stick:Vollgas und Gier rechts) |
V0.68a I.Busker 28.12.2007 |
- SPI.c & SPI.h ins Projekt aufgenommen |
SPI-Kommuikation kann in SPI.h aktiviert/deaktivert werden |
V0.68c H.Buss 05.01.2008 |
- Stickauswertung verbessert -> träger und präziser |
- Alle Settings angepasst |
V0.69g H.Buss 05.05.2008 |
- kleinere Bugs beseitigt |
- Schneller Sinkflug jetzt möglich |
- Min- und Maxgas in den Settings geändert |
- Lagewinkel wird jetzt in 0,1 Grad an Kompass und Navi gesendet |
- Kalibrierung für MK3Mag -> Nick unten beim Kalibrieren |
- Kompassroutine um den Ersatzkompass (Gyro unterstützt Kompasswert) erweitert |
V0.69h H.Buss 21.05.2008 |
- STICK_GAIN = 4 eingeführt. Das erhöht die Auflösung der Sollwerte. Stick_P und Stick_I müssen nun um Faktor 4 erhöht werden |
- SenderOkay auch an das Naviboard übertragen |
- Bessere Parameter bei Senderausfall |
V0.69j H.Buss 30.05.2008 |
- Höhere Auflösung der Achsenkopplung |
V0.69k H.Buss 31.05.2008 |
- Bug in SPI.C behoben |
- in 0.69h war ein Bug, der zu ungewollten Loopings führen konnte |
V0.69L H.Buss 14.06.2008 |
- feinere Cam-Servo-Auflösung |
V0.70a H.Buss 01.07.2008 |
- Unterstützung der V1.3-Hardware mit automatischem Hardware-Gyro-Abgleich |
V0.70b H.Buss 14.07.2008 |
- flexible Einstellungsmöglichkeit von J16 und J17 (Transistorausgänge) |
- eigene Parameter für GPS-Naviboard |
- eigener Parameter für ExternalControl (war vorher UserParameter1 bzw. 8) |
- neue Parameter im EEPROM-Datensatz: J16Bitmask, J16Timing, ExternalControl, Navi... |
- MikroKopterFlags eingeführt, damit das Navi den Status des MKs kennt |
- KopterTool-Kompatibilität auf 8 erhöht |
V0.70c H.Buss 30.07.2008 |
- Parameter der Datenfusion leicht modifiziert |
- EEPROM-Parameter für Looping-Umschlag angepasst (von 100 auf 85) |
- MaxStick wird auf 100 begrenzt |
V0.70d H.Buss 02.08.2008 |
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an |
Anpassungen bzgl. V0.70d |
G.Stobrawa 02.08.2008: |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |
- PPM24 Support für bis zu 12 RC-Kanäle. |
- 2. Uart wird nun unterstützt (MCU = atmega644p im Makefile) |
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard |
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC und Conrad UBLOX Modul |
- Makefile: EXT=KILLAGREG Unterstützung vom KillagregBoard mit MM3 und Conrad UBLOX Modul |
- Ausertung des UBX-Protocols an 1. oder 2. Uart |
- GPS-Hold-Funktion hinzugefügt |
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht) |
- Poti3 steuert die GPS Funktionen (Poti3 < 70:GPS inaktiv, 70<=Poti3<160: GPS Hold, 160<=Poti3: GPS Home) |
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter |
/branches/V0.70d CRK HexaLotte |
---|
Property changes: |
Added: tsvn:logminsize |
+8 |
\ No newline at end of property |