/branches/V0.75b_Ligi_ExternEvents/FlightCtrl.aps |
---|
0,0 → 1,0 |
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>11-Oct-2007 22:58:54</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>F:\SVN\MikroKopter\FlightCtrl\branches\V0.64_ZeroWarnings\</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><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>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></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>eeprom.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>menu.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>printf_P.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>rc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer0.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>twimaster.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart.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 -DVERSION_HAUPTVERSION=0 -DVERSION_NEBENVERSION=64 -DVERSION_KOMPATIBEL=5</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Program Files\WinAVR\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Program Files\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>Settings.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>timer0.h</FileName><Status>1</Status></File00018></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio> |
/branches/V0.75b_Ligi_ExternEvents/GPS.c |
---|
0,0 → 1,32 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
signed int GPS_Nick2 = 0; |
signed int GPS_Roll2 = 0; |
long GpsAktuell_X = 0; |
long GpsAktuell_Y = 0; |
long GpsZiel_X = 0; |
long GpsZiel_Y = 0; |
void GPS_Neutral(void) |
{ |
GpsZiel_X = GpsAktuell_X; |
GpsZiel_Y = GpsAktuell_Y; |
} |
void GPS_BerechneZielrichtung(void) |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
/branches/V0.75b_Ligi_ExternEvents/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.75b_Ligi_ExternEvents/Settings.h |
---|
--- branches/V0.75b_Ligi_ExternEvents/Spectrum.c (nonexistent) |
+++ branches/V0.75b_Ligi_ExternEvents/Spectrum.c (revision 1234) |
@@ -0,0 +1,318 @@ |
+/*####################################################################################### |
+Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit |
+#######################################################################################*/ |
+ |
+#include "Spectrum.h" |
+#include "main.h" |
+ |
+//--------------------------------------------------------------// |
+ |
+//--------------------------------------------------------------// |
+void SpektrumBinding(void) |
+{ |
+ unsigned int timerTimeout = SetDelay(10000); // Timeout 10 sec. |
+ unsigned char connected = 0; |
+ unsigned int delaycounter; |
+ |
+ UCSR1B &= ~(1 << RXCIE1); // disable rx-interrupt |
+ UCSR1B &= ~(1<<RXEN1); // disable Uart-Rx |
+ PORTD &= ~(1 << PORTD2); // disable pull-up |
+ |
+ printf("\n\rPlease connect Spektrum receiver for binding NOW..."); |
+ |
+ while(!CheckDelay(timerTimeout)) |
+ { |
+ if (PIND & (1 << PORTD2)) { timerTimeout = SetDelay(90); connected = 1; break; } |
+ } |
+ |
+ if (connected) |
+ { |
+ |
+ printf("ok.\n\r"); |
+ DDRD |= (1 << DDD2); // Rx as output |
+ |
+ while(!CheckDelay(timerTimeout)); // delay after startup of RX |
+ for (delaycounter = 0; delaycounter < 100; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2); |
+ for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2); |
+ |
+ } |
+ else |
+ { printf("Timeout.\n\r"); |
+ |
+ |
+ } |
+ |
+ DDRD &= ~(1 << DDD2); // RX as input |
+ PORTD &= ~(1 << PORTD2); |
+ |
+ Uart1Init(); // init Uart again |
+} |
+ |
+//############################################################################ |
+// zum Decodieren des Spektrum Satelliten wird USART1 benutzt. |
+// USART1 initialisation from killagreg |
+void Uart1Init(void) |
+//############################################################################ |
+ { |
+ // -- Start of USART1 initialisation for Spekturm seriell-mode |
+ // USART1 Control and Status Register A, B, C and baud rate register |
+ uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1); |
+ // disable RX-Interrupt |
+ UCSR1B &= ~(1 << RXCIE1); |
+ // disable TX-Interrupt |
+ UCSR1B &= ~(1 << TXCIE1); |
+ // disable DRE-Interrupt |
+ UCSR1B &= ~(1 << UDRIE1); |
+ // set direction of RXD1 and TXD1 pins |
+ // set RXD1 (PD2) as an input pin |
+ PORTD |= (1 << PORTD2); |
+ DDRD &= ~(1 << DDD2); |
+ // USART0 Baud Rate Register |
+ // set clock divider |
+ |
+ UBRR1H = (uint8_t)(ubrr>>8); |
+ UBRR1L = (uint8_t)ubrr; |
+ // enable double speed operation |
+ UCSR1A |= (1 << U2X1); |
+ // enable receiver and transmitter |
+ //UCSR1B = (1<<RXEN1)|(1<<TXEN1); |
+ |
+ |
+ |
+ |
+ UCSR1B = (1<<RXEN1); |
+ // set asynchronous mode |
+ UCSR1C &= ~(1 << UMSEL11); |
+ UCSR1C &= ~(1 << UMSEL10); |
+ // no parity |
+ UCSR1C &= ~(1 << UPM11); |
+ UCSR1C &= ~(1 << UPM10); |
+ // 1 stop bit |
+ UCSR1C &= ~(1 << USBS1); |
+ // 8-bit |
+ UCSR1B &= ~(1 << UCSZ12); |
+ UCSR1C |= (1 << UCSZ11); |
+ UCSR1C |= (1 << UCSZ10); |
+ // flush receive buffer explicit |
+ while(UCSR1A & (1<<RXC1)) UDR1; |
+ // enable RX-interrupts at the end |
+ UCSR1B |= (1 << RXCIE1); |
+ // -- End of USART1 initialisation |
+ return; |
+ } |
+ |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// + Copyright (c) Rainer Walther |
+// + RC-routines from original MK rc.c (c) H&I |
+// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2 |
+// + only for non-profit use |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// |
+// 20080808 rw Modified for Spektrum AR6100 (PPM) |
+// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only) |
+// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels) |
+// Replace AR6100-coding with original composit-signal routines |
+// |
+// --- |
+// Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben! |
+// Binding is not implemented. Bind with external Receiver. |
+// Servo output J3, J4, J5 not serviced |
+// |
+// Anschuß Spektrum Receiver |
+// Orange: 3V von der FC (keinesfalls an 5V anschließen!) |
+// Schwarz: GND |
+// Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker |
+// |
+// --- |
+// Satellite-Reciever connected on USART1: |
+// |
+// DX7/DX6i: One data-frame at 115200 baud every 22ms. |
+// DX7se: One data-frame at 115200 baud every 11ms. |
+// byte1: unknown |
+// byte2: unknown |
+// byte3: and byte4: channel data (FLT-Mode) |
+// byte5: and byte6: channel data (Roll) |
+// byte7: and byte8: channel data (Nick) |
+// byte9: and byte10: channel data (Gier) |
+// byte11: and byte12: channel data (Gear Switch) |
+// byte13: and byte14: channel data (Gas) |
+// byte15: and byte16: channel data (AUX2) |
+// |
+// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9 |
+// 1st Frame: |
+// byte1: unknown |
+// byte2: unknown |
+// byte3: and byte4: channel data |
+// byte5: and byte6: channel data |
+// byte7: and byte8: channel data |
+// byte9: and byte10: channel data |
+// byte11: and byte12: channel data |
+// byte13: and byte14: channel data |
+// byte15: and byte16: channel data |
+// 2nd Frame: |
+// byte1: unknown |
+// byte2: unknown |
+// byte3: and byte4: channel data |
+// byte5: and byte6: channel data |
+// byte7: and byte8: 0xffff |
+// byte9: and byte10: 0xffff |
+// byte11: and byte12: 0xffff |
+// byte13: and byte14: 0xffff |
+// byte15: and byte16: 0xffff |
+// |
+// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as: |
+// |
+// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0 |
+// |
+// 0 means a '0' bit |
+// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only) |
+// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter) |
+// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel |
+// |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+//############################################################################ |
+//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever |
+SIGNAL(USART1_RX_vect) |
+//############################################################################ |
+{ |
+static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer; |
+ unsigned int Channel, index; |
+ signed int signal, tmp; |
+ int bCheckDelay; |
+ uint8_t c; |
+ |
+ c = UDR1; // get data byte |
+ |
+ if (ReSync == 1) |
+ { |
+ // wait for beginning of new frame |
+ ReSync = 0; |
+ |
+ FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames |
+ FrameCnt = 0; |
+ Sync = 0; |
+ ByteHigh = 0; |
+ } |
+ else |
+ { |
+ bCheckDelay = CheckDelay(FrameTimer); |
+ if ( Sync == 0 ) |
+ { |
+ if(bCheckDelay) |
+ { |
+ // nach einer Pause von mind. 7ms erstes Sync-Character gefunden |
+ // Zeichen ignorieren, da Bedeutung unbekannt |
+ Sync = 1; |
+ FrameCnt ++; |
+ } |
+ else |
+ { |
+ // Zeichen kam vor Ablauf der 7ms Sync-Pause |
+ // warten auf erstes Sync-Zeichen |
+ } |
+ } |
+ else if((Sync == 1) && !bCheckDelay) |
+ { |
+ // zweites Sync-Character ignorieren, Bedeutung unbekannt |
+ Sync = 2; |
+ FrameCnt ++; |
+ } |
+ else if((Sync == 2) && !bCheckDelay) |
+ { |
+ // Datenbyte high |
+ ByteHigh = c; |
+ |
+ if (FrameCnt == 2) |
+ { |
+ // is 1st Byte of Channel-data |
+ // Frame 1 with Channel 1-7 comming next |
+ Frame2 = 0; |
+ if(ByteHigh & 0x80) |
+ { |
+ // DS9: Frame 2 with Channel 8-9 comming next |
+ Frame2 = 1; |
+ } |
+ } |
+ Sync = 3; |
+ FrameCnt ++; |
+ } |
+ else if((Sync == 3) && !bCheckDelay) |
+ { |
+ // Datenbyte low |
+ |
+ // High-Byte for next channel comes next |
+ Sync = 2; |
+ FrameCnt ++; |
+ |
+ index = (ByteHigh >> 2) & 0x0f; |
+ index ++; |
+ Channel = (ByteHigh << 8) | c; |
+ signal = Channel & 0x3ff; |
+ signal -= 0x200; // Offset, range 0x000..0x3ff? |
+ signal = signal/3; // scaling to fit PPM resolution |
+ |
+ if(index >= 0 && index <= 10) |
+ { |
+ // Stabiles Signal |
+ if(abs(signal - PPM_in[index]) < 6) |
+ { |
+ if(SenderOkay < 200) SenderOkay += 10; |
+ else |
+ { |
+ SenderOkay = 200; |
+ TIMSK1 &= ~_BV(ICIE1); // disable PPM-Input |
+ } |
+ } |
+ tmp = (3 * (PPM_in[index]) + signal) / 4; |
+ if(tmp > signal+1) tmp--; else |
+ if(tmp < signal-1) tmp++; |
+ if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
+ else PPM_diff[index] = 0; |
+ PPM_in[index] = tmp; |
+ } |
+ } |
+ else |
+ { |
+ // hier stimmt was nicht: neu synchronisieren |
+ ReSync = 1; |
+ FrameCnt = 0; |
+ Frame2 = 0; |
+ } |
+ |
+ // 16 Bytes per frame |
+ if(FrameCnt >= 16) |
+ { |
+ // Frame complete |
+ if(Frame2 == 0) |
+ { |
+ // Null bedeutet: Neue Daten |
+ // nur beim ersten Frame (CH 0-7) setzen |
+ NewPpmData = 0; |
+ } |
+ |
+ // new frame next, nach fruehestens 7ms erwartet |
+ FrameCnt = 0; |
+ Frame2 = 0; |
+ Sync = 0; |
+ } |
+ // Zeit bis zum nächsten Zeichen messen |
+ FrameTimer = SetDelay(7); |
+ } |
+} |
+ |
+ |
/branches/V0.75b_Ligi_ExternEvents/Spectrum.h |
---|
0,0 → 1,9 |
/*####################################################################################### |
Dekodieren eines Spectrum Signals |
#######################################################################################*/ |
#ifndef _SPECTRUM_H |
#define _SPECTRUM_H |
void Uart1Init(void); |
void SpektrumBinding(void); |
#endif //_RC_H |
/branches/V0.75b_Ligi_ExternEvents/_Settings.h |
---|
0,0 → 1,43 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Abstimmung |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define ACC_AMPLIFY 6 |
#define FAKTOR_P 1 |
#define FAKTOR_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 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Sender |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define K_NICK 0 |
#define K_ROLL 1 |
#define K_GAS 2 |
#define K_GIER 3 |
#define K_POTI1 4 |
#define K_POTI2 5 |
#define K_POTI3 6 |
#define K_POTI4 7 |
// +++++++++++++++++++++++++++++++ |
// + 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.75b_Ligi_ExternEvents/analog.c |
---|
0,0 → 1,459 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100; |
volatile int AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0; |
volatile int HiResNick = 2500, HiResRoll = 2500; |
volatile int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
volatile char messanzahl_AccHoch = 0; |
volatile long Luftdruck = 32000; |
volatile int StartLuftdruck; |
volatile unsigned int MessLuftdruck = 1023; |
unsigned char DruckOffsetSetting; |
signed char ExpandBaro = 0; |
volatile int HoeheD = 0; |
volatile char messanzahl_Druck; |
volatile int tmpLuftdruck; |
volatile unsigned int ZaehlMessungen = 0; |
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115; |
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0; |
volatile unsigned char AdReady = 1; |
//####################################################################################### |
// |
void ADC_Init(void) |
//####################################################################################### |
{ |
ADMUX = 0;//Referenz ist extern |
ANALOG_ON; |
} |
void SucheLuftruckOffset(void) |
{ |
unsigned int off; |
off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]); |
if(off > 20) off -= 10; |
OCR0A = off; |
ExpandBaro = 0; |
Delay_ms_Mess(100); |
if(MessLuftdruck < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms_Mess(50); |
printf("."); |
if(MessLuftdruck < 850) break; |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off); |
DruckOffsetSetting = off; |
Delay_ms_Mess(300); |
} |
void SucheGyroOffset(void) |
{ |
unsigned char i, ready = 0; |
int timeout; |
GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0; |
timeout = SetDelay(2000); |
for(i=140; i != 0; i--) |
{ |
if(ready == 3 && i > 10) i = 9; |
ready = 0; |
if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++; |
if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++; |
if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++; |
twi_state = 8; |
i2c_start(); |
if(AnalogOffsetNick < 10) { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;}; |
if(AnalogOffsetRoll < 10) { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;}; |
if(AnalogOffsetGier < 10) { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;}; |
while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;} |
messanzahl_Druck = 0; |
ANALOG_ON; |
while(messanzahl_Druck == 0); |
if(i<10) Delay_ms_Mess(10); |
} |
Delay_ms_Mess(70); |
} |
/* |
0 n |
1 r |
2 g |
3 y |
4 x |
5 n |
6 r |
7 u |
8 z |
9 L |
10 n |
11 r |
12 g |
13 y |
14 x |
15 n |
16 r |
17 L |
*/ |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static signed int gier1, roll1, nick1, nick_filter, roll_filter; |
static signed int accy, accx; |
switch(state++) |
{ |
case 0: |
nick1 = ADC; |
kanal = AD_ROLL; |
break; |
case 1: |
roll1 = ADC; |
kanal = AD_GIER; |
break; |
case 2: |
gier1 = ADC; |
kanal = AD_ACC_Y; |
break; |
case 3: |
Aktuell_ay = NeutralAccY - ADC; |
accy = Aktuell_ay; |
kanal = AD_ACC_X; |
break; |
case 4: |
Aktuell_ax = ADC - NeutralAccX; |
accx = Aktuell_ax; |
kanal = AD_NICK; |
break; |
case 5: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 6: |
roll1 += ADC; |
kanal = AD_UBAT; |
break; |
case 7: |
UBat = (3 * UBat + ADC / 3) / 4; |
kanal = AD_ACC_Z; |
break; |
case 8: |
AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
if(AdWertAccHoch > 1) |
{ |
if(NeutralAccZ < 750) |
{ |
NeutralAccZ += 0.02; |
if(modell_fliegt < 500) NeutralAccZ += 0.1; |
} |
} |
else if(AdWertAccHoch < -1) |
{ |
if(NeutralAccZ > 550) |
{ |
NeutralAccZ-= 0.02; |
if(modell_fliegt < 500) NeutralAccZ -= 0.1; |
} |
} |
messanzahl_AccHoch = 1; |
Aktuell_az = ADC; |
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
kanal = AD_DRUCK; |
break; |
// "case 8:" fehlt hier absichtlich |
case 10: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 11: |
roll1 += ADC; |
kanal = AD_GIER; |
break; |
case 12: |
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 1) / 2; |
else |
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1); |
else AdWertGier = (ADC + gier1); |
kanal = AD_ACC_Y; |
break; |
case 13: |
Aktuell_ay = NeutralAccY - ADC; |
AdWertAccRoll = (Aktuell_ay + accy); |
kanal = AD_ACC_X; |
break; |
case 14: |
Aktuell_ax = ADC - NeutralAccX; |
AdWertAccNick = (Aktuell_ax + accx); |
kanal = AD_NICK; |
break; |
case 15: |
nick1 += ADC; |
if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4; |
AdWertNick = nick1 / 8; |
nick_filter = (nick_filter + nick1) / 2; |
HiResNick = nick_filter - AdNeutralNick; |
AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2; |
kanal = AD_ROLL; |
break; |
case 16: |
roll1 += ADC; |
if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4; |
AdWertRoll = roll1 / 8; |
roll_filter = (roll_filter + roll1) / 2; |
HiResRoll = roll_filter - AdNeutralRoll; |
AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2; |
kanal = AD_DRUCK; |
break; |
case 17: |
state = 0; |
AdReady = 1; |
ZaehlMessungen++; |
// "break" fehlt hier absichtlich |
case 9: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) |
{ |
tmpLuftdruck /= 2; |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32; // D-Anteil = neuerWert - AlterWert |
Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8; |
HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; |
tmpLuftdruck /= 2; |
} |
kanal = AD_NICK; |
break; |
default: |
kanal = 0; state = 0; kanal = AD_NICK; |
break; |
} |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
} |
/* |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static signed int gier1, roll1, nick1; |
static signed long nick_filter, roll_filter; |
static signed int accy, accx; |
switch(state++) |
{ |
case 0: |
nick1 = ADC; |
kanal = AD_ROLL; |
break; |
case 1: |
roll1 = ADC; |
kanal = AD_GIER; |
break; |
case 2: |
gier1 = ADC; |
kanal = AD_ACC_Y; |
break; |
case 3: |
Aktuell_ay = NeutralAccY - ADC; |
accy = Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 4: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 5: |
roll1 += ADC; |
kanal = AD_ACC_Z; |
break; |
case 6: |
AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
if(AdWertAccHoch > 1) |
{ |
if(NeutralAccZ < 750) |
{ |
NeutralAccZ += 0.02; |
if(modell_fliegt < 500) NeutralAccZ += 0.1; |
} |
} |
else if(AdWertAccHoch < -1) |
{ |
if(NeutralAccZ > 550) |
{ |
NeutralAccZ-= 0.02; |
if(modell_fliegt < 500) NeutralAccZ -= 0.1; |
} |
} |
messanzahl_AccHoch = 1; |
Aktuell_az = ADC; |
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
kanal = AD_NICK; |
break; |
case 7: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 8: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 9: |
Aktuell_ax = ADC - NeutralAccX; |
accx = Aktuell_ax; |
kanal = AD_GIER; |
break; |
case 10: |
gier1 += ADC; |
kanal = AD_NICK; |
break; |
case 11: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 12: |
roll1 += ADC; |
kanal = AD_UBAT; |
break; |
case 13: |
UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2; |
kanal = AD_ACC_Y; |
break; |
case 14: |
Aktuell_ay = NeutralAccY - ADC; |
accy += Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 15: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 16: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 17: |
Aktuell_ax = ADC - NeutralAccX; |
accx += Aktuell_ax; |
kanal = AD_NICK; |
break; |
case 18: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 19: |
roll1 += ADC; |
kanal = AD_GIER; |
break; |
case 20: |
gier1 += ADC; |
kanal = AD_ACC_Y; |
break; |
case 21: |
Aktuell_ay = NeutralAccY - ADC; |
accy += Aktuell_ay; |
kanal = AD_NICK; |
break; |
case 22: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 23: |
roll1 += ADC; |
kanal = AD_DRUCK; |
break; |
case 24: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) |
{ |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8; // D-Anteil = neuerWert - AlterWert |
Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; |
tmpLuftdruck = 0; |
} |
kanal = AD_NICK; |
break; |
case 25: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 26: |
roll1 += ADC; |
kanal = AD_ACC_X; |
break; |
case 27: |
Aktuell_ax = ADC - NeutralAccX; |
accx += Aktuell_ax; |
kanal = AD_GIER; |
break; |
case 28: |
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1 + 2) / 4; |
else |
if(PlatinenVersion == 20) AdWertGier = 2047 - (ADC + gier1 + 1) / 2; |
else AdWertGier = (ADC + gier1 + 1) / 2; |
kanal = AD_NICK; |
break; |
case 29: |
nick1 += ADC; |
kanal = AD_ROLL; |
break; |
case 30: |
roll1 += ADC; |
kanal = AD_ACC_Y; |
break; |
case 31: |
Aktuell_ay = NeutralAccY - ADC; |
AdWertAccRoll = (Aktuell_ay + accy); |
kanal = AD_NICK; |
break; |
case 32: |
AdWertNick = (ADC + nick1 + 3) / 5; |
nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2; |
if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;} |
HiResNick = nick_filter - 20 * AdNeutralNick; |
AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4; |
DebugOut.Analog[21] = AdWertNickFilter / 4; |
kanal = AD_ROLL; |
break; |
case 33: |
AdWertRoll = (ADC + roll1 + 3) / 5; |
roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2; |
if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;} |
HiResRoll = roll_filter - 20 * AdNeutralRoll; |
AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4; |
DebugOut.Analog[22] = AdWertRollFilter / 4; |
kanal = AD_ACC_X; |
break; |
case 34: |
Aktuell_ax = ADC - NeutralAccX; |
AdWertAccNick = (Aktuell_ax + accx); |
kanal = AD_NICK; |
state = 0; |
AdReady = 1; |
ZaehlMessungen++; |
break; |
default: |
kanal = 0; |
state = 0; |
break; |
} |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
} |
*/ |
/branches/V0.75b_Ligi_ExternEvents/analog.h |
---|
0,0 → 1,45 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
/*####################################################################################### |
#######################################################################################*/ |
extern volatile int UBat; |
extern volatile int AdWertNick, AdWertRoll, AdWertGier; |
extern volatile int AdWertAccRoll,AdWertAccNick,AdWertAccHoch; |
extern volatile int HiResNick, HiResRoll; |
extern volatile int AdWertNickFilter, AdWertRollFilter, AdWertGierFilter; |
extern volatile int Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az; |
extern volatile long Luftdruck; |
extern volatile char messanzahl_Druck; |
extern volatile unsigned int ZaehlMessungen; |
extern unsigned char DruckOffsetSetting; |
extern signed char ExpandBaro; |
extern volatile int HoeheD; |
extern volatile unsigned int MessLuftdruck; |
extern volatile int StartLuftdruck; |
extern volatile char MessanzahlNick; |
extern unsigned char AnalogOffsetNick,AnalogOffsetRoll,AnalogOffsetGier; |
extern volatile unsigned char AdReady; |
unsigned int ReadADC(unsigned char adc_input); |
void ADC_Init(void); |
void SucheLuftruckOffset(void); |
void SucheGyroOffset(void); |
#define AD_GIER 0 |
#define AD_ROLL 1 |
#define AD_NICK 2 |
#define AD_DRUCK 3 |
#define AD_UBAT 4 |
#define AD_ACC_Z 5 |
#define AD_ACC_Y 6 |
#define AD_ACC_X 7 |
#define ANALOG_OFF ADCSRA=0 |
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
//Signle trigger Mode, Interrupt on |
#endif //_ANALOG_H |
/branches/V0.75b_Ligi_ExternEvents/eeprom.c |
---|
0,0 → 1,248 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Konstanten |
// + 0-250 -> normale Werte |
// + 251 -> Poti1 |
// + 252 -> Poti2 |
// + 253 -> Poti3 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
void DefaultStickMapping(void) |
{ |
EE_Parameter.Kanalbelegung[K_NICK] = 1; |
EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
EE_Parameter.Kanalbelegung[K_GAS] = 3; |
EE_Parameter.Kanalbelegung[K_GIER] = 4; |
EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
} |
void DefaultKonstanten1(void) |
{ |
EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;//CFG_HOEHEN_SCHALTER |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
EE_Parameter.Stick_P = 14; // Wert : 1-6 |
EE_Parameter.Stick_D = 16; // Wert : 0-64 |
EE_Parameter.Gier_P = 12; // Wert : 1-20 |
EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.Gyro_D = 3; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 32; // Wert : 0-250 ( Automatische Zellenerkennung bei < 50) |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 32; |
EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 3; |
EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
EE_Parameter.AchsKopplung1 = 90; |
EE_Parameter.AchsKopplung2 = 80; |
EE_Parameter.CouplingYawCorrection = 1; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 16; // 1/k |
EE_Parameter.Driftkomp = 32; |
EE_Parameter.DynamicStability = 100; |
EE_Parameter.J16Bitmask = 95; |
EE_Parameter.J17Bitmask = 243; |
EE_Parameter.J16Timing = 15; |
EE_Parameter.J17Timing = 15; |
EE_Parameter.NaviGpsModeControl = 253; |
EE_Parameter.NaviGpsGain = 100; |
EE_Parameter.NaviGpsP = 90; |
EE_Parameter.NaviGpsI = 90; |
EE_Parameter.NaviGpsD = 90; |
EE_Parameter.NaviGpsPLimit = 75; |
EE_Parameter.NaviGpsILimit = 75; |
EE_Parameter.NaviGpsDLimit = 75; |
EE_Parameter.NaviGpsACC = 0; |
EE_Parameter.NaviGpsMinSat = 6; |
EE_Parameter.NaviStickThreshold = 8; |
EE_Parameter.NaviWindCorrection = 90; |
EE_Parameter.NaviSpeedCompensation = 30; |
EE_Parameter.NaviOperatingRadius = 100; |
EE_Parameter.NaviAngleLimitation = 100; |
EE_Parameter.NaviPH_LoginTime = 4; |
memcpy(EE_Parameter.Name, "Sport\0", 12); |
} |
void DefaultKonstanten2(void) |
{ |
EE_Parameter.GlobalConfig = CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 3; // Wert : 0-50 |
EE_Parameter.Stick_P = 10; // Wert : 1-6 |
EE_Parameter.Stick_D = 16; // Wert : 0-64 |
EE_Parameter.Gier_P = 6; // Wert : 1-20 |
EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 90; // Wert : 0-250 |
EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
EE_Parameter.Gyro_D = 3; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 32; // Wert : 0-250 ( Automatische Zellenerkennung bei < 50) |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 32; |
EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 3; |
EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
EE_Parameter.AchsKopplung1 = 90; |
EE_Parameter.AchsKopplung2 = 80; |
EE_Parameter.CouplingYawCorrection = 60; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 32; // 1/k |
EE_Parameter.Driftkomp = 32; |
EE_Parameter.DynamicStability = 75; |
EE_Parameter.J16Bitmask = 95; |
EE_Parameter.J17Bitmask = 243; |
EE_Parameter.J16Timing = 20; |
EE_Parameter.J17Timing = 20; |
EE_Parameter.NaviGpsModeControl = 253; |
EE_Parameter.NaviGpsGain = 100; |
EE_Parameter.NaviGpsP = 90; |
EE_Parameter.NaviGpsI = 90; |
EE_Parameter.NaviGpsD = 90; |
EE_Parameter.NaviGpsPLimit = 75; |
EE_Parameter.NaviGpsILimit = 75; |
EE_Parameter.NaviGpsDLimit = 75; |
EE_Parameter.NaviGpsACC = 0; |
EE_Parameter.NaviGpsMinSat = 6; |
EE_Parameter.NaviStickThreshold = 8; |
EE_Parameter.NaviWindCorrection = 90; |
EE_Parameter.NaviSpeedCompensation = 30; |
EE_Parameter.NaviOperatingRadius = 100; |
EE_Parameter.NaviAngleLimitation = 100; |
EE_Parameter.NaviPH_LoginTime = 4; |
memcpy(EE_Parameter.Name, "Normal\0", 12); |
} |
void DefaultKonstanten3(void) |
{ |
EE_Parameter.GlobalConfig = CFG_DREHRATEN_BEGRENZER | CFG_ACHSENKOPPLUNG_AKTIV | CFG_KOMPASS_AKTIV | CFG_GPS_AKTIV;///*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
EE_Parameter.Hoehe_MinGas = 30; |
EE_Parameter.MaxHoehe = 251; // Wert : 0-250 251 -> Poti1 |
EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
EE_Parameter.Luftdruck_D = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250 |
EE_Parameter.Hoehe_Verstaerkung = 3; // Wert : 0-50 |
EE_Parameter.Stick_P = 8; // Wert : 1-6 |
EE_Parameter.Stick_D = 16; // Wert : 0-64 |
EE_Parameter.Gier_P = 6; // Wert : 1-20 |
EE_Parameter.Gas_Min = 8; // Wert : 0-32 |
EE_Parameter.Gas_Max = 230; // Wert : 33-250 |
EE_Parameter.GyroAccFaktor = 30; // Wert : 1-64 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 100; // Wert : 0-250 |
EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
EE_Parameter.Gyro_D = 3; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 32; // Wert : 0-250 ( Automatische Zellenerkennung bei < 50) |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 16; |
EE_Parameter.UserParam1 = 0; // zur freien Verwendung |
EE_Parameter.UserParam2 = 0; // zur freien Verwendung |
EE_Parameter.UserParam3 = 0; // zur freien Verwendung |
EE_Parameter.UserParam4 = 0; // zur freien Verwendung |
EE_Parameter.UserParam5 = 0; // zur freien Verwendung |
EE_Parameter.UserParam6 = 0; // zur freien Verwendung |
EE_Parameter.UserParam7 = 0; // zur freien Verwendung |
EE_Parameter.UserParam8 = 0; // zur freien Verwendung |
EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 3; |
EE_Parameter.ServoRollControl = 100; // Wert : 0-250 // Stellung des Servos |
EE_Parameter.ServoRollComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
EE_Parameter.ServoRollMin = 0; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoRollMax = 250; // Wert : 0-250 // Anschlag |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
EE_Parameter.AchsKopplung1 = 90; |
EE_Parameter.AchsKopplung2 = 80; |
EE_Parameter.CouplingYawCorrection = 70; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 32; // 1/k |
EE_Parameter.Driftkomp = 32; |
EE_Parameter.DynamicStability = 50; |
EE_Parameter.J16Bitmask = 95; |
EE_Parameter.J17Bitmask = 243; |
EE_Parameter.J16Timing = 30; |
EE_Parameter.J17Timing = 30; |
EE_Parameter.NaviGpsModeControl = 253; |
EE_Parameter.NaviGpsGain = 100; |
EE_Parameter.NaviGpsP = 90; |
EE_Parameter.NaviGpsI = 90; |
EE_Parameter.NaviGpsD = 90; |
EE_Parameter.NaviGpsPLimit = 75; |
EE_Parameter.NaviGpsILimit = 75; |
EE_Parameter.NaviGpsDLimit = 75; |
EE_Parameter.NaviGpsACC = 0; |
EE_Parameter.NaviGpsMinSat = 6; |
EE_Parameter.NaviStickThreshold = 8; |
EE_Parameter.NaviWindCorrection = 90; |
EE_Parameter.NaviSpeedCompensation = 30; |
EE_Parameter.NaviOperatingRadius = 100; |
EE_Parameter.NaviAngleLimitation = 100; |
EE_Parameter.NaviPH_LoginTime = 4; |
memcpy(EE_Parameter.Name, "Beginner\0", 12); |
} |
/branches/V0.75b_Ligi_ExternEvents/fc.c |
---|
0,0 → 1,1460 |
/*####################################################################################### |
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 "main.h" |
#include "eeprom.c" |
unsigned char h,m,s; |
volatile unsigned int I2CTimeout = 100; |
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
int TrimNick, TrimRoll; |
int AdNeutralGierBias; |
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
volatile float NeutralAccZ = 0; |
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
long IntegralNick = 0,IntegralNick2 = 0; |
long IntegralRoll = 0,IntegralRoll2 = 0; |
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
long Integral_Gier = 0; |
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
volatile long Mess_Integral_Hoch = 0; |
int KompassValue = 0; |
int KompassStartwert = 0; |
int KompassRichtung = 0; |
unsigned int KompassSignalSchlecht = 500; |
unsigned char MAX_GAS,MIN_GAS; |
unsigned char Notlandung = 0; |
unsigned char HoehenReglerAktiv = 0; |
unsigned char TrichterFlug = 0; |
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
long ErsatzKompass; |
int ErsatzKompassInGrad; // Kompasswert in Grad |
int GierGyroFehler = 0; |
char GyroFaktor,GyroFaktorGier; |
char IntegralFaktor,IntegralFaktorGier; |
int DiffNick,DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
char MotorenEin = 0; |
int HoehenWert = 0; |
int SollHoehe = 0; |
int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
//float Ki = FAKTOR_I; |
int Ki = 10300 / 33; |
unsigned char Looping_Nick = 0,Looping_Roll = 0; |
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
unsigned char Parameter_UserParam1 = 0; |
unsigned char Parameter_UserParam2 = 0; |
unsigned char Parameter_UserParam3 = 0; |
unsigned char Parameter_UserParam4 = 0; |
unsigned char Parameter_UserParam5 = 0; |
unsigned char Parameter_UserParam6 = 0; |
unsigned char Parameter_UserParam7 = 0; |
unsigned char Parameter_UserParam8 = 0; |
unsigned char Parameter_ServoNickControl = 100; |
unsigned char Parameter_ServoRollControl = 100; |
unsigned char Parameter_LoopGasLimit = 70; |
unsigned char Parameter_AchsKopplung1 = 90; |
unsigned char Parameter_AchsKopplung2 = 65; |
unsigned char Parameter_CouplingYawCorrection = 64; |
//unsigned char Parameter_AchsGegenKopplung1 = 0; |
unsigned char Parameter_DynamicStability = 100; |
unsigned char Parameter_J16Bitmask; // for the J16 Output |
unsigned char Parameter_J16Timing; // for the J16 Output |
unsigned char Parameter_J17Bitmask; // for the J17 Output |
unsigned char Parameter_J17Timing; // for the J17 Output |
unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard |
unsigned char Parameter_NaviGpsGain; |
unsigned char Parameter_NaviGpsP; |
unsigned char Parameter_NaviGpsI; |
unsigned char Parameter_NaviGpsD; |
unsigned char Parameter_NaviGpsACC; |
unsigned char Parameter_NaviOperatingRadius; |
unsigned char Parameter_NaviWindCorrection; |
unsigned char Parameter_NaviSpeedCompensation; |
unsigned char Parameter_ExternalControl; |
struct mk_param_struct EE_Parameter; |
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
int MaxStickNick = 0,MaxStickRoll = 0; |
unsigned int modell_fliegt = 0; |
volatile unsigned char MikroKopterFlags = 0; |
long GIER_GRAD_FAKTOR = 1291; |
signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
unsigned char RequiredMotors = 4; |
unsigned char Motor[MAX_MOTORS]; |
signed int tmp_motorwert[MAX_MOTORS]; |
int MotorSmoothing(int neu, int alt) |
{ |
int motor; |
if(neu > alt) motor = (1*(int)alt + neu) / 2; |
else motor = neu - (alt - neu)*1; |
//if(Poti2 < 20) return(neu); |
return(motor); |
} |
void Piep(unsigned char Anzahl, unsigned int dauer) |
{ |
if(MotorenEin) return; //auf keinen Fall im Flug! |
while(Anzahl--) |
{ |
beeptime = dauer; |
while(beeptime); |
Delay_ms(dauer * 2); |
} |
} |
//############################################################################ |
// Nullwerte ermitteln |
void SetNeutral(void) |
//############################################################################ |
{ |
unsigned char i; |
unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
ServoActive = 0; HEF4017R_ON; |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralGier = 0; |
AdNeutralGierBias = 0; |
Parameter_AchsKopplung1 = 0; |
Parameter_AchsKopplung2 = 0; |
ExpandBaro = 0; |
CalibrierMittelwert(); |
Delay_ms_Mess(100); |
CalibrierMittelwert(); |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
#define NEUTRAL_FILTER 32 |
for(i=0; i<NEUTRAL_FILTER; i++) |
{ |
Delay_ms_Mess(10); |
gier_neutral += AdWertGier; |
nick_neutral += AdWertNick; |
roll_neutral += AdWertRoll; |
} |
AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
AdNeutralGierBias = AdNeutralGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
{ |
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
NeutralAccZ = Aktuell_az; |
} |
else |
{ |
NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]); |
NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]); |
NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]); |
} |
MesswertNick = 0; |
MesswertRoll = 0; |
MesswertGier = 0; |
Delay_ms_Mess(100); |
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
Mess_Integral_Gier = 0; |
StartLuftdruck = Luftdruck; |
HoeheD = 0; |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
GPS_Neutral(); |
beeptime = 50; |
Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
ExternHoehenValue = 0; |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
GierGyroFehler = 0; |
SendVersionToNavi = 1; |
LED_Init(); |
MikroKopterFlags |= FLAG_CALIBRATE; |
FromNaviCtrl_Value.Kalman_K = -1; |
FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110; |
Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110; |
Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110; |
Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110; |
// ServoActive = 1; |
SenderOkay = 100; |
} |
//############################################################################ |
// Bearbeitet die Messwerte |
void Mittelwert(void) |
//############################################################################ |
{ |
static signed long tmpl,tmpl2,tmpl3,tmpl4; |
static signed int oldNick, oldRoll, d2Roll, d2Nick; |
signed long winkel_nick, winkel_roll; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
// MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
MesswertNick = (signed int) AdWertNickFilter / 8; |
MesswertRoll = (signed int) AdWertRollFilter / 8; |
RohMesswertNick = MesswertNick; |
RohMesswertRoll = MesswertRoll; |
//DebugOut.Analog[21] = MesswertNick; |
//DebugOut.Analog[22] = MesswertRoll; |
//DebugOut.Analog[22] = Mess_Integral_Gier; |
//DebugOut.Analog[21] = MesswertNick; |
//DebugOut.Analog[22] = MesswertRoll; |
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L; |
Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L; |
Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
NaviAccNick += AdWertAccNick; |
NaviAccRoll += AdWertAccRoll; |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
ANALOG_ON; |
AdReady = 0; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L; |
else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L; |
else winkel_roll = Mess_IntegralRoll; |
if(Mess_IntegralNick > 93000L) winkel_nick = 93000L; |
else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L; |
else winkel_nick = Mess_IntegralNick; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_Integral_Gier += MesswertGier; |
ErsatzKompass += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
{ |
tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
tmpl3 *= Parameter_AchsKopplung2; //65 |
tmpl3 /= 4096L; |
tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
tmpl4 *= Parameter_AchsKopplung2; //65 |
tmpl4 /= 4096L; |
KopplungsteilNickRoll = tmpl3; |
KopplungsteilRollNick = tmpl4; |
tmpl4 -= tmpl3; |
ErsatzKompass += tmpl4; |
if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
tmpl *= Parameter_AchsKopplung1; // 90 |
tmpl /= 4096L; |
tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
tmpl2 *= Parameter_AchsKopplung1; |
tmpl2 /= 4096L; |
if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
} |
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
TrimRoll = tmpl - tmpl2 / 100L; |
TrimNick = -tmpl2 + tmpl / 100L; |
// Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
if(Mess_IntegralRoll > Umschlag180Roll) |
{ |
Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
} |
if(Mess_IntegralRoll <-Umschlag180Roll) |
{ |
Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
} |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mess_IntegralNick2 += MesswertNick + TrimNick; |
Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
if(Mess_IntegralNick > Umschlag180Nick) |
{ |
Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
if(Mess_IntegralNick <-Umschlag180Nick) |
{ |
Mess_IntegralNick = (Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
Integral_Gier = Mess_Integral_Gier; |
IntegralNick = Mess_IntegralNick; |
IntegralRoll = Mess_IntegralRoll; |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
#define D_LIMIT 128 |
MesswertNick = HiResNick / 8; |
MesswertRoll = HiResRoll / 8; |
if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
if(Parameter_Gyro_D) |
{ |
d2Nick = HiResNick - oldNick; |
oldNick = (oldNick + HiResNick)/2; |
if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
d2Roll = HiResRoll - oldRoll; |
oldRoll = (oldRoll + HiResRoll)/2; |
if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
} |
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
{ |
if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256); |
} |
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
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; |
} |
//############################################################################ |
// Messwerte beim Ermitteln der Nullage |
void CalibrierMittelwert(void) |
//############################################################################ |
{ |
if(PlatinenVersion == 13) SucheGyroOffset(); |
// ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
ANALOG_OFF; |
MesswertNick = AdWertNick; |
MesswertRoll = AdWertRoll; |
MesswertGier = AdWertGier; |
Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
Mittelwert_AccHoch = (long)AdWertAccHoch; |
// ADC einschalten |
ANALOG_ON; |
if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
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; |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
} |
//############################################################################ |
// Senden der Motorwerte per I2C-Bus |
void SendMotorData(void) |
//############################################################################ |
{ |
unsigned char i; |
if(!MotorenEin) |
{ |
MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
for(i=0;i<MAX_MOTORS;i++) |
{ |
if(!PC_MotortestActive) MotorTest[i] = 0; |
Motor[i] = MotorTest[i]; |
} |
if(PC_MotortestActive) PC_MotortestActive--; |
} |
else MikroKopterFlags |= FLAG_MOTOR_RUN; |
DebugOut.Analog[12] = Motor[0]; |
DebugOut.Analog[13] = Motor[1]; |
DebugOut.Analog[14] = Motor[3]; |
DebugOut.Analog[15] = Motor[2]; |
//Start I2C Interrupt Mode |
twi_state = 0; |
motor = 0; |
i2c_start(); |
} |
//############################################################################ |
// Trägt ggf. das Poti als Parameter ein |
void ParameterZuordnung(void) |
//############################################################################ |
{ |
#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,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; } |
CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255); |
CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255); |
CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); |
CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); |
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl,0,255); |
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255); |
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
Ki = 10300 / (Parameter_I_Faktor + 1); |
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
} |
void start_engines(void) |
{ |
} |
//############################################################################ |
// |
void MotorRegler(void) |
//############################################################################ |
{ |
int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
int GierMischanteil,GasMischanteil; |
static long SummeNick=0,SummeRoll=0; |
static long sollGier = 0,tmp_long,tmp_long2; |
static long IntegralFehlerNick = 0; |
static long IntegralFehlerRoll = 0; |
static unsigned int RcLostTimer; |
static unsigned char delay_neutral = 0; |
static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
static int hoehenregler = 0; |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
static long ausgleichNick, ausgleichRoll; |
int IntegralNickMalFaktor,IntegralRollMalFaktor; |
unsigned char i; |
Mittelwert(); |
if (ExternEvent.key!=0) |
{ |
switch((int)ExternEvent.key) |
{ |
case 1: |
// start engines |
modell_fliegt = 1; |
MotorenEin = 1; |
sollGier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
break; |
case 2: |
// stop engines |
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
break; |
} |
ExternEvent.key=0; |
} |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = StickGas; |
if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Empfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
if(!PcZugriff) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 15000; |
BeepMuster = 0x0c00; |
} |
} |
if(RcLostTimer) RcLostTimer--; |
else |
{ |
MotorenEin = 0; |
Notlandung = 0; |
} |
ROT_ON; |
if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMischanteil = EE_Parameter.NotGas; |
Notlandung = 1; |
PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
} |
else MotorenEin = 0; |
} |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang gut |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay > 140) |
{ |
Notlandung = 0; |
RcLostTimer = EE_Parameter.NotGasZeit * 50; |
if(GasMischanteil > 40 && MotorenEin) |
{ |
if(modell_fliegt < 0xffff) modell_fliegt++; |
} |
if((modell_fliegt < 256)) |
{ |
SummeNick = 0; |
SummeRoll = 0; |
if(modell_fliegt == 250) |
{ |
NeueKompassRichtungMerken = 1; |
sollGier = 0; |
Mess_Integral_Gier = 0; |
// Mess_Integral_Gier2 = 0; |
} |
} else MikroKopterFlags |= FLAG_FLY; |
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// auf Nullwerte kalibrieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
{ |
unsigned char setting=1; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
SetActiveParamSetNumber(setting); // aktiven Datensatz merken |
} |
// else |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
{ |
WinkelOut.CalcState = 1; |
beeptime = 1000; |
} |
else |
{ |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
SetNeutral(); |
ServoActive = 1; |
DDRD |=0x80; // enable J7 -> Servo signal |
Piep(GetActiveParamSetNumber(),120); |
} |
} |
} |
else |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
SetNeutral(); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256); |
Piep(GetActiveParamSetNumber(),120); |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120) |
{ |
// Starten |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Einschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
delay_einschalten = 200; |
modell_fliegt = 1; |
MotorenEin = 1; |
sollGier = 0; |
Mess_Integral_Gier = 0; |
Mess_Integral_Gier2 = 0; |
Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
MikroKopterFlags |= FLAG_START; |
} |
} |
else delay_einschalten = 0; |
//Auf Neutralwerte setzen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auschalten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
} |
} |
else delay_ausschalten = 0; |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// neue Werte von der Funke |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || Notlandung) |
{ |
static int stick_nick,stick_roll; |
ParameterZuordnung(); |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
if(StickGier > 2) StickGier -= 2; else |
if(StickGier < -2) StickGier += 2; else StickGier = 0; |
StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
GyroFaktor = (Parameter_Gyro_P + 10.0); |
IntegralFaktor = Parameter_Gyro_I; |
GyroFaktorGier = (Parameter_Gyro_P + 10.0); |
IntegralFaktorGier = Parameter_Gyro_I; |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
{ |
StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
StickGier += ExternControl.Gier; |
ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
if(StickGas < 0) StickGas = 0; |
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
if(GyroFaktor < 0) GyroFaktor = 0; |
if(IntegralFaktor < 0) IntegralFaktor = 0; |
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--; |
if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
else |
{ |
{ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; |
} |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
else |
{ |
if(Looping_Rechts) // Hysterese |
{ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; |
} |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
else |
{ |
if(Looping_Oben) // Hysterese |
{ |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; |
} |
} |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
else |
{ |
if(Looping_Unten) // Hysterese |
{ |
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
} |
} |
if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0; |
if(Looping_Oben || Looping_Unten) { Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0; |
} // Ende neue Funken-Werte |
if(Looping_Roll || Looping_Nick) |
{ |
if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
TrichterFlug = 1; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Notlandung) |
{ |
StickGier = 0; |
StickNick = 0; |
StickRoll = 0; |
GyroFaktor = 90; |
IntegralFaktor = 120; |
GyroFaktorGier = 90; |
IntegralFaktorGier = 120; |
Looping_Roll = 0; |
Looping_Nick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define ABGLEICH_ANZAHL 256L |
MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
MittelIntegralRoll += IntegralRoll; |
MittelIntegralNick2 += IntegralNick2; |
MittelIntegralRoll2 += IntegralRoll2; |
if(Looping_Nick || Looping_Roll) |
{ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
MittelIntegralNick = 0; |
MittelIntegralRoll = 0; |
MittelIntegralNick2 = 0; |
MittelIntegralRoll2 = 0; |
Mess_IntegralNick2 = Mess_IntegralNick; |
Mess_IntegralRoll2 = Mess_IntegralRoll; |
ZaehlMessungen = 0; |
LageKorrekturNick = 0; |
LageKorrekturRoll = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
{ |
long tmp_long, tmp_long2; |
if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/) |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
{ |
tmp_long /= 2; |
tmp_long2 /= 2; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
} |
else |
{ |
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
} |
//if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;} |
Mess_IntegralNick -= tmp_long; |
Mess_IntegralRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
{ |
static int cnt = 0; |
static char last_n_p,last_n_n,last_r_p,last_r_n; |
static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) |
{ |
MittelIntegralNick /= ABGLEICH_ANZAHL; |
MittelIntegralRoll /= ABGLEICH_ANZAHL; |
IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
#define MAX_I 0//(Poti2/10) |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)) |
{ |
LageKorrekturNick /= 2; |
LageKorrekturRoll /= 2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MittelIntegralNick2 /= ABGLEICH_ANZAHL; |
MittelIntegralRoll2 /= ABGLEICH_ANZAHL; |
tmp_long = IntegralNick2 - IntegralNick; |
tmp_long2 = IntegralRoll2 - IntegralRoll; |
//DebugOut.Analog[25] = MittelIntegralRoll2 / 26; |
IntegralFehlerNick = tmp_long; |
IntegralFehlerRoll = tmp_long2; |
Mess_IntegralNick2 -= IntegralFehlerNick; |
Mess_IntegralRoll2 -= IntegralFehlerRoll; |
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
if(EE_Parameter.Driftkomp) |
{ |
if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
} |
//DebugOut.Analog[22] = MittelIntegralRoll / 26; |
//DebugOut.Analog[24] = GierGyroFehler; |
GierGyroFehler = 0; |
/*DebugOut.Analog[17] = IntegralAccNick / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
*/ |
//DebugOut.Analog[21] = MittelIntegralNick / 26; |
//MittelIntegralRoll = MittelIntegralRoll; |
//DebugOut.Analog[28] = ausgleichNick; |
/* |
DebugOut.Analog[29] = ausgleichRoll; |
DebugOut.Analog[30] = LageKorrekturRoll * 10; |
*/ |
#define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
#define BEWEGUNGS_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4; |
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
{ |
if(IntegralFehlerNick > FEHLER_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8); |
ausgleichNick = IntegralFehlerNick / 8; |
if(ausgleichNick > 5000) ausgleichNick = 5000; |
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; |
} |
else last_n_p = 1; |
} else last_n_p = 0; |
if(IntegralFehlerNick < -FEHLER_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8); |
ausgleichNick = IntegralFehlerNick / 8; |
if(ausgleichNick < -5000) ausgleichNick = -5000; |
LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; |
} |
else last_n_n = 1; |
} else last_n_n = 0; |
} |
else |
{ |
cnt = 0; |
KompassSignalSchlecht = 1000; |
} |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4; |
ausgleichRoll = 0; |
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
{ |
if(IntegralFehlerRoll > FEHLER_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8); |
ausgleichRoll = IntegralFehlerRoll / 8; |
if(ausgleichRoll > 5000) ausgleichRoll = 5000; |
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; |
} |
else last_r_p = 1; |
} else last_r_p = 0; |
if(IntegralFehlerRoll < -FEHLER_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8); |
ausgleichRoll = IntegralFehlerRoll / 8; |
if(ausgleichRoll < -5000) ausgleichRoll = -5000; |
LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL; |
} |
else last_r_n = 1; |
} else last_r_n = 0; |
} else |
{ |
cnt = 0; |
KompassSignalSchlecht = 1000; |
} |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift; |
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
} |
else |
{ |
LageKorrekturRoll = 0; |
LageKorrekturNick = 0; |
TrichterFlug = 0; |
} |
if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MittelIntegralNick_Alt = MittelIntegralNick; |
MittelIntegralRoll_Alt = MittelIntegralRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
IntegralAccNick = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MittelIntegralNick = 0; |
MittelIntegralRoll = 0; |
MittelIntegralNick2 = 0; |
MittelIntegralRoll2 = 0; |
ZaehlMessungen = 0; |
} // ZaehlMessungen >= ABGLEICH_ANZAHL |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
if(abs(StickGier) > 15) // war 35 |
{ |
KompassSignalSchlecht = 1000; |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
{ |
NeueKompassRichtungMerken = 1; |
}; |
} |
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[16] = KompassSignalSchlecht; |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v,r,fehler,korrektur; |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
korrektur = w / 8 + 1; |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
if(abs(MesswertGier) > 128) |
{ |
fehler = 0; |
} |
if(!KompassSignalSchlecht && w < 25) |
{ |
GierGyroFehler += fehler; |
if(NeueKompassRichtungMerken) |
{ |
beeptime = 200; |
// KompassStartwert = KompassValue; |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
NeueKompassRichtungMerken = 0; |
} |
} |
ErsatzKompass += (fehler * 8) / korrektur; |
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w >= 0) |
{ |
if(!KompassSignalSchlecht) |
{ |
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
// r = KompassRichtung; |
v = (r * w) / v; // nach Kompass ausrichten |
w = 3 * Parameter_KompassWirkung; |
if(v > w) v = w; // Begrenzen |
else |
if(v < -w) v = -w; |
Mess_Integral_Gier += v; |
} |
if(KompassSignalSchlecht) KompassSignalSchlecht--; |
} |
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[16]=MotorenEin; |
//DebugOut.Analog[16] = Mittelwert_AccHoch; |
//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
//DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
// DebugOut.Analog[24] = MesswertNick/2; |
// DebugOut.Analog[25] = MesswertRoll/2; |
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
/* 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] = MesswertNick; |
// DebugOut.Analog[9] = SollHoehe; |
// DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[11] = KompassStartwert; |
// DebugOut.Analog[10] = Parameter_Gyro_I; |
// DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
// DebugOut.Analog[9] = KompassRichtung; |
// DebugOut.Analog[10] = GasMischanteil; |
// DebugOut.Analog[3] = HoeheD * 32; |
// DebugOut.Analog[4] = hoehenregler; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0; |
if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) / (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0; |
#define TRIM_MAX 200 |
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
// Maximalwerte abfangen |
// #define MAX_SENSOR (4096*STICK_GAIN) |
#define MAX_SENSOR (4096*4) |
if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; |
if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR; |
if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR; |
if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; |
if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// all BL-Ctrl connected? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) |
{ |
modell_fliegt = 1; |
GasMischanteil = MIN_GAS; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Höhenregelung |
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil *= STICK_GAIN; |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
{ |
int tmp_int; |
static char delay = 100; |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ |
if(Parameter_MaxHoehe < 50) |
{ |
if(!delay--) |
{ |
if((MessLuftdruck > 1000) && OCR0A < 254) |
{ |
if(OCR0A < 244) |
{ |
ExpandBaro -= 10; |
OCR0A = DruckOffsetSetting - ExpandBaro; |
} |
else OCR0A = 254; |
beeptime = 300; |
delay = 250; |
} |
else |
if((MessLuftdruck < 100) && OCR0A > 1) |
{ |
if(OCR0A > 10) |
{ |
ExpandBaro += 10; |
OCR0A = DruckOffsetSetting - ExpandBaro; |
} |
else OCR0A = 1; |
beeptime = 300; |
delay = 250; |
} |
else |
{ |
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
HoehenReglerAktiv = 0; |
delay = 1; |
} |
} |
} |
else |
{ |
HoehenReglerAktiv = 1; |
delay = 200; |
} |
} |
else |
{ |
SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; |
HoehenReglerAktiv = 1; |
} |
if(Notlandung) SollHoehe = 0; |
h = HoehenWert; |
if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
{ |
h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil |
h = GasMischanteil - h; // vom Gas abziehen |
h -= (HoeheD)/(8/STICK_GAIN); // D-Anteil |
tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (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; |
hoehenregler = (hoehenregler*15 + h) / 16; |
if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
{ |
if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
} |
if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
GasMischanteil = hoehenregler; |
} |
} |
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMischanteil; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gier-Anteil |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define MUL_G 1.0 |
GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
// GierMischanteil = 0; |
#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
if(GasMischanteil > MIN_GIERGAS) |
{ |
if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; |
if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); |
} |
else |
{ |
if(GierMischanteil > (MIN_GIERGAS / 2)) GierMischanteil = MIN_GIERGAS / 2; |
if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
} |
tmp_int = MAX_GAS*STICK_GAIN; |
if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
else SummeNick += DiffNick; // I-Anteil bei HH |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
// Motor Vorn |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
else SummeRoll += DiffRoll; // I-Anteil bei HH |
if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i=0; i<MAX_MOTORS; i++) |
{ |
signed int tmp_int; |
if(Mixer.Motor[i][0] > 0) |
{ |
tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
tmp_int = tmp_motorwert[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
Motor[i] = tmp_int; |
} |
else Motor[i] = 0; |
} |
/* |
if(Poti1 > 20) Motor1 = 0; |
if(Poti1 > 90) Motor6 = 0; |
if(Poti1 > 140) Motor2 = 0; |
//if(Poti1 > 200) Motor7 = 0; |
*/ |
} |
/branches/V0.75b_Ligi_ExternEvents/fc.h |
---|
0,0 → 1,188 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
#ifndef _FC_H |
#define _FC_H |
//#define GIER_GRAD_FAKTOR 1291L // Abhängigkeit zwischen GyroIntegral und Winkel |
//#define GIER_GRAD_FAKTOR 1160L |
extern long GIER_GRAD_FAKTOR; // Abhängigkeit zwischen GyroIntegral und Winkel |
#define STICK_GAIN 4 |
#define FLAG_MOTOR_RUN 1 |
#define FLAG_FLY 2 |
#define FLAG_CALIBRATE 4 |
#define FLAG_START 8 |
#define MAX_MOTORS 12 |
#define CHECK_MIN_MAX(wert,min,max) {if(wert < min) wert = min; else if(wert > max) wert = max;} |
extern volatile unsigned char MikroKopterFlags; |
extern volatile unsigned int I2CTimeout; |
extern unsigned char Sekunde,Minute; |
extern long IntegralNick,IntegralNick2; |
extern long IntegralRoll,IntegralRoll2; |
//extern int IntegralNick,IntegralNick2; |
//extern int IntegralRoll,IntegralRoll2; |
extern long Mess_IntegralNick,Mess_IntegralNick2; |
extern long Mess_IntegralRoll,Mess_IntegralRoll2; |
extern long IntegralAccNick,IntegralAccRoll; |
extern volatile long Mess_Integral_Hoch; |
extern long Integral_Gier,Mess_Integral_Gier,Mess_Integral_Gier2; |
extern int KompassValue; |
extern int KompassStartwert; |
extern int KompassRichtung; |
extern int TrimNick, TrimRoll; |
extern long ErsatzKompass; |
extern int ErsatzKompassInGrad; // Kompasswert in Grad |
extern int HoehenWert; |
extern int SollHoehe; |
extern int MesswertNick,MesswertRoll,MesswertGier; |
extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier, Mittelwert_AccNick, Mittelwert_AccRoll; |
extern int NeutralAccX, NeutralAccY,Mittelwert_AccHoch; |
extern unsigned char HoehenReglerAktiv; |
extern volatile float NeutralAccZ; |
extern long Umschlag180Nick, Umschlag180Roll; |
extern signed int ExternStickNick,ExternStickRoll,ExternStickGier; |
extern unsigned char Parameter_UserParam1,Parameter_UserParam2,Parameter_UserParam3,Parameter_UserParam4,Parameter_UserParam5,Parameter_UserParam6,Parameter_UserParam7,Parameter_UserParam8; |
extern int NaviAccNick,NaviAccRoll,NaviCntAcc; |
extern unsigned int modell_fliegt; |
void MotorRegler(void); |
void SendMotorData(void); |
void CalibrierMittelwert(void); |
void Mittelwert(void); |
void SetNeutral(void); |
void Piep(unsigned char Anzahl, unsigned int dauer); |
void start_engines(void); |
extern unsigned char h,m,s; |
extern volatile unsigned char Timeout ; |
extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
extern int DiffNick,DiffRoll; |
extern int Poti1, Poti2, Poti3, Poti4; |
extern volatile unsigned char SenderOkay; |
extern unsigned char RequiredMotors; |
extern int StickNick,StickRoll,StickGier; |
extern char MotorenEin; |
extern void DefaultKonstanten1(void); |
extern void DefaultKonstanten2(void); |
extern void DefaultKonstanten3(void); |
extern void DefaultStickMapping(void); |
#define STRUCT_PARAM_LAENGE sizeof(EE_Parameter) |
struct mk_param_struct |
{ |
unsigned char Kanalbelegung[8]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
unsigned char Hoehe_MinGas; // Wert : 0-100 |
unsigned char Luftdruck_D; // Wert : 0-250 |
unsigned char MaxHoehe; // Wert : 0-32 |
unsigned char Hoehe_P; // Wert : 0-32 |
unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
unsigned char Stick_P; // Wert : 1-6 |
unsigned char Stick_D; // Wert : 0-64 |
unsigned char Gier_P; // Wert : 1-20 |
unsigned char Gas_Min; // Wert : 0-32 |
unsigned char Gas_Max; // Wert : 33-250 |
unsigned char GyroAccFaktor; // Wert : 1-64 |
unsigned char KompassWirkung; // Wert : 0-32 |
unsigned char Gyro_P; // Wert : 10-250 |
unsigned char Gyro_I; // Wert : 0-250 |
unsigned char Gyro_D; // Wert : 0-250 |
unsigned char UnterspannungsWarnung; // Wert : 0-250 |
unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
unsigned char UfoAusrichtung; // X oder + Formation |
unsigned char I_Faktor; // Wert : 0-250 |
unsigned char UserParam1; // Wert : 0-250 |
unsigned char UserParam2; // Wert : 0-250 |
unsigned char UserParam3; // Wert : 0-250 |
unsigned char UserParam4; // Wert : 0-250 |
unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
//--- Seit V0.75 |
unsigned char ServoRollControl; // Wert : 0-250 // Stellung des Servos |
unsigned char ServoRollComp; // Wert : 0-250 |
unsigned char ServoRollMin; // Wert : 0-250 |
unsigned char ServoRollMax; // Wert : 0-250 |
//--- |
unsigned char ServoNickRefresh; // |
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
unsigned char AchsKopplung2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
unsigned char CouplingYawCorrection; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
unsigned char Driftkomp; |
unsigned char DynamicStability; |
unsigned char UserParam5; // Wert : 0-250 |
unsigned char UserParam6; // Wert : 0-250 |
unsigned char UserParam7; // Wert : 0-250 |
unsigned char UserParam8; // Wert : 0-250 |
//---Output --------------------------------------------- |
unsigned char J16Bitmask; // for the J16 Output |
unsigned char J16Timing; // for the J16 Output |
unsigned char J17Bitmask; // for the J17 Output |
unsigned char J17Timing; // for the J17 Output |
//---NaviCtrl--------------------------------------------- |
unsigned char NaviGpsModeControl; // Parameters for the Naviboard |
unsigned char NaviGpsGain; |
unsigned char NaviGpsP; |
unsigned char NaviGpsI; |
unsigned char NaviGpsD; |
unsigned char NaviGpsPLimit; |
unsigned char NaviGpsILimit; |
unsigned char NaviGpsDLimit; |
unsigned char NaviGpsACC; |
unsigned char NaviGpsMinSat; |
unsigned char NaviStickThreshold; |
unsigned char NaviWindCorrection; |
unsigned char NaviSpeedCompensation; |
unsigned char NaviOperatingRadius; |
unsigned char NaviAngleLimitation; |
unsigned char NaviPH_LoginTime; |
//---Ext.Ctrl--------------------------------------------- |
unsigned char ExternalControl; // for serial Control |
//------------------------------------------------ |
unsigned char BitConfig; // (war Loop-Cfg) Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
unsigned char ServoCompInvert; // // 0x01 = Nick, 0x02 = Roll 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
}; |
struct |
{ |
char Revision; |
char Name[12]; |
signed char Motor[16][4]; |
} Mixer; |
extern struct mk_param_struct EE_Parameter; |
extern unsigned char Parameter_Luftdruck_D; |
extern unsigned char Parameter_MaxHoehe; |
extern unsigned char Parameter_Hoehe_P; |
extern unsigned char Parameter_Hoehe_ACC_Wirkung; |
extern unsigned char Parameter_KompassWirkung; |
extern unsigned char Parameter_Gyro_P; |
extern unsigned char Parameter_Gyro_I; |
extern unsigned char Parameter_Gier_P; |
extern unsigned char Parameter_ServoNickControl; |
extern unsigned char Parameter_ServoRollControl; |
extern unsigned char Parameter_AchsKopplung1; |
extern unsigned char Parameter_AchsKopplung2; |
//extern unsigned char Parameter_AchsGegenKopplung1; |
extern unsigned char Parameter_J16Bitmask; // for the J16 Output |
extern unsigned char Parameter_J16Timing; // for the J16 Output |
extern unsigned char Parameter_J17Bitmask; // for the J17 Output |
extern unsigned char Parameter_J17Timing; // for the J17 Output |
extern signed char MixerTable[MAX_MOTORS][4]; |
extern unsigned char Motor[MAX_MOTORS]; |
#endif //_FC_H |
/branches/V0.75b_Ligi_ExternEvents/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="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><File path="led.h"></File><File path="led.c"></File><File path="fc.c"></File><File path="Spectrum.c"></File><File path="Spectrum.h"></File></Project> |
/branches/V0.75b_Ligi_ExternEvents/flight.pnps |
---|
0,0 → 1,0 |
<pd><ViewState><e p="Flight-Ctrl" x="true"></e></ViewState></pd> |
/branches/V0.75b_Ligi_ExternEvents/gps.h |
---|
0,0 → 1,7 |
extern signed int GPS_Nick; |
extern signed int GPS_Roll; |
extern signed int GPS_Nick2; |
extern signed int GPS_Roll2; |
void GPS_Neutral(void); |
void GPS_BerechneZielrichtung(void); |
/branches/V0.75b_Ligi_ExternEvents/led.c |
---|
0,0 → 1,49 |
#include <inttypes.h> |
#include "main.h" |
uint16_t LED1_Timing = 0; |
uint16_t LED2_Timing = 0; |
unsigned char J16Blinkcount = 0, J16Mask = 1; |
unsigned char 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 UpdateMotors() every 2ms |
void LED_Update(void) |
{ |
static char delay = 0; |
if(!delay--) // 10ms Intervall |
{ |
delay = 4; |
if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing > 230)) {if(EE_Parameter.J16Bitmask & 128) J16_ON; else J16_OFF;} |
else |
if((EE_Parameter.J16Timing > 250) && (Parameter_J16Timing < 10)) {if(EE_Parameter.J16Bitmask & 128) J16_OFF; else J16_ON;} |
else |
if(!J16Blinkcount--) |
{ |
J16Blinkcount = Parameter_J16Timing-1; |
if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2; |
if(J16Mask & EE_Parameter.J16Bitmask) J16_ON; else J16_OFF; |
} |
if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing > 230)) {if(EE_Parameter.J17Bitmask & 128) J17_ON; else J17_OFF;} |
else |
if((EE_Parameter.J17Timing > 250) && (Parameter_J17Timing < 10)) {if(EE_Parameter.J17Bitmask & 128) J17_OFF; else J17_ON;} |
else |
if(!J17Blinkcount--) |
{ |
J17Blinkcount = Parameter_J17Timing-1; |
if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2; |
if(J17Mask & EE_Parameter.J17Bitmask) J17_ON; else J17_OFF; |
} |
} |
} |
/branches/V0.75b_Ligi_ExternEvents/led.h |
---|
0,0 → 1,11 |
#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) |
extern void LED_Init(void); |
extern void LED_Update(void); |
/branches/V0.75b_Ligi_ExternEvents/main.c |
---|
0,0 → 1,393 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt und genannt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
unsigned char EEPromArray[E2END+1] EEMEM; |
unsigned char PlatinenVersion = 10; |
unsigned char SendVersionToNavi = 1; |
unsigned char BattLowVoltageWarning = 94; |
// -- Parametersatz aus EEPROM lesen --- |
// number [1..5] |
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if((number > 5)||(number < 1)) number = 3; |
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length); |
LED_Init(); |
} |
// -- Parametersatz ins EEPROM schreiben --- |
// number [1..5] |
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if(number > 5) number = 5; |
if(number < 1) return; |
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_LENGTH], length); // Länge der Datensätze merken |
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_CHANNELS], 8); // 8 Kanäle merken |
SetActiveParamSetNumber(number); |
LED_Init(); |
} |
unsigned char GetActiveParamSetNumber(void) |
{ |
unsigned char set; |
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
if((set > 5) || (set < 1)) |
{ |
set = 3; |
SetActiveParamSetNumber(set); // diesen Parametersatz als aktuell merken |
} |
return(set); |
} |
void SetActiveParamSetNumber(unsigned char number) |
{ |
if(number > 5) number = 5; |
if(number < 1) return; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
} |
void CalMk3Mag(void) |
{ |
static unsigned char stick = 1; |
if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -20) stick = 0; |
if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) && !stick) |
{ |
stick = 1; |
WinkelOut.CalcState++; |
if(WinkelOut.CalcState > 4) |
{ |
// WinkelOut.CalcState = 0; // in Uart.c |
beeptime = 1000; |
} |
else Piep(WinkelOut.CalcState,150); |
} |
DebugOut.Analog[19] = WinkelOut.CalcState; |
} |
//############################################################################ |
//Hauptprogramm |
int main (void) |
//############################################################################ |
{ |
unsigned int timer,i; |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // verzögern |
if(PINB & 0x01) |
{ |
if(PINB & 0x02) PlatinenVersion = 13; |
else PlatinenVersion = 11; |
} |
else |
{ |
if(PINB & 0x02) PlatinenVersion = 20; |
else PlatinenVersion = 10; |
} |
DDRC = 0x81; // SCL |
DDRC |=0x40; // HEF4017 Reset |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
PORTD = 0x47; // LED |
HEF4017R_ON; |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
beeptime = 2000; |
StickGier = 0; PPM_in[K_GAS] = 0;StickRoll = 0; StickNick = 0; |
if(PlatinenVersion >= 20) GIER_GRAD_FAKTOR = 1160; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC |
ROT_OFF; |
Timer_Init(); |
TIMER2_Init(); |
UART_Init(); |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
SPI_MasterInit(); |
sei(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a'); |
if(UCSR1A == 0x20 && UCSR1C == 0x06) // initial Values for 644P |
{ |
Uart1Init(); |
} |
GRN_ON; |
ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes |
if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && // Check Revision in the first Byte |
(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff)) // Settings reset via Koptertool |
{ |
unsigned char i; |
RequiredMotors = 0; |
eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;} |
} |
else // default |
{ |
unsigned char i; |
printf("\n\rGenerating default Mixer Table"); |
for(i=0; i<16;i++) { Mixer.Motor[i][0] = 0;Mixer.Motor[i][1] = 0;Mixer.Motor[i][2] = 0;Mixer.Motor[i][3] = 0;}; |
// default = Quadro |
Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] = 0; Mixer.Motor[0][3] = +64; |
Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] = 0; Mixer.Motor[1][3] = +64; |
Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] = 0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64; |
Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] = 0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64; |
Mixer.Revision = MIXER_REVISION; |
memcpy(Mixer.Name, "Quadro\0", 11); |
eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Check connected BL-Ctrls |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
printf("\n\rFound BL-Ctrl: "); |
motorread = 0; UpdateMotor = 0; SendMotorData(); while(!UpdateMotor); motorread = 0; // read the first I2C-Data |
timer = SetDelay(2000); |
for(i=0; i < MAX_MOTORS; i++) |
{ |
UpdateMotor = 0; |
SendMotorData(); |
while(!UpdateMotor); |
if(Mixer.Motor[i][0] > 0) // wait max 2 sec for the BL-Ctrls to wake up |
{ |
while(!CheckDelay(timer) && !MotorPresent[i]) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);}; |
} |
if(MotorPresent[i]) printf("%d ",i+1); |
} |
for(i=0; i < MAX_MOTORS; i++) |
{ |
if(!MotorPresent[i] && Mixer.Motor[i][0] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
MotorError[i] = 0; |
} |
printf("\n\r==================================="); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Check Settings |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
{ |
DefaultKonstanten1(); |
printf("\n\rInit. EEPROM"); |
for (unsigned char i=1;i<6;i++) |
{ |
if(i==2) DefaultKonstanten2(); // Kamera |
if(i==3) DefaultKonstanten3(); // Beginner |
if(i>3) DefaultKonstanten2(); // Kamera |
if(PlatinenVersion >= 20) |
{ |
EE_Parameter.Gyro_D = 5; |
EE_Parameter.Driftkomp = 0; |
EE_Parameter.GyroAccFaktor = 27; |
EE_Parameter.WinkelUmschlagNick = 78; |
EE_Parameter.WinkelUmschlagRoll = 78; |
} |
// valid Stick-Settings? |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12 && eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12) |
{ |
EE_Parameter.Kanalbelegung[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]); |
EE_Parameter.Kanalbelegung[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]); |
EE_Parameter.Kanalbelegung[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]); |
EE_Parameter.Kanalbelegung[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]); |
EE_Parameter.Kanalbelegung[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]); |
EE_Parameter.Kanalbelegung[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]); |
EE_Parameter.Kanalbelegung[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]); |
EE_Parameter.Kanalbelegung[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]); |
if(i==1) printf(": Generating Default-Parameter using old Stick Settings"); |
} else DefaultStickMapping(); |
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
SetActiveParamSetNumber(3); // default-Setting |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
} |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) |
{ |
printf("\n\rACC not calibrated !"); |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
printf("\n\rUsing parameterset %d", GetActiveParamSetNumber()); |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
printf("\n\rCalibrating pressure sensor.."); |
timer = SetDelay(1000); |
SucheLuftruckOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
SetNeutral(); |
ROT_OFF; |
beeptime = 2000; |
ExternControl.Digital[0] = 0x55; |
printf("\n\rControl: "); |
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Normal (ACC-Mode)"); |
LcdClear(); |
I2CTimeout = 5000; |
WinkelOut.Orientation = 1; |
printf("\n\rBatt:"); |
if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung |
{ |
timer = SetDelay(500); |
while (!CheckDelay(timer)); |
if(UBat < 130) |
{ |
BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung; |
Piep(3,200); |
printf(" 3 Cells "); |
} |
else |
{ |
BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung; |
Piep(4,200); |
printf(" 4 Cells "); |
} |
} |
else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung; |
printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10); |
printf("\n\r===================================\n\r"); |
while (1) |
{ |
if(UpdateMotor && AdReady) // ReglerIntervall |
{ |
UpdateMotor=0; |
if(WinkelOut.CalcState) CalMk3Mag(); |
else MotorRegler(); |
SendMotorData(); |
ROT_OFF; |
if(PcZugriff) PcZugriff--; |
else |
{ |
ExternControl.Config = 0; |
ExternStickNick = 0; |
ExternStickRoll = 0; |
ExternStickGier = 0; |
} |
if(SenderOkay) SenderOkay--; |
else |
{ |
TIMSK1 |= _BV(ICIE1); // enable PPM-Input |
} |
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160; |
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101; |
if(NaviDataOkay) |
{ |
if(--NaviDataOkay == 0) |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
} |
if(!--I2CTimeout || MissingMotor) |
{ |
if(!I2CTimeout) |
{ |
i2c_reset(); |
I2CTimeout = 5; |
} |
if((BeepMuster == 0xffff) && MotorenEin) |
{ |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} |
else |
{ |
ROT_OFF; |
} |
if(SIO_DEBUG && (!UpdateMotor || !MotorenEin)) |
{ |
DatenUebertragung(); |
BearbeiteRxDaten(); |
} |
else BearbeiteRxDaten(); |
if(CheckDelay(timer)) |
{ |
if(UBat < BattLowVoltageWarning) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
timer = SetDelay(20); |
} |
LED_Update(); |
} |
if(!SendSPI) { SPI_TransmitByte(); } |
} |
return (1); |
} |
/branches/V0.75b_Ligi_ExternEvents/main.h |
---|
0,0 → 1,127 |
#ifndef _MAIN_H |
#define _MAIN_H |
#define QUADRO |
//Hier die Quarz Frequenz einstellen |
#if defined (__AVR_ATmega32__) |
#define SYSCLK 20000000L //Quarz Frequenz in Hz |
#endif |
#if defined (__AVR_ATmega644__) |
#define SYSCLK 20000000L //Quarz Frequenz in Hz |
#endif |
#if defined (__AVR_ATmega644P__) |
#define SYSCLK 20000000L //Quarz Frequenz in Hz |
#endif |
// neue Hardware |
#define ROT_OFF {if((PlatinenVersion == 10)||(PlatinenVersion == 20)) PORTB &=~0x01; else PORTB |= 0x01;} |
#define ROT_ON {if((PlatinenVersion == 10)||(PlatinenVersion == 20)) PORTB |= 0x01; else PORTB &=~0x01;} |
#define ROT_FLASH PORTB ^= 0x01 |
#define GRN_OFF {if((PlatinenVersion < 12)) PORTB &=~0x02; else PORTB |= 0x02;} |
#define GRN_ON {if((PlatinenVersion < 12)) PORTB |= 0x02; else PORTB &=~0x02;} |
#define GRN_FLASH PORTB ^= 0x02 |
#define F_CPU SYSCLK |
//#ifndef F_CPU |
//#error ################## F_CPU nicht definiert oder ungültig ############# |
//#endif |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define EE_DATENREVISION 76 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
#define MIXER_REVISION 1 // wird angepasst, wenn sich die Mixer-Daten geändert haben |
#define EEPROM_ADR_VALID 1 |
#define EEPROM_ADR_ACTIVE_SET 2 |
#define EEPROM_ADR_LAST_OFFSET 3 |
#define EEPROM_ADR_ACC_NICK 4 |
#define EEPROM_ADR_ACC_ROLL 6 |
#define EEPROM_ADR_ACC_Z 8 |
#define EEPROM_ADR_CHANNELS 80 |
#define EEPROM_ADR_PARAM_LENGTH 98 |
#define EEPROM_ADR_PARAM_BEGIN 100 |
#define EEPROM_ADR_MIXER_TABLE 1000 // 1001 - 1100 |
#define CFG_HOEHENREGELUNG 0x01 |
#define CFG_HOEHEN_SCHALTER 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_KOMPASS_AKTIV 0x08 |
#define CFG_KOMPASS_FIX 0x10 |
#define CFG_GPS_AKTIV 0x20 |
#define CFG_ACHSENKOPPLUNG_AKTIV 0x40 |
#define CFG_DREHRATEN_BEGRENZER 0x80 |
#define CFG_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
#define CFG_RES1 0x10 |
#define CFG_RES2 0x20 |
#define CFG_RES3 0x40 |
#define CFG_RES4 0x80 |
#define J3High PORTD |= 0x20 |
#define J3Low PORTD &= ~0x20 |
#define J4High PORTD |= 0x10 |
#define J4Low PORTD &= ~0x10 |
#define J5High PORTD |= 0x08 |
#define J5Low PORTD &= ~0x08 |
//#define SYSCLK |
//extern unsigned long SYSCLK; |
extern volatile unsigned char SenderOkay; |
extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
extern unsigned char PlatinenVersion; |
extern unsigned char SendVersionToNavi; |
void ReadParameterSet (unsigned char number, unsigned char *buffer, unsigned char length); |
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length); |
extern unsigned char GetActiveParamSetNumber(void); |
void SetActiveParamSetNumber(unsigned char number); |
extern unsigned char EEPromArray[]; |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <avr/eeprom.h> |
#include <avr/boot.h> |
#include <avr/wdt.h> |
#include "old_macros.h" |
#include "_Settings.h" |
#include "printf_P.h" |
#include "timer0.h" |
#include "uart.h" |
#include "analog.h" |
#include "twimaster.h" |
#include "menu.h" |
#include "rc.h" |
#include "fc.h" |
#include "gps.h" |
#include "spi.h" |
#include "led.h" |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#define DEBUG_DISPLAY_INTERVALL 123 // in ms |
#define DELAY_US(x) ((unsigned int)( (x) * 1e-6 * F_CPU )) |
#endif //_MAIN_H |
/branches/V0.75b_Ligi_ExternEvents/makefile |
---|
0,0 → 1,449 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 75 |
VERSION_PATCH = 1 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol |
VERSION_SERIAL_MINOR = 1 # Serial Protocol |
NC_SPI_COMPATIBLE = 7 # Navi-Kompatibilität |
#------------------------------------------------------------------- |
ifeq ($(MCU), atmega32) |
# FUSE_SETTINGS= -u -U lfuse:w:0xff:m -U hfuse:w:0xcf:m |
HEX_NAME = MEGA32 |
endif |
ifeq ($(MCU), atmega644) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
#FUSE_SETTINGS = -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
# -u bei neuen Controllern wieder einspielen |
HEX_NAME = MEGA644 |
endif |
ifeq ($(MCU), atmega644p) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644 |
endif |
ifeq ($(F_CPU), 16000000) |
QUARZ = 16MHZ |
endif |
ifeq ($(F_CPU), 20000000) |
QUARZ = 20MHZ |
endif |
# Output format. (can be srec, ihex, binary) |
FORMAT = ihex |
# Target file name (without extension). |
ifeq ($(VERSION_PATCH), 0) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)a |
endif |
ifeq ($(VERSION_PATCH), 1) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)b |
endif |
ifeq ($(VERSION_PATCH), 2) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c |
endif |
ifeq ($(VERSION_PATCH), 3) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d |
endif |
ifeq ($(VERSION_PATCH), 4) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e |
endif |
ifeq ($(VERSION_PATCH), 5) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f |
endif |
ifeq ($(VERSION_PATCH), 6) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g |
endif |
ifeq ($(VERSION_PATCH), 7) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h |
endif |
ifeq ($(VERSION_PATCH), 8) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i |
endif |
ifeq ($(VERSION_PATCH), 9) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j |
endif |
ifeq ($(VERSION_PATCH), 10) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k |
endif |
ifeq ($(VERSION_PATCH), 11) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)L |
endif |
ifeq ($(VERSION_PATCH), 12) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m |
endif |
ifeq ($(VERSION_PATCH), 13) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n |
endif |
ifeq ($(VERSION_PATCH), 14) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)o |
endif |
ifeq ($(VERSION_PATCH), 15) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)p |
endif |
ifeq ($(VERSION_PATCH), 16) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)q |
endif |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
OPT = 2 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c |
SRC += twimaster.c rc.c fc.c GPS.c spi.c led.c Spectrum.c |
########################################################################################################## |
# List Assembler source files here. |
# Make them always end in a capital .S. Files ending in a lowercase .s |
# will not be considered source files but generated files (assembler |
# output from the compiler), and will be deleted upon "make clean"! |
# Even though the DOS/Win* filesystem matches both .s and .S the same, |
# it will preserve the spelling of the filenames, and gcc itself does |
# care about how the name is spelled on its command-line. |
ASRC = |
# List any extra directories to look for include files here. |
# Each directory must be seperated by a space. |
EXTRAINCDIRS = |
# Optional compiler flags. |
# -g: generate debugging information (for GDB, or for COFF conversion) |
# -O*: optimization level |
# -f...: tuning, see gcc manual and avr-libc documentation |
# -Wall...: warning level |
# -Wa,...: tell GCC to pass this to the assembler. |
# -ahlms: create assembler listing |
CFLAGS = -O$(OPT) \ |
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \ |
-Wall -Wstrict-prototypes \ |
-Wa,-adhlns=$(<:.c=.lst) \ |
$(patsubst %,-I%,$(EXTRAINCDIRS)) |
# Set a "language standard" compiler flag. |
# Unremark just one line below to set the language standard to use. |
# gnu99 = C99 + GNU extensions. See GCC manual for more information. |
#CFLAGS += -std=c89 |
#CFLAGS += -std=gnu89 |
#CFLAGS += -std=c99 |
CFLAGS += -std=gnu99 |
CFLAGS += -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) |
# 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. |
# Display size of file. |
sizebefore: |
@if [ -f $(TARGET).elf ]; then echo Size before:; $(ELFSIZE); $(HEXSIZE); echo; fi |
sizeafter: |
@if [ -f $(TARGET).elf ]; then echo Size after:; $(ELFSIZE); $(HEXSIZE); 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.75b_Ligi_ExternEvents/menu.c |
---|
0,0 → 1,168 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
unsigned int TestInt = 0; |
#define ARRAYGROESSE 10 |
unsigned char Array[ARRAYGROESSE] = {1,2,3,4,5,6,7,8,9,10}; |
char DisplayBuff[80] = "Hallo Welt"; |
unsigned char DispPtr = 0; |
unsigned char MaxMenue = 13; |
unsigned char MenuePunkt = 0; |
unsigned char RemoteKeys = 0; |
#define KEY1 0x01 |
#define KEY2 0x02 |
#define KEY3 0x04 |
#define KEY4 0x08 |
#define KEY5 0x10 |
void LcdClear(void) |
{ |
unsigned char i; |
for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
} |
void Menu(void) |
{ |
if(MenuePunkt > MaxMenue) MenuePunkt = MaxMenue; |
if(RemoteKeys & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue;} |
if(RemoteKeys & KEY2) { if(MenuePunkt == MaxMenue) MenuePunkt = 0; else MenuePunkt++;} |
if((RemoteKeys & KEY1) && (RemoteKeys & KEY2)) MenuePunkt = 0; |
LcdClear(); |
if(MenuePunkt < 10) {LCD_printfxy(17,0,"[%i]",MenuePunkt);} |
else {LCD_printfxy(16,0,"[%i]",MenuePunkt);}; |
switch(MenuePunkt) |
{ |
case 0: |
LCD_printfxy(0,0,"+ MikroKopter +"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH +'a'); |
LCD_printfxy(0,2,"Setting:%d %s",GetActiveParamSetNumber(),Mixer.Name); |
if(I2CTimeout < 6) LCD_printfxy(0,3,"I2C ERROR!!!") |
else |
if(MissingMotor) LCD_printfxy(0,3,"Missing BL-Ctrl:%d!!",MissingMotor) |
else LCD_printfxy(0,3,"(c) Holger Buss"); |
// if(RemoteTasten & KEY3) TestInt--; |
// if(RemoteTasten & KEY4) TestInt++; |
break; |
case 1: |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
LCD_printfxy(0,0,"Hoehe: %5i",HoehenWert); |
LCD_printfxy(0,1,"SollHoehe: %5i",SollHoehe); |
LCD_printfxy(0,2,"Luftdruck: %5i",MessLuftdruck); |
LCD_printfxy(0,3,"Off : %5i",DruckOffsetSetting); |
} |
else |
{ |
LCD_printfxy(0,1,"Keine "); |
LCD_printfxy(0,2,"Höhenregelung"); |
} |
break; |
case 2: |
LCD_printfxy(0,0,"akt. Lage"); |
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024); |
LCD_printfxy(0,3,"Kompass: %5i",KompassValue); |
break; |
case 3: |
LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]); |
LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]); |
LCD_printfxy(0,2,"K5:%4i K6:%4i ",PPM_in[5],PPM_in[6]); |
LCD_printfxy(0,3,"K7:%4i K8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4: |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_NICK]],PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Gi:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_GAS]],PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]); |
break; |
case 5: |
LCD_printfxy(0,0,"Gyro - Sensor"); |
if(PlatinenVersion == 10) |
{ |
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",AdWertNick - AdNeutralNick/8, AdNeutralNick/8, AdNeutralNick%8); |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdWertRoll - AdNeutralRoll/8, AdNeutralRoll/8, AdNeutralRoll%8); |
LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier); |
} |
else |
if((PlatinenVersion == 11) || (PlatinenVersion == 20)) |
{ |
LCD_printfxy(0,1,"Nick %4i (%3i.%x)",AdWertNick - AdNeutralNick/8, AdNeutralNick/16, (AdNeutralNick%16)/2); |
LCD_printfxy(0,2,"Roll %4i (%3i.%x)",AdWertRoll - AdNeutralRoll/8, AdNeutralRoll/16, (AdNeutralRoll%16)/2); |
LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier/2); |
} |
else |
if(PlatinenVersion == 13) |
{ |
LCD_printfxy(0,1,"Nick %4i (%3i)(%3i)",AdWertNick - AdNeutralNick/8, AdNeutralNick/16,AnalogOffsetNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)(%3i)",AdWertRoll - AdNeutralRoll/8, AdNeutralRoll/16,AnalogOffsetRoll); |
LCD_printfxy(0,3,"Gier %4i (%3i)(%3i)",AdNeutralGier - AdWertGier, AdNeutralGier/2,AnalogOffsetGier); |
} |
break; |
case 6: |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertAccNick,NeutralAccX); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertAccRoll,NeutralAccY); |
LCD_printfxy(0,3,"Hoch %4i (%3i)",Mittelwert_AccHoch/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ); |
break; |
case 7: |
LCD_printfxy(0,1,"Spannung: %5i",UBat); |
LCD_printfxy(0,2,"Empf.Pegel:%5i",SenderOkay); |
break; |
case 8: |
LCD_printfxy(0,0,"Kompass "); |
LCD_printfxy(0,1,"Richtung: %5i",KompassRichtung); |
LCD_printfxy(0,2,"Messwert: %5i",KompassValue); |
LCD_printfxy(0,3,"Start: %5i",KompassStartwert); |
break; |
case 9: |
LCD_printfxy(0,0,"Poti1: %3i",Poti1); |
LCD_printfxy(0,1,"Poti2: %3i",Poti2); |
LCD_printfxy(0,2,"Poti3: %3i",Poti3); |
LCD_printfxy(0,3,"Poti4: %3i",Poti4); |
break; |
case 10: |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl); |
LCD_printfxy(0,2,"Stellung: %3i",ServoValue); |
LCD_printfxy(0,3,"Range:%3i-%3i",EE_Parameter.ServoNickMin,EE_Parameter.ServoNickMax); |
break; |
case 11: |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick,ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Gi:%4i ",ExternControl.Gas,ExternControl.Gier); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight,ExternControl.Config); |
break; |
case 12: |
LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
LCD_printfxy(0,1," %3d %3d %3d %3d ",MotorError[0],MotorError[1],MotorError[2],MotorError[3]); |
LCD_printfxy(0,2," %3d %3d %3d %3d ",MotorError[4],MotorError[5],MotorError[6],MotorError[7]); |
LCD_printfxy(0,3," %3d %3d %3d %3d ",MotorError[8],MotorError[9],MotorError[10],MotorError[11]); |
break; |
case 13: |
LCD_printfxy(0,0,"BL-Ctrl found " ); |
LCD_printfxy(0,1," %c %c %c %c ",MotorPresent[0] + '-',MotorPresent[1] + '-',MotorPresent[2] + '-',MotorPresent[3] + '-'); |
LCD_printfxy(0,2," %c %c %c %c ",MotorPresent[4] + '-',MotorPresent[5] + '-',MotorPresent[6] + '-',MotorPresent[7] + '-'); |
LCD_printfxy(0,3," %c - - -",MotorPresent[8] + '-'); |
if(MotorPresent[9]) LCD_printfxy(4,3,"10"); |
if(MotorPresent[10]) LCD_printfxy(8,3,"11"); |
if(MotorPresent[11]) LCD_printfxy(12,3,"12"); |
break; |
default: MaxMenue = MenuePunkt - 1; |
MenuePunkt = 0; |
break; |
} |
RemoteKeys = 0; |
} |
/branches/V0.75b_Ligi_ExternEvents/menu.h |
---|
0,0 → 1,9 |
extern void Menu(void); |
extern void LcdClear(void); |
extern char DisplayBuff[80]; |
extern unsigned char DispPtr; |
extern unsigned char MaxMenue; |
extern unsigned char MenuePunkt; |
extern unsigned char RemoteKeys; |
/branches/V0.75b_Ligi_ExternEvents/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.75b_Ligi_ExternEvents/printf_P.c |
---|
0,0 → 1,480 |
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt |
/* |
Copyright (C) 1993 Free Software Foundation |
This file is part of the GNU IO Library. This library is free |
software; you can redistribute it and/or modify it under the |
terms of the GNU General Public License as published by the |
Free Software Foundation; either version 2, or (at your option) |
any later version. |
This library is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this library; see the file COPYING. If not, write to the Free |
Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
As a special exception, if you link this library with files |
compiled with a GNU compiler to produce an executable, this does not cause |
the resulting executable to be covered by the GNU General Public License. |
This exception does not however invalidate any other reasons why |
the executable file might be covered by the GNU General Public License. */ |
/* |
* Copyright (c) 1990 Regents of the University of California. |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* 3. [rescinded 22 July 1999] |
* 4. Neither the name of the University nor the names of its contributors |
* may be used to endorse or promote products derived from this software |
* without specific prior written permission. |
* |
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND |
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE |
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
* SUCH DAMAGE. |
*/ |
/****************************************************************************** |
This file is a patched version of printf called _printf_P |
It is made to work with avr-gcc for Atmel AVR MCUs. |
There are some differences from standard printf: |
1. There is no floating point support (with fp the code is about 8K!) |
2. Return type is void |
3. Format string must be in program memory (by using macro printf this is |
done automaticaly) |
4. %n is not implemented (just remove the comment around it if you need it) |
5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the |
folowing specifiers are disabled : |
space # * . - + p s o O |
6. A function void uart_sendchar(char c) is used for output. The UART must |
be initialized before using printf. |
Alexander Popov |
sasho@vip.orbitel.bg |
******************************************************************************/ |
/* |
* Actual printf innards. |
* |
* This code is large and complicated... |
*/ |
#include <string.h> |
#ifdef __STDC__ |
#include <stdarg.h> |
#else |
#include <varargs.h> |
#endif |
#include "main.h" |
//#define LIGHTPRINTF |
char PrintZiel; |
char Putchar(char zeichen) |
{ |
if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);} |
else 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.75b_Ligi_ExternEvents/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.75b_Ligi_ExternEvents/rc.c |
---|
0,0 → 1,122 |
/*####################################################################################### |
Decodieren eines RC Summen Signals |
#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "rc.h" |
#include "main.h" |
volatile int PPM_in[11]; |
volatile int PPM_diff[11]; // das diffenzierte Stick-Signal |
volatile unsigned char NewPpmData = 1; |
//############################################################################ |
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input |
//Capture Funktion benutzt: |
void rc_sum_init (void) |
//############################################################################ |
{ |
TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64 |
// TCCR1B=(1<<CS11)|(0<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64 |
TIMSK1 |= _BV(ICIE1); |
AdNeutralGier = 0; |
AdNeutralRoll = 0; |
AdNeutralNick = 0; |
return; |
} |
//############################################################################ |
//Diese Routine startet und inizialisiert den Timer für RC |
SIGNAL(SIG_INPUT_CAPTURE1) |
//############################################################################ |
{ |
static unsigned int AltICR=0; |
signed int signal = 0,tmp; |
static int index; |
signal = (unsigned int) ICR1 - AltICR; |
AltICR = ICR1; |
//Syncronisationspause? |
if((signal > 1100) && (signal < 8000)) |
{ |
if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten |
index = 1; |
} |
else |
{ |
if(index < 10) |
{ |
if((signal > 250) && (signal < 687)) |
{ |
signal -= 466; |
// Stabiles Signal |
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;} |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
if(tmp > signal+1) tmp--; else |
if(tmp < signal-1) tmp++; |
if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
else PPM_diff[index] = 0; |
PPM_in[index] = tmp; |
} |
index++; |
if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen |
if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen |
if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen |
} |
} |
} |
/* |
//############################################################################ |
//Diese Routine startet und inizialisiert den Timer für RC |
SIGNAL(SIG_INPUT_CAPTURE1) |
//############################################################################ |
{ |
static unsigned int AltICR=0; |
signed int signal = 0,tmp; |
static int index; |
signal = (unsigned int) ICR1 - AltICR; |
DebugOut.Analog[16] = signal; |
signal /= 2; |
AltICR = ICR1; |
//Syncronisationspause? |
if((signal > 1100*2) && (signal < 8000*2)) |
{ |
if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten |
index = 1; |
} |
else |
{ |
if(index < 10) |
{ |
if((signal > 250) && (signal < 687*2)) |
{ |
signal -= 962; |
// Stabiles Signal |
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;} |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
if(tmp > signal+1) tmp--; else |
if(tmp < signal-1) tmp++; |
if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; |
else PPM_diff[index] = 0; |
PPM_in[index] = tmp; |
} |
index++; |
if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen |
if(index == 2) J4High; else J4Low; // Servosignal an J4 anlegen |
if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen |
} |
} |
} |
*/ |
/branches/V0.75b_Ligi_ExternEvents/rc.h |
---|
0,0 → 1,30 |
/*####################################################################################### |
Derkodieren eines RC Summen Signals |
#######################################################################################*/ |
#ifndef _RC_H |
#define _RC_H |
#if defined (__AVR_ATmega32__) |
#define TIMER_TEILER CK64 |
#define TIMER_RELOAD_VALUE 250 |
#endif |
#if defined (__AVR_ATmega644__) |
#define TIMER_RELOAD_VALUE 250 |
#endif |
#if defined (__AVR_ATmega644P__) |
#define TIMER_RELOAD_VALUE 250 |
#endif |
#define GAS PPM_in[2] |
extern void rc_sum_init (void); |
extern volatile int PPM_in[11]; |
extern volatile int PPM_diff[11]; // das diffenzierte Stick-Signal |
extern volatile unsigned char NewPpmData; |
#endif //_RC_H |
/branches/V0.75b_Ligi_ExternEvents/spi.c |
---|
0,0 → 1,295 |
// ######################## SPI - FlightCtrl ################### |
#include "main.h" |
//struct str_ToNaviCtrl_Version ToNaviCtrl_Version; |
//struct str_FromNaviCtrl_Version FromNaviCtrl_Version; |
struct str_ToNaviCtrl ToNaviCtrl; |
struct str_FromNaviCtrl FromNaviCtrl; |
struct str_FromNaviCtrl_Value FromNaviCtrl_Value; |
struct str_SPI_VersionInfo SPI_VersionInfo; |
unsigned char SPI_BufferIndex; |
unsigned char SPI_RxBufferIndex; |
volatile unsigned char SPI_Buffer[sizeof(FromNaviCtrl)]; |
unsigned char *SPI_TX_Buffer; |
unsigned char SPITransferCompleted, SPI_ChkSum; |
unsigned char SPI_RxDataValid,NaviDataOkay = 0; |
unsigned char SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
unsigned char SPI_CommandCounter = 0; |
#ifdef USE_SPI_COMMUNICATION |
//------------------------------------------------------ |
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); |
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); |
SPITransferCompleted = 1; |
//SPDR = 0x00; // dummy write |
ToNaviCtrl.Sync1 = 0xAA; |
ToNaviCtrl.Sync2 = 0x83; |
ToNaviCtrl.Command = SPI_CMD_USER; |
ToNaviCtrl.IntegralNick = 0; |
ToNaviCtrl.IntegralRoll = 0; |
FromNaviCtrl_Value.SerialDataOkay = 0; |
SPI_RxDataValid = 0; |
SPI_VersionInfo.Major = VERSION_MAJOR; |
SPI_VersionInfo.Minor = VERSION_MINOR; |
SPI_VersionInfo.Patch = VERSION_PATCH; |
SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
} |
//------------------------------------------------------ |
void SPI_StartTransmitPacket(void) |
{ |
//if ((SLAVE_SELECT_PORT & (1 << SPI_SLAVE_SELECT)) == 0) return; // transfer of prev. packet not completed |
if (!SPITransferCompleted) return; |
// _delay_us(30); |
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
SPI_TX_Buffer = (unsigned char *) &ToNaviCtrl; |
ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
SPITransferCompleted = 0; |
UpdateSPI_Buffer(); // update buffer |
SPI_BufferIndex = 1; |
//ebugOut.Analog[16]++; |
// -- 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; |
SPDR = ToNaviCtrl.Sync1; // Start transmission |
// SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
} |
//------------------------------------------------------ |
//SIGNAL(SIG_SPI) |
void SPI_TransmitByte(void) |
{ |
static unsigned char SPI_RXState = 0; |
unsigned char rxdata; |
static unsigned char rxchksum; |
if (SPITransferCompleted) return; |
if (!(SPSR & (1 << SPIF))) return; |
SendSPI = 4; |
// _delay_us(30); |
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
rxdata = SPDR; |
switch ( SPI_RXState) |
{ |
case 0: |
SPI_RxBufferIndex = 0; |
//DebugOut.Analog[17]++; |
rxchksum = rxdata; |
if (rxdata == 0x81 ) { SPI_RXState = 1; } // 1. Syncbyte ok |
break; |
case 1: |
if (rxdata == 0x55) { rxchksum += rxdata; SPI_RXState = 2; } // 2. Syncbyte ok |
else SPI_RXState = 0; |
//DebugOut.Analog[18]++; |
break; |
case 2: |
SPI_Buffer[SPI_RxBufferIndex++]= rxdata; // get data |
//DebugOut.Analog[19]++; |
if (SPI_RxBufferIndex >= sizeof(FromNaviCtrl)) |
{ |
if (rxdata == rxchksum) |
{ |
unsigned char *ptr = (unsigned char *)&FromNaviCtrl; |
memcpy(ptr, (unsigned char *) SPI_Buffer, sizeof(SPI_Buffer)); |
SPI_RxDataValid = 1; |
} |
else SPI_RxDataValid = 0; |
SPI_RXState = 0; |
} |
else rxchksum += rxdata; |
break; |
} |
if (SPI_BufferIndex < sizeof(ToNaviCtrl)) |
{ |
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_TX_Buffer[SPI_BufferIndex]; |
ToNaviCtrl.Chksum += SPI_TX_Buffer[SPI_BufferIndex]; |
// SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
} |
else SPITransferCompleted = 1; |
SPI_BufferIndex++; |
} |
//------------------------------------------------------ |
void UpdateSPI_Buffer(void) |
{ |
signed int tmp; |
cli(); |
ToNaviCtrl.IntegralNick = (int) (IntegralNick / (long)(EE_Parameter.GyroAccFaktor * 4)); |
ToNaviCtrl.IntegralRoll = (int) (IntegralRoll / (long)(EE_Parameter.GyroAccFaktor * 4)); |
ToNaviCtrl.GyroCompass = (10 * ErsatzKompass) / GIER_GRAD_FAKTOR; |
ToNaviCtrl.AccNick = ((int) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc))/4; |
ToNaviCtrl.AccRoll = ((int) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc))/4; |
NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
// ToNaviCtrl.User8 = Parameter_UserParam8; |
// ToNaviCtrl.CalState = WinkelOut.CalcState; |
switch(ToNaviCtrl.Command) // |
{ |
case SPI_CMD_USER: |
ToNaviCtrl.Param.Byte[0] = Parameter_UserParam1; |
ToNaviCtrl.Param.Byte[1] = Parameter_UserParam2; |
ToNaviCtrl.Param.Byte[2] = Parameter_UserParam3; |
ToNaviCtrl.Param.Byte[3] = Parameter_UserParam4; |
ToNaviCtrl.Param.Byte[4] = Parameter_UserParam5; |
ToNaviCtrl.Param.Byte[5] = Parameter_UserParam6; |
ToNaviCtrl.Param.Byte[6] = Parameter_UserParam7; |
ToNaviCtrl.Param.Byte[7] = Parameter_UserParam8; |
ToNaviCtrl.Param.Byte[8] = (unsigned char) MikroKopterFlags; |
MikroKopterFlags &= ~(FLAG_CALIBRATE | FLAG_START); |
ToNaviCtrl.Param.Byte[9] = (unsigned char) UBat; |
ToNaviCtrl.Param.Byte[10] =(unsigned char) EE_Parameter.UnterspannungsWarnung; |
ToNaviCtrl.Param.Byte[11] =(unsigned char) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
break; |
case SPI_CMD_PARAMETER1: |
ToNaviCtrl.Param.Byte[0] = EE_Parameter.NaviGpsModeControl; // Parameters for the Naviboard |
ToNaviCtrl.Param.Byte[1] = EE_Parameter.NaviGpsGain; |
ToNaviCtrl.Param.Byte[2] = EE_Parameter.NaviGpsP; |
ToNaviCtrl.Param.Byte[3] = EE_Parameter.NaviGpsI; |
ToNaviCtrl.Param.Byte[4] = EE_Parameter.NaviGpsD; |
ToNaviCtrl.Param.Byte[5] = EE_Parameter.NaviGpsACC; |
ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviGpsMinSat; |
ToNaviCtrl.Param.Byte[7] = EE_Parameter.NaviStickThreshold; |
ToNaviCtrl.Param.Byte[8] = EE_Parameter.NaviOperatingRadius; |
ToNaviCtrl.Param.Byte[9] = EE_Parameter.NaviWindCorrection; |
ToNaviCtrl.Param.Byte[10] = EE_Parameter.NaviSpeedCompensation; |
ToNaviCtrl.Param.Byte[11] = EE_Parameter.NaviAngleLimitation; |
break; |
case SPI_CMD_STICK: |
tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
ToNaviCtrl.Param.Byte[0] = (char) tmp; |
tmp = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
ToNaviCtrl.Param.Byte[1] = (char) tmp; |
tmp = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
ToNaviCtrl.Param.Byte[2] = (char) tmp; |
tmp = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -127) tmp = -127; |
ToNaviCtrl.Param.Byte[3] = (char) tmp; |
ToNaviCtrl.Param.Byte[4] = (unsigned char) Poti1; |
ToNaviCtrl.Param.Byte[5] = (unsigned char) Poti2; |
ToNaviCtrl.Param.Byte[6] = (unsigned char) Poti3; |
ToNaviCtrl.Param.Byte[7] = (unsigned char) Poti4; |
ToNaviCtrl.Param.Byte[8] = (unsigned char) SenderOkay; |
break; |
case SPI_CMD_MISC: |
if(WinkelOut.CalcState > 5) |
{ |
WinkelOut.CalcState = 0; |
ToNaviCtrl.Param.Byte[0] = 5; |
} |
else ToNaviCtrl.Param.Byte[0] = WinkelOut.CalcState; |
ToNaviCtrl.Param.Int[1] = HoehenWert; |
ToNaviCtrl.Param.Byte[1] = EE_Parameter.NaviPH_LoginTime; |
ToNaviCtrl.Param.Byte[4] = EE_Parameter.NaviGpsPLimit; |
ToNaviCtrl.Param.Byte[5] = EE_Parameter.NaviGpsILimit; |
ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviGpsDLimit; |
break; |
case SPI_CMD_VERSION: |
ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
ToNaviCtrl.Param.Byte[4] = PlatinenVersion; |
break; |
} |
sei(); |
if(SPI_RxDataValid) |
{ |
if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) |
{ |
GPS_Nick = FromNaviCtrl.GPS_Nick; |
GPS_Roll = FromNaviCtrl.GPS_Roll; |
NaviDataOkay = 250; |
} |
if(FromNaviCtrl.CompassValue <= 360) KompassValue = FromNaviCtrl.CompassValue; |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
if(FromNaviCtrl.BeepTime > beeptime && !WinkelOut.CalcState) beeptime = FromNaviCtrl.BeepTime; |
switch (FromNaviCtrl.Command) |
{ |
case SPI_KALMAN: |
FromNaviCtrl_Value.Kalman_K = FromNaviCtrl.Param.sByte[0]; |
FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.sByte[1]; |
FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.sByte[2]; |
FromNaviCtrl_Value.SerialDataOkay = FromNaviCtrl.Param.Byte[3]; |
break; |
case SPI_EXTCTRL: |
ExternControl.Digital[0] = FromNaviCtrl.Param.Byte[0]; |
ExternControl.Digital[1] = FromNaviCtrl.Param.Byte[1]; |
ExternControl.RemoteTasten = FromNaviCtrl.Param.Byte[2]; |
ExternControl.Nick = FromNaviCtrl.Param.sByte[3]; |
ExternControl.Roll = FromNaviCtrl.Param.sByte[4]; |
ExternControl.Gier = FromNaviCtrl.Param.sByte[5]; |
ExternControl.Gas = FromNaviCtrl.Param.Byte[6]; |
ExternControl.Hight = FromNaviCtrl.Param.sByte[7]; |
ExternControl.free = FromNaviCtrl.Param.Byte[8]; |
ExternControl.Frame = FromNaviCtrl.Param.Byte[9]; |
ExternControl.Config = FromNaviCtrl.Param.Byte[10]; |
break; |
default: |
break; |
} |
} |
else |
{ |
// KompassValue = 0; |
// KompassRichtung = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
} |
#endif |
/branches/V0.75b_Ligi_ExternEvents/spi.h |
---|
0,0 → 1,144 |
// ######################## SPI - FlightCtrl ################### |
#ifndef _SPI_H |
#define _SPI_H |
#include <util/delay.h> |
#define USE_SPI_COMMUNICATION |
#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_MISC 12 |
#define SPI_CMD_PARAMETER1 13 |
#define SPI_CMD_VERSION 14 |
struct str_ToNaviCtrl |
{ |
unsigned char Sync1, Sync2; |
unsigned char Command; |
signed int IntegralNick; |
signed int IntegralRoll; |
signed int AccNick; |
signed int AccRoll; |
signed int GyroCompass; |
signed int GyroNick; |
signed int GyroRoll; |
signed int GyroGier; |
union |
{ |
char sByte[12]; |
unsigned char Byte[12]; |
int Int[6]; |
long Long[3]; |
float Float[3]; |
} Param; |
unsigned char Chksum; |
}; |
#define SPI_KALMAN 103 |
#define SPI_EXTCTRL 104 |
struct str_FromNaviCtrl |
{ |
unsigned char Command; |
signed int GPS_Nick; |
signed int GPS_Roll; |
signed int GPS_Gier; |
signed int CompassValue; |
signed int Status; |
unsigned int BeepTime; |
union |
{ |
char sByte[12]; |
unsigned char Byte[12]; |
int Int[6]; |
long Long[3]; |
float Float[3]; |
} Param; |
unsigned char Chksum; |
}; |
struct str_FromNaviCtrl_Value |
{ |
signed char Kalman_K; |
signed char Kalman_MaxDrift; |
signed char Kalman_MaxFusion; |
unsigned char SerialDataOkay; |
}; |
struct str_SPI_VersionInfo |
{ |
unsigned char Major; |
unsigned char Minor; |
unsigned char Patch; |
unsigned char Compatible; |
}; |
#ifdef USE_SPI_COMMUNICATION |
extern struct str_FromNaviCtrl_Value FromNaviCtrl_Value; |
extern struct str_ToNaviCtrl ToNaviCtrl; |
extern struct str_FromNaviCtrl FromNaviCtrl; |
extern unsigned char SPI_CommandCounter,NaviDataOkay; |
//#define SPI_CMD_VALUE 0x03 |
extern void SPI_MasterInit(void); |
extern void SPI_StartTransmitPacket(void); |
extern void UpdateSPI_Buffer(void); |
extern void SPI_TransmitByte(void); |
#else |
// -------------------------------- Dummy ----------------------------------------- |
#define SPI_MasterInit() ; |
#define SPI_StartTransmitPacket() ; |
#define UpdateSPI_Buffer() ; |
#define SPI_TransmitByte() ; |
#endif |
#endif |
/branches/V0.75b_Ligi_ExternEvents/timer0.c |
---|
0,0 → 1,402 |
#include "main.h" |
volatile unsigned int CountMilliseconds = 0; |
volatile static unsigned int tim_main; |
volatile unsigned char UpdateMotor = 0; |
volatile unsigned int cntKompass = 0; |
volatile unsigned int beeptime = 0; |
volatile unsigned char SendSPI = 0, ServoActive = 0; |
unsigned int BeepMuster = 0xffff; |
int ServoValue = 0; |
volatile int16_t ServoNickValue = 0; |
volatile int16_t ServoRollValue = 0; |
enum { |
STOP = 0, |
CK = 1, |
CK8 = 2, |
CK64 = 3, |
CK256 = 4, |
CK1024 = 5, |
T0_FALLING_EDGE = 6, |
T0_RISING_EDGE = 7 |
}; |
SIGNAL (SIG_OVERFLOW0) // 8kHz |
{ |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char pieper_ein = 0; |
// TCNT0 -= 250;//TIMER_RELOAD_VALUE; |
if(SendSPI) SendSPI--; |
if(!cnt--) |
{ |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; |
CountMilliseconds++; |
} |
if(beeptime >= 1) |
{ |
beeptime--; |
if(beeptime & BeepMuster) |
{ |
pieper_ein = 1; |
} |
else pieper_ein = 0; |
} |
else |
{ |
pieper_ein = 0; |
BeepMuster = 0xffff; |
} |
if(pieper_ein) |
{ |
if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2 |
else PORTC |= (1<<7); // Speaker an PORTC.7 |
} |
else |
{ |
if(PlatinenVersion == 10) PORTD &= ~(1<<2); |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
if(PINC & 0x10) |
{ |
cntKompass++; |
} |
else |
{ |
if((cntKompass) && (cntKompass < 362)) |
{ |
cntKompass += cntKompass / 41; |
if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; |
} |
// if(cntKompass < 10) cntKompass = 10; |
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
cntKompass = 0; |
} |
} |
} |
// ----------------------------------------------------------------------- |
unsigned int SetDelay (unsigned int t) |
{ |
// TIMSK0 &= ~_BV(TOIE0); |
return(CountMilliseconds + t + 1); |
// TIMSK0 |= _BV(TOIE0); |
} |
// ----------------------------------------------------------------------- |
char CheckDelay(unsigned int t) |
{ |
// TIMSK0 &= ~_BV(TOIE0); |
return(((t - CountMilliseconds) & 0x8000) >> 9); |
// TIMSK0 |= _BV(TOIE0); |
} |
// ----------------------------------------------------------------------- |
void Delay_ms(unsigned int w) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while (!CheckDelay(akt)); |
} |
void Delay_ms_Mess(unsigned int w) |
{ |
unsigned int akt; |
akt = SetDelay(w); |
while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;} |
} |
/*****************************************************/ |
/* 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(); |
PORTD &= ~(1<<PORTD7); // set PD7 to low |
DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
HEF4017R_ON; |
// Timer/Counter 2 Control Register A |
// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1) |
// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
TCCR2A |= (1<<WGM21)|(1<<WGM20); |
// Timer/Counter 2 Control Register B |
// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22)); |
TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22); |
// Initialize the Timer/Counter 2 Register |
TCNT2 = 0; |
// Initialize the Output Compare Register A used for PWM generation on port PD7. |
OCR2A = 255; |
TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0 |
// Timer/Counter 2 Interrupt Mask Register |
// Enable timer output compare match A Interrupt only |
TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
TIMSK2 |= (1<<OCIE2A); |
SREG = sreg; |
} |
//---------------------------- |
void Timer_Init(void) |
{ |
tim_main = SetDelay(10); |
TCCR0B = CK8; |
TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM |
OCR0A = 0; |
OCR0B = 120; |
TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload |
//OCR1 = 0x00; |
TIMSK0 |= _BV(TOIE0); |
} |
/*****************************************************/ |
/* Control Servo Position */ |
/*****************************************************/ |
ISR(TIMER2_COMPA_vect) |
{ |
// frame len 22.5 ms = 14063 * 1.6 us |
// stop pulse: 0.3 ms = 188 * 1.6 us |
// min servo pulse: 0.6 ms = 375 * 1.6 us |
// max servo pulse: 2.4 ms = 1500 * 1.6 us |
// resolution: 1500 - 375 = 1125 steps |
#define IRS_RUNTIME 127 |
#define PPM_STOPPULSE 188 |
// #define PPM_FRAMELEN (14063 |
#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh) |
#define MINSERVOPULSE 375 |
#define MAXSERVOPULSE 1500 |
#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
static uint8_t PulseOutput = 0; |
static uint16_t RemainingPulse = 0; |
static uint16_t ServoFrameTime = 0; |
static uint8_t ServoIndex = 0; |
#define MULTIPLYER 4 |
static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
if(PlatinenVersion < 20) |
{ |
//--------------------------- |
// Nick servo state machine |
//--------------------------- |
if(!PulseOutput) // pulse output complete |
{ |
if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
{ |
TCCR2A &= ~(1<<COM2A0);// make a high pulse |
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
// range servo pulse width |
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
// accumulate time for correct update rate |
ServoFrameTime = RemainingPulse; |
} |
else // we had a high pulse |
{ |
TCCR2A |= (1<<COM2A0); // make a low pulse |
RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
} |
// set pulse output active |
PulseOutput = 1; |
} |
} // EOF Nick servo state machine |
else |
{ |
//----------------------------------------------------- |
// PPM state machine, onboard demultiplexed by HEF4017 |
//----------------------------------------------------- |
if(!PulseOutput) // pulse output complete |
{ |
if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
{ |
TCCR2A &= ~(1<<COM2A0);// make a high pulse |
if(ServoIndex == 0) // if we are at the sync gap |
{ |
RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
ServoFrameTime = 0; // reset servo frame time |
HEF4017R_ON; // enable HEF4017 reset |
} |
else // servo channels |
{ |
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
switch(ServoIndex) // map servo channels |
{ |
case 1: // Nick Compensation Servo |
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
break; |
case 2: // Roll Compensation Servo |
ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x02) |
{ // inverting movement of servo |
ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
} |
else |
if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
} |
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoRollValue /= MULTIPLYER; |
//DebugOut.Analog[20] = ServoRollValue; |
break; |
default: // other servo channels |
RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
break; |
} |
// range servo pulse width |
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
// substract stop pulse width |
RemainingPulse -= PPM_STOPPULSE; |
// accumulate time for correct sync gap |
ServoFrameTime += RemainingPulse; |
} |
} |
else // we had a high pulse |
{ |
TCCR2A |= (1<<COM2A0); // make a low pulse |
// set pulsewidth to stop pulse width |
RemainingPulse = PPM_STOPPULSE; |
// accumulate time for correct sync gap |
ServoFrameTime += RemainingPulse; |
if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset |
else HEF4017R_ON; |
ServoIndex++; // change to next servo channel |
if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
} |
// set pulse output active |
PulseOutput = 1; |
} |
} // EOF PPM state machine |
// General pulse output generator |
if(RemainingPulse > (255 + IRS_RUNTIME)) |
{ |
OCR2A = 255; |
RemainingPulse -= 255; |
} |
else |
{ |
if(RemainingPulse > 255) // this is the 2nd last part |
{ |
if((RemainingPulse - 255) < IRS_RUNTIME) |
{ |
OCR2A = 255 - IRS_RUNTIME; |
RemainingPulse -= 255 - IRS_RUNTIME; |
} |
else // last part > ISR_RUNTIME |
{ |
OCR2A = 255; |
RemainingPulse -= 255; |
} |
} |
else // this is the last part |
{ |
OCR2A = RemainingPulse; |
RemainingPulse = 0; |
PulseOutput = 0; // trigger to stop pulse |
} |
} // EOF general pulse output generator |
} |
/branches/V0.75b_Ligi_ExternEvents/timer0.h |
---|
0,0 → 1,20 |
#define TIMER_TEILER CK8 |
#define TIMER_RELOAD_VALUE 250 |
#define HEF4017R_ON PORTC |= (1<<PORTC6) |
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
void Timer_Init(void); |
void TIMER2_Init(void); |
void Delay_ms(unsigned int); |
void Delay_ms_Mess(unsigned int); |
unsigned int SetDelay (unsigned int t); |
char CheckDelay (unsigned int t); |
extern volatile unsigned int CountMilliseconds; |
extern volatile unsigned char UpdateMotor; |
extern volatile unsigned int beeptime; |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
extern unsigned int BeepMuster; |
extern volatile unsigned char SendSPI, ServoActive; |
/branches/V0.75b_Ligi_ExternEvents/twimaster.c |
---|
0,0 → 1,216 |
/*############################################################################ |
############################################################################*/ |
#include "main.h" |
volatile unsigned char twi_state = 0; |
unsigned char motor = 0; |
unsigned char motorread = 0,MissingMotor = 0; |
unsigned char motor_rx[16],motor_rx2[16]; |
unsigned char MotorPresent[MAX_MOTORS]; |
unsigned char MotorError[MAX_MOTORS]; |
//############################################################################ |
//Initzialisieren der I2C (TWI) Schnittstelle |
void i2c_init(void) |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
//############################################################################ |
//Start I2C |
void i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
} |
//############################################################################ |
void i2c_stop(void) |
//############################################################################ |
{ |
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
} |
void i2c_reset(void) |
//############################################################################ |
{ |
i2c_stop(); |
twi_state = 0; |
motor = TWDR; |
motor = 0; |
TWCR = 0x80; |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
i2c_init(); |
i2c_start(); |
i2c_write_byte(0); |
} |
//############################################################################ |
void i2c_write_byte(char byte) |
//############################################################################ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
} |
/****************************************/ |
/* 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); |
} |
//############################################################################ |
SIGNAL (TWI_vect) |
//############################################################################ |
{ |
static unsigned char missing_motor; |
switch(twi_state++) |
{ |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Writing the Data |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 0: |
while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++; // skip if not used |
if(motor == MAX_MOTORS) // writing finished -> now read |
{ |
motor = 0; |
twi_state = 3; |
i2c_write_byte(0x53+(motorread*2)); |
} |
else i2c_write_byte(0x52+(motor*2)); |
break; |
case 1: |
i2c_write_byte(Motor[motor++]); |
break; |
case 2: |
if(TWSR == 0x30) { if(!missing_motor) missing_motor = motor; if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;} |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
i2c_start(); |
break; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Reading Data |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 3: |
//Transmit 1st byte for reading |
if(TWSR != 0x40) // Error? |
{ |
MotorPresent[motorread] = 0; |
motorread++; |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
} |
else |
{ |
MotorPresent[motorread] = ('1' - '-') + motorread; |
I2C_ReceiveByte(); |
} |
MissingMotor = missing_motor; |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
motor_rx[motorread] = TWDR; |
I2C_ReceiveLastByte(); //nack |
break; |
case 5: |
//Read 2nd byte |
motor_rx2[motorread++] = TWDR; |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
break; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// writing Gyro-Offset |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 8: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 9: |
i2c_write_byte(0x10); // Update Channel A |
break; |
case 10: |
i2c_write_byte(AnalogOffsetNick); // Value |
break; |
case 11: |
i2c_write_byte(0x80); // Value |
break; |
case 12: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
break; |
case 13: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 14: |
i2c_write_byte(0x12); // Update Channel B |
break; |
case 15: |
i2c_write_byte(AnalogOffsetRoll); // Value |
break; |
case 16: |
i2c_write_byte(0x80); // Value |
break; |
case 17: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
break; |
case 18: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 19: |
i2c_write_byte(0x14); // Update Channel C |
break; |
case 20: |
i2c_write_byte(AnalogOffsetGier); // Value |
break; |
case 21: |
i2c_write_byte(0x80); // Value |
break; |
case 22: |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
break; |
default: twi_state = 0; |
break; |
} |
TWCR |= 0x80; |
} |
/branches/V0.75b_Ligi_ExternEvents/twimaster.h |
---|
0,0 → 1,35 |
/*############################################################################ |
############################################################################*/ |
#ifndef _I2C_MASTER_H |
#define _I2C_MASTER_H |
//############################################################################ |
// I2C Konstanten |
#define SCL_CLOCK 200000L |
#define I2C_TIMEOUT 30000 |
#define I2C_START 0x08 |
#define I2C_REPEATED_START 0x10 |
#define I2C_TX_SLA_ACK 0x18 |
#define I2C_TX_DATA_ACK 0x28 |
#define I2C_RX_SLA_ACK 0x40 |
#define I2C_RX_DATA_ACK 0x50 |
//############################################################################ |
extern volatile unsigned char twi_state; |
extern unsigned char motor,MissingMotor; |
extern unsigned char motorread; |
extern unsigned char motor_rx[]; |
extern unsigned char MotorPresent[]; |
extern unsigned char MotorError[]; |
void i2c_reset(void); |
extern void i2c_init (void); // I2C initialisieren |
extern void i2c_start (void); // Start I2C |
extern void i2c_stop (void); // Stop I2C |
extern void i2c_write_byte (char byte); // 1 Byte schreiben |
extern void i2c_reset(void); |
#endif |
/branches/V0.75b_Ligi_ExternEvents/uart.c |
---|
0,0 → 1,577 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdarg.h> |
#include <string.h> |
#include "main.h" |
#include "uart.h" |
#define FC_ADDRESS 1 |
#define NC_ADDRESS 2 |
#define MK3MAG_ADDRESS 3 |
unsigned char GetExternalControl = 0,DebugDisplayAnforderung1 = 0, DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0, GetPPMChannelAnforderung = 0; |
unsigned char DisplayLine = 0; |
unsigned volatile char SioTmp = 0; |
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF]; |
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF]; |
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF]; |
unsigned volatile char NeuerDatensatzEmpfangen = 0; |
unsigned volatile char NeueKoordinateEmpfangen = 0; |
unsigned volatile char UebertragungAbgeschlossen = 1; |
unsigned volatile char CntCrcError = 0; |
unsigned volatile char AnzahlEmpfangsBytes = 0; |
unsigned char *pRxData = 0; |
unsigned char RxDataLen = 0; |
unsigned volatile char PC_DebugTimeout = 0; |
unsigned volatile char PC_MotortestActive = 0; |
unsigned char DebugTextAnforderung = 255; |
unsigned char PcZugriff = 100; |
unsigned char MotorTest[16]; |
unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl |
unsigned char ConfirmFrame; |
struct str_DebugOut DebugOut; |
struct str_ExternControl ExternControl; |
struct str_ExternEvent ExternEvent; |
struct str_VersionInfo VersionInfo; |
struct str_WinkelOut WinkelOut; |
struct str_Data3D Data3D; |
int Debug_Timer,Kompass_Timer,Timer3D; |
unsigned int DebugDataIntervall = 200, Intervall3D = 0; |
const unsigned char ANALOG_TEXT[32][16] = |
{ |
//1234567890123456 |
"AngleNick ", //0 |
"AngleRoll ", |
"AccNick ", |
"AccRoll ", |
"GyroGier ", |
"Hight Value ", //5 |
"AccZ ", |
"Gas ", |
"Compass Value ", |
"Voltage ", |
"Empfang ", //10 |
"Gyro Kompass ", |
"Motor Front ", |
"Motor Rear ", |
"Motor Left ", |
"Motor Right ", //15 |
"ligi ", |
" ", |
" ", |
"MK3Mag CalState ", |
"Servo ", //20 |
" ", |
" ", |
" ", |
" ", |
" ", //25 |
" ", |
"Kalman_MaxDrift ", |
" ", |
"Navi Serial Data", |
"GPS_Nick ", //30 |
"GPS_Roll " |
}; |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++ Sende-Part der Datenübertragung |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
SIGNAL(INT_VEC_TX) |
{ |
static unsigned int ptr = 0; |
unsigned char tmp_tx; |
if(!UebertragungAbgeschlossen) |
{ |
ptr++; // die [0] wurde schon gesendet |
tmp_tx = SendeBuffer[ptr]; |
if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF)) |
{ |
ptr = 0; |
UebertragungAbgeschlossen = 1; |
} |
UDR = tmp_tx; |
} |
else ptr = 0; |
} |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
SIGNAL(INT_VEC_RX) |
{ |
static unsigned int crc; |
static unsigned char crc1,crc2,buf_ptr; |
static unsigned char UartState = 0; |
unsigned char CrcOkay = 0; |
SioTmp = UDR; |
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0; |
if(SioTmp == '\r' && UartState == 2) |
{ |
UartState = 0; |
crc -= RxdBuffer[buf_ptr-2]; |
crc -= RxdBuffer[buf_ptr-1]; |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
CrcOkay = 0; |
if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;}; |
if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet |
{ |
NeuerDatensatzEmpfangen = 1; |
AnzahlEmpfangsBytes = buf_ptr + 1; |
RxdBuffer[buf_ptr] = '\r'; |
if(RxdBuffer[2] == 'R') |
{ |
wdt_enable(WDTO_250MS); // Reset-Commando |
ServoActive = 0; |
} |
} |
} |
else |
switch(UartState) |
{ |
case 0: |
if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet |
buf_ptr = 0; |
RxdBuffer[buf_ptr++] = SioTmp; |
crc = SioTmp; |
break; |
case 1: // Adresse auswerten |
UartState++; |
RxdBuffer[buf_ptr++] = SioTmp; |
crc += SioTmp; |
break; |
case 2: // Eingangsdaten sammeln |
RxdBuffer[buf_ptr] = SioTmp; |
if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++; |
else UartState = 0; |
crc += SioTmp; |
break; |
default: |
UartState = 0; |
break; |
} |
} |
// -------------------------------------------------------------------------- |
void AddCRC(unsigned int wieviele) |
{ |
unsigned int tmpCRC = 0,i; |
for(i = 0; i < wieviele;i++) |
{ |
tmpCRC += SendeBuffer[i]; |
} |
tmpCRC %= 4096; |
SendeBuffer[i++] = '=' + tmpCRC / 64; |
SendeBuffer[i++] = '=' + tmpCRC % 64; |
SendeBuffer[i++] = '\r'; |
UebertragungAbgeschlossen = 0; |
UDR = SendeBuffer[0]; |
} |
// -------------------------------------------------------------------------- |
void SendOutData(unsigned char cmd,unsigned char address, unsigned char BufferAnzahl, ...) //unsigned char *snd, unsigned char len) |
{ |
va_list ap; |
unsigned int pt = 0; |
unsigned char a,b,c; |
unsigned char ptr = 0; |
unsigned char *snd = 0; |
int len = 0; |
SendeBuffer[pt++] = '#'; // Startzeichen |
SendeBuffer[pt++] = 'a' + address; // Adresse (a=0; b=1,...) |
SendeBuffer[pt++] = cmd; // Commando |
va_start(ap, BufferAnzahl); |
if(BufferAnzahl) |
{ |
snd = va_arg(ap, unsigned char*); |
len = va_arg(ap, int); |
ptr = 0; |
BufferAnzahl--; |
} |
while(len) |
{ |
if(len) |
{ |
a = snd[ptr++]; |
len--; |
if((!len) && BufferAnzahl) |
{ |
snd = va_arg(ap, unsigned char*); |
len = va_arg(ap, int); |
ptr = 0; |
BufferAnzahl--; |
} |
} |
else a = 0; |
if(len) |
{ |
b = snd[ptr++]; |
len--; |
if((!len) && BufferAnzahl) |
{ |
snd = va_arg(ap, unsigned char*); |
len = va_arg(ap, int); |
ptr = 0; |
BufferAnzahl--; |
} |
} |
else b = 0; |
if(len) |
{ |
c = snd[ptr++]; |
len--; |
if((!len) && BufferAnzahl) |
{ |
snd = va_arg(ap, unsigned char*); |
len = va_arg(ap, int); |
ptr = 0; |
BufferAnzahl--; |
} |
} |
else c = 0; |
SendeBuffer[pt++] = '=' + (a >> 2); |
SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
SendeBuffer[pt++] = '=' + ( c & 0x3f); |
} |
va_end(ap); |
AddCRC(pt); |
} |
// -------------------------------------------------------------------------- |
void Decode64(void) // die daten werden im rx buffer dekodiert, das geht nur, weil aus 4 byte immer 3 gemacht werden. |
{ |
unsigned char a,b,c,d; |
unsigned char x,y,z; |
unsigned char ptrIn = 3; // start at begin of data block |
unsigned char ptrOut = 3; |
unsigned char len = AnzahlEmpfangsBytes - 6; // von der Gesamtbytezahl eines Frames gehen 3 Bytes des Headers ('#',Addr, Cmd) und 3 Bytes des Footers (CRC1, CRC2, '\r') ab. |
while(len) |
{ |
a = RxdBuffer[ptrIn++] - '='; |
b = RxdBuffer[ptrIn++] - '='; |
c = RxdBuffer[ptrIn++] - '='; |
d = RxdBuffer[ptrIn++] - '='; |
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if(len--) RxdBuffer[ptrOut++] = x; else break; |
if(len--) RxdBuffer[ptrOut++] = y; else break; |
if(len--) RxdBuffer[ptrOut++] = z; else break; |
} |
pRxData = (unsigned char*)&RxdBuffer[3]; // decodierte Daten beginnen beim 4. Byte |
RxDataLen = ptrOut - 3; // wie viele Bytes wurden dekodiert? |
} |
// -------------------------------------------------------------------------- |
void BearbeiteRxDaten(void) |
{ |
if(!NeuerDatensatzEmpfangen) return; |
unsigned char tempchar1, tempchar2; |
Decode64(); // dekodiere datenblock im Empfangsbuffer |
switch(RxdBuffer[1]-'a') // check for Slave Address |
{ |
case FC_ADDRESS: // FC special commands |
switch(RxdBuffer[2]) |
{ |
case 'K':// Kompasswert |
memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue)); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
break; |
case 't':// Motortest |
if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4); |
PC_MotortestActive = 240; |
//while(!UebertragungAbgeschlossen); |
//SendOutData('T', MeineSlaveAdresse, 0); |
PcZugriff = 255; |
break; |
case 'n':// "Get Mixer |
while(!UebertragungAbgeschlossen); |
SendOutData('N', FC_ADDRESS, 1, (unsigned char *) &Mixer,sizeof(Mixer)); |
break; |
case 'm':// "Write Mixer |
while(!UebertragungAbgeschlossen); |
if(pRxData[0] == MIXER_REVISION) |
{ |
memcpy(&Mixer, (unsigned char *)pRxData, sizeof(Mixer)); |
eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
tempchar1 = 1; |
} |
else tempchar1 = 0; |
SendOutData('M', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
break; |
case 'p': // get PPM Channels |
GetPPMChannelAnforderung = 1; |
break; |
case 'q':// "Get"-Anforderung für Settings |
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
if(pRxData[0] == 0xFF) |
{ |
pRxData[0] = GetActiveParamSetNumber(); |
} |
// limit settings range |
if(pRxData[0] < 1) pRxData[0] = 1; // limit to 5 |
else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
ReadParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
while(!UebertragungAbgeschlossen); |
tempchar1 = pRxData[0]; |
tempchar2 = EE_DATENREVISION; |
SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
break; |
case 's': // Parametersatz speichern |
if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EE_DATENREVISION)) // check for setting to be in range |
{ |
memcpy((unsigned char *) &EE_Parameter.Kanalbelegung[0], (unsigned char *)&pRxData[2], STRUCT_PARAM_LAENGE); |
WriteParameterSet(pRxData[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
SetActiveParamSetNumber(pRxData[0]); |
tempchar1 = GetActiveParamSetNumber(); |
Piep(tempchar1,110); |
} |
else |
{ |
tempchar1 = 0; // mark in response an invlid setting |
} |
while(!UebertragungAbgeschlossen); |
SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
break; |
} // case FC_ADDRESS: |
default: // any Slave Address |
switch(RxdBuffer[2]) |
{ |
// 't' comand placed here only for compatibility to BL |
case 't':// Motortest |
if(AnzahlEmpfangsBytes > 20) memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
else memcpy(&MotorTest[0], (unsigned char *)pRxData, 4); |
while(!UebertragungAbgeschlossen); |
SendOutData('T', MeineSlaveAdresse, 0); |
PC_MotortestActive = 250; |
PcZugriff = 255; |
break; |
// 'K' comand placed here only for compatibility to old MK3MAG software, that does not send the right Slave Address |
case 'K':// Kompasswert |
memcpy((unsigned char *)&KompassValue , (unsigned char *)pRxData, sizeof(KompassValue)); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
break; |
case 'a':// Texte der Analogwerte |
DebugTextAnforderung = pRxData[0]; |
if (DebugTextAnforderung > 31) DebugTextAnforderung = 31; |
PcZugriff = 255; |
break; |
case 'b': |
memcpy((unsigned char *)&ExternControl, (unsigned char *)pRxData, sizeof(ExternControl)); |
ConfirmFrame = ExternControl.Frame; |
PcZugriff = 255; |
break; |
case 'c': // Poll the 3D-Data |
if(!Intervall3D) { if(pRxData[0]) Timer3D = SetDelay(pRxData[0] * 10);} |
Intervall3D = pRxData[0] * 10; |
break; |
case 'd': // Poll the debug data |
DebugDataIntervall = pRxData[0] * 10; |
if(DebugDataIntervall > 0) DebugDataAnforderung = 1; |
break; |
case 'h':// x-1 Displayzeilen |
PcZugriff = 255; |
RemoteKeys |= pRxData[0]; |
if(RemoteKeys) DisplayLine = 0; |
DebugDisplayAnforderung = 1; |
break; |
case 'l':// x-1 Displayzeilen |
PcZugriff = 255; |
MenuePunkt = pRxData[0]; |
DebugDisplayAnforderung1 = 1; |
break; |
case 'v': // Version-Anforderung und Ausbaustufe |
GetVersionAnforderung = 1; |
break; |
case 'g':// |
GetExternalControl = 1; |
break; |
case 'e': |
while(!UebertragungAbgeschlossen); |
memcpy(&ExternEvent, (unsigned char *)pRxData, sizeof(ExternEvent)); |
tempchar1=pRxData[0]; |
SendOutData('E', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
break; |
} |
break; // default: |
} |
NeuerDatensatzEmpfangen = 0; |
pRxData = 0; |
RxDataLen = 0; |
} |
//############################################################################ |
//Routine für die Serielle Ausgabe |
int uart_putchar (char c) |
//############################################################################ |
{ |
if (c == '\n') |
uart_putchar('\r'); |
//Warten solange bis Zeichen gesendet wurde |
loop_until_bit_is_set(USR, UDRE); |
//Ausgabe des Zeichens |
UDR = c; |
return (0); |
} |
// -------------------------------------------------------------------------- |
void WriteProgramData(unsigned int pos, unsigned char wert) |
{ |
//if (ProgramLocation == IN_RAM) Buffer[pos] = wert; |
// else eeprom_write_byte(&EE_Buffer[pos], wert); |
// Buffer[pos] = wert; |
} |
//############################################################################ |
//INstallation der Seriellen Schnittstelle |
void UART_Init (void) |
//############################################################################ |
{ |
//Enable TXEN im Register UCR TX-Data Enable & RX Enable |
UCR=(1 << TXEN) | (1 << RXEN); |
// UART Double Speed (U2X) |
USR |= (1<<U2X); |
// RX-Interrupt Freigabe |
UCSRB |= (1<<RXCIE); |
// TX-Interrupt Freigabe |
UCSRB |= (1<<TXCIE); |
//Teiler wird gesetzt |
UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1); |
//UBRR = 33; |
//öffnet einen Kanal für printf (STDOUT) |
//fdevopen (uart_putchar, 0); |
//sbi(PORTD,4); |
Debug_Timer = SetDelay(DebugDataIntervall); |
Kompass_Timer = SetDelay(220); |
VersionInfo.SWMajor = VERSION_MAJOR; |
VersionInfo.SWMinor = VERSION_MINOR; |
VersionInfo.SWPatch = VERSION_PATCH; |
VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
pRxData = 0; |
RxDataLen = 0; |
} |
//--------------------------------------------------------------------------------------------- |
void DatenUebertragung(void) |
{ |
if(!UebertragungAbgeschlossen) return; |
if(DebugDisplayAnforderung && UebertragungAbgeschlossen) |
{ |
Menu(); |
SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
DisplayLine++; |
if(DisplayLine >= 4) DisplayLine = 0; |
DebugDisplayAnforderung = 0; |
} |
if(DebugDisplayAnforderung1 && UebertragungAbgeschlossen) |
{ |
Menu(); |
SendOutData('L', FC_ADDRESS, 3, &MenuePunkt, sizeof(MenuePunkt), &MaxMenue, sizeof(MaxMenue), DisplayBuff, sizeof(DisplayBuff)); |
DebugDisplayAnforderung1 = 0; |
} |
if(GetVersionAnforderung && UebertragungAbgeschlossen) |
{ |
SendOutData('V', FC_ADDRESS, 1, (unsigned char *) &VersionInfo, sizeof(VersionInfo)); |
GetVersionAnforderung = 0; |
} |
if(GetExternalControl && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
{ |
SendOutData('G',MeineSlaveAdresse, 1, (unsigned char *) &ExternControl, sizeof(ExternControl)); |
GetExternalControl = 0; |
} |
if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen) |
{ |
WinkelOut.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
WinkelOut.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
WinkelOut.UserParameter[0] = Parameter_UserParam1; |
WinkelOut.UserParameter[1] = Parameter_UserParam2; |
SendOutData('w', MK3MAG_ADDRESS, 1, (unsigned char *) &WinkelOut,sizeof(WinkelOut)); |
if(WinkelOut.CalcState > 4) WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt |
Kompass_Timer = SetDelay(99); |
} |
if(((DebugDataIntervall>0 && CheckDelay(Debug_Timer)) || DebugDataAnforderung) && UebertragungAbgeschlossen) |
{ |
SendOutData('D', FC_ADDRESS, 1, (unsigned char *) &DebugOut,sizeof(DebugOut)); |
DebugDataAnforderung = 0; |
if(DebugDataIntervall>0) Debug_Timer = SetDelay(DebugDataIntervall); |
} |
if(Intervall3D > 0 && CheckDelay(Timer3D) && UebertragungAbgeschlossen) |
{ |
Data3D.Winkel[0] = (int) (IntegralNick / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
Data3D.Winkel[1] = (int) (IntegralRoll / (EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad |
Data3D.Winkel[2] = (int) ((10 * ErsatzKompass) / GIER_GRAD_FAKTOR); |
SendOutData('C', FC_ADDRESS, 1, (unsigned char *) &Data3D,sizeof(Data3D)); |
Timer3D = SetDelay(Intervall3D); |
} |
if(DebugTextAnforderung != 255) // Texte für die Analogdaten |
{ |
SendOutData('A', FC_ADDRESS, 2, (unsigned char *)&DebugTextAnforderung, sizeof(DebugTextAnforderung),(unsigned char *) ANALOG_TEXT[DebugTextAnforderung], 16); |
DebugTextAnforderung = 255; |
} |
if(ConfirmFrame && UebertragungAbgeschlossen) // Datensatz bestätigen |
{ |
SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
ConfirmFrame = 0; |
} |
if(GetPPMChannelAnforderung && UebertragungAbgeschlossen) |
{ |
SendOutData('P', FC_ADDRESS, 1, (unsigned char *) &PPM_in, sizeof(PPM_in)); |
GetPPMChannelAnforderung = 0; |
} |
} |
/branches/V0.75b_Ligi_ExternEvents/uart.h |
---|
0,0 → 1,146 |
#ifndef _UART_H |
#define _UART_H |
#define MAX_SENDE_BUFF 150 |
#define MAX_EMPFANGS_BUFF 150 |
void BearbeiteRxDaten(void); |
extern unsigned char DebugGetAnforderung; |
extern unsigned volatile char SendeBuffer[MAX_SENDE_BUFF]; |
extern unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF]; |
extern unsigned volatile char UebertragungAbgeschlossen; |
extern unsigned volatile char PC_DebugTimeout; |
extern unsigned volatile char NeueKoordinateEmpfangen; |
extern unsigned volatile char PC_MotortestActive; |
extern unsigned char MeineSlaveAdresse; |
extern unsigned char PcZugriff; |
extern unsigned char RemotePollDisplayLine; |
extern int Debug_Timer,Kompass_Timer; |
extern void UART_Init (void); |
extern int uart_putchar (char c); |
extern void boot_program_page (uint32_t page, uint8_t *buf); |
extern void DatenUebertragung(void); |
extern void Uart1Init(void); |
extern void BearbeiteRxDaten(void); |
extern unsigned char MotorTest[16]; |
struct str_DebugOut |
{ |
unsigned char Digital[2]; |
signed int Analog[32]; // Debugwerte |
}; |
extern struct str_DebugOut DebugOut; |
struct str_WinkelOut |
{ |
signed int Winkel[2]; |
unsigned char UserParameter[2]; |
unsigned char CalcState; |
unsigned char Orientation; |
}; |
extern struct str_WinkelOut WinkelOut; |
struct str_Data3D |
{ |
signed int Winkel[3]; // nick, roll, compass in 0,1° |
signed char reserve[8]; |
}; |
extern struct str_Data3D Data3D; |
struct str_ExternControl |
{ |
unsigned char Digital[2]; |
unsigned char RemoteTasten; |
signed char Nick; |
signed char Roll; |
signed char Gier; |
unsigned char Gas; |
signed char Hight; |
unsigned char free; |
unsigned char Frame; |
unsigned char Config; |
}; |
extern struct str_ExternControl ExternControl; |
struct str_ExternEvent |
{ |
unsigned char key; |
unsigned char values[4]; |
}; |
extern struct str_ExternEvent ExternEvent; |
struct str_VersionInfo |
{ |
unsigned char SWMajor; |
unsigned char SWMinor; |
unsigned char ProtoMajor; |
unsigned char ProtoMinor; |
unsigned char SWPatch; |
unsigned char Reserved[5]; |
}; |
extern struct str_VersionInfo VersionInfo; |
//Die Baud_Rate der Seriellen Schnittstelle ist 9600 Baud |
//#define BAUD_RATE 9600 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 14400 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 28800 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 38400 //Baud Rate für die Serielle Schnittstelle |
#define BAUD_RATE 57600 //Baud Rate für die Serielle Schnittstelle |
//Anpassen der seriellen Schnittstellen Register wenn ein ATMega128 benutzt wird |
#if defined (__AVR_ATmega128__) |
# define USR UCSR0A |
# define UCR UCSR0B |
# define UDR UDR0 |
# define UBRR UBRR0L |
# define EICR EICRB |
#endif |
#if defined (__AVR_ATmega32__) |
# define USR UCSRA |
# define UCR UCSRB |
# define UBRR UBRRL |
# define EICR EICRB |
# define INT_VEC_RX SIG_UART_RECV |
# define INT_VEC_TX SIG_UART_TRANS |
#endif |
#if defined (__AVR_ATmega644__) |
# define USR UCSR0A |
# define UCR UCSR0B |
# define UDR UDR0 |
# define UBRR UBRR0L |
# define EICR EICR0B |
# define TXEN TXEN0 |
# define RXEN RXEN0 |
# define RXCIE RXCIE0 |
# define TXCIE TXCIE0 |
# define U2X U2X0 |
# define UCSRB UCSR0B |
# define UDRE UDRE0 |
# define INT_VEC_RX SIG_USART_RECV |
# define INT_VEC_TX SIG_USART_TRANS |
#endif |
#if defined (__AVR_ATmega644P__) |
# define USR UCSR0A |
# define UCR UCSR0B |
# define UDR UDR0 |
# define UBRR UBRR0L |
# define EICR EICR0B |
# define TXEN TXEN0 |
# define RXEN RXEN0 |
# define RXCIE RXCIE0 |
# define TXCIE TXCIE0 |
# define U2X U2X0 |
# define UCSRB UCSR0B |
# define UDRE UDRE0 |
# define INT_VEC_RX SIG_USART_RECV |
# define INT_VEC_TX SIG_USART_TRANS |
#endif |
#endif //_UART_H |
/branches/V0.75b_Ligi_ExternEvents/version.txt |
---|
0,0 → 1,271 |
------- |
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.69e 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 Präzision 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 |
0.71b: H.Buss 19.10.2008 |
Kommunikation zum Navi erweitert: |
- Beeptime jetzt 32Bit |
- Datenfusion und Driftkopensation wird durch NaviBoard unterstützt |
0.71c: H.Buss 20.10.2008 |
- LoopConfig heisst jetzt BitConfig |
- 3-Fach-Schalter für Höhensteuerung möglich -> kann man mit GPS-Schalter zusammenlegen |
- bei den Settings wurde Setting[0] mit abgespeichert, welches es nicht gab. |
- in Zukunft werden bei neuen EEPROM-Settings die Kanäle von Setting 1 übernommen |
- Variablen NaviWindCorrection, NaviSpeedCompensation, NaviOperatingRadius eingeführt |
0.71f: H.Buss 15.11.2008 |
- Ausschalten der Höhenregelung per Schalter um 0,3 sek verzögert |
- bei der seriellen Übertragung hat die FC jetzt als SlaveAdresse die 1 |
- VersionInfo.NaviKompatibel eingeführt |
- wenn manuell gegiert wird, wird der GyroKompass-Wert auf den Kompasswert gesetzt |
- Luftdruckwert wird an das Navi übertragen |
- Der Baro-Offset wird jetzt nachgeführt, um den Messbereich zu erweitern. Geht nur bei Höhenregler mit Schalter |
- Debugdaten können jetzt mit 'f' gepollt werden |
0.71g: Gregor 09.12.2008 |
- Kommunikation überarbeitet |
Infos hier: http://www.mikrokopter.de/ucwiki/en/SerialCommands |
0.71h: H.Buss 15.12.2008 |
- Freigegebene Version |
- NaviAngleLimitation als Parameter zum Navi implementiert |
- Antwort auf CMD: 't' entfernt |
0.72d: H.Buss 22.01.2009 |
- OCTO als Compilerschalter |
- Unterstützung der FC 2.0 (ME) |
- GYRO_D eingeführt |
- Achsenkopplung jetzt auch auf Nick/Roll-Bewegung |
0.72e: H.Buss 27.01.2009 |
- die 0.72d hatte kein Integral im Gier |
- Parameter eingeführt: |
EE_Parameter.NaviGpsPLimit |
EE_Parameter.NaviGpsILimit |
EE_Parameter.NaviGpsDLimit |
EE_Parameter.NaviPH_LoginTime |
EE_Parameter.AchsKopplung2 |
EE_Parameter.CouplingYawCorrection |
0.72f: H.Buss 28.01.2009 |
- Bug im Ersatzkompass entfernt |
0.72h: H.Buss 05.02.2009 |
- Algorithmen beschleunigt -> Floats durch Fixkomma ersetzt |
- Achsentkopplung weiter verbessert |
- Nick- und Roll im Octo-Mischer auf jeweils vier Motoren aufgeteilt |
0.72i: H.Buss 07.02.2009 |
- Abtastrate von 1kHz auf 2kHz erhöht |
0.72j: H.Buss 09.02.2009 |
- neue Implementierung der Servoausgänge |
0.72k: H.Buss 10.02.2009 |
- Abtastrate auf 5kHz erhöht |
0.72L: H.Buss 13.02.2009 |
- Signalfilterung überarbeitet |
- OCTO2 implementiert |
0.72M: H.Buss 13.02.2009 |
- Code Cleanup |
0.72o: H.Buss 24.02.2009 |
- Abtastrate auf 2kHz |
- HW-Version an Navi |
- neuer Datensatz 'c' -> Lagedaten für 3D-Grafik |
- Auswerteroutine für Spectrum-Satteliten implementiert |
- Kanalsettings werden beim Parameterreset nicht mehr gelöscht |
- die Driftkompensation wird jetzt feiner aufgelöst --> EE_Parameter.Driftkomp muss mal 8 genommen werden |
- die Integrale und ACC-Werte werden jetzt im Scope in ca. 0,1° angezeigt (wie beim NaviBrd) |
0.72p: H.Buss 01.03.2009 |
- Octo3 erstellt |
- Analogwerte umbenannt |
0.73a-d: H.Buss 05.04.2009 |
- MixerTabelle implementiert |
- I2C-Bus auf bis zu 12 Motoren erweitert |
- die Busfehler der BL-Regler werden im Menü angezeigt |
- Revision der MixerTabelle eingeführt |
- MixerTabelle wird bei Parameterreset neu initialisiert |
- Motortest auf [12] erweitert |
- Motorschalter nicht mehr 3-Stufig |
0.74a |
- Datenfusion im Flug auch, wenn ACC-Z < 512 |
- Wert für die Luftdruck-Messbereichserweiterung abgefangen |
0.74d |
- Die Driftkompensation ist jetzt dreistufig -> 0,5% pro sekunde zusätzlich eingeführt |
0.75a G.Stobrawa 22.5.2009 |
- Extern Control also received from NC via SPI |
0.75b H.Buss 27.05.2009 |
- Spektrum-Singale schalten den PPM-Eingang aus |
- max. 2 Sekunden nach dem Start auf die BL-Regler warten |
- Automatische Zellenerkennung, wenn Spannungswarnung < 5,0V |
- Bei automatischer Zellenerkennung piept es je nach Zellenzahl |
- EE_DATENREVISION auf 76 erhöht |
- Servo: |
- Roll-Servo für FC ME implementiert |
- Update-Cmd stoppt Servos |
- Servos werden erst nach dem ersten Kalibrieren aktiviert |