/branches/thjac/V1_11/Dokumentation/Anleitung_V1_11a.doc |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/branches/thjac/V1_11/Hex-Files/BootLoader_MEGA644_20MHZ_V0_1.hex |
---|
0,0 → 1,64 |
:10F8000011241FBECFEFD0E1DEBFCDBF11E0A0E0DD |
:10F81000B1E0E0EEFBEF02C005900D92A030B10721 |
:10F82000D9F712E0A0E0B1E001C01D92A230B1070B |
:10F83000E1F70C943B7C0C941D7C0895982F8091EB |
:10F84000C00085FFFCCF9093C60008958091C00052 |
:10F850008823E4F78091C600992708958DE40E94DB |
:10F860001E7C8BE40E941E7C82E40E941E7C8CE441 |
:10F870000E941E7C0895CFEFD0E1DEBFCDBFE0E057 |
:10F88000F0E014915F01772474BE98E10FB6F8940C |
:10F8900090936000109260000FBE7092C5008AE2E3 |
:10F8A0008093C4008091C00082608093C0009093D8 |
:10F8B000C10086E08093C200EF01072D8091C00057 |
:10F8C00087FF0DC0013031F48091C6008A3AB1F152 |
:10F8D00000E005C08091C6008B3109F401E0CE0143 |
:10F8E00021968436910528F11F3F41F0112331F014 |
:10F8F000E0910001F091010109951BC01092C50033 |
:10F900008AE28093C4008AE00E941E7C8DE00E94FF |
:10F910001E7C86E50E941E7C80E30E941E7C8EE297 |
:10F920000E941E7C81E30E941E7C8AE30E941E7C52 |
:10F9300005C080E593EC0197F1F7C0CF0E942E7CC3 |
:10F940000E94267C813611F489E547C1813471F427 |
:10F950000E94267CA82EBB24BA2CAA240E94267CB6 |
:10F960009927A82AB92AB694A79406C1823629F401 |
:10F9700089E50E941E7C81E00BC1823409F083C0BE |
:10F980000E94267C9927D82FCC270E94267C992775 |
:10F99000C82BD92B0E94267C082F12E0812E11E063 |
:10F9A000912EEE24FF2464010894811C911CEC1616 |
:10F9B000FD0618F40E94267C01C08FEFD6018C93BF |
:10F9C0000894E11CF11CBFEFEB16F10461F358F34E |
:10F9D000E4E77E1609F0DEC0063409F03DC0FFEF13 |
:10F9E000AF16F7EFBF0608F033C0DE01A5016627AA |
:10F9F0007727440F551F661F771F6A017B01C2E0FE |
:10FA0000D1E001E08991992729913327322F2227CC |
:10FA1000822B932B0C01FA0100935700E8951124D7 |
:10FA20004E5F5F4F6F4F7F4F129761F785E0F60192 |
:10FA300080935700E89507B600FCFDCF81E18093E5 |
:10FA40005700E89576956795579547955A0194C064 |
:10FA500080E00E941E7C90C0053409F08DC0F50145 |
:10FA60009E01A2E0B1E0E1BD8F2F992782BD8D916B |
:10FA700080BD3196FA9AF99AF999FECF215030401B |
:10FA800091F75F0179C0873609F047C00E94267C54 |
:10FA90009927D82ECC240E94267C9927C82AD92AB7 |
:10FAA0000E94267C863411F5E6016501EE24FF24D0 |
:10FAB000CC0CDD1CEE1CFF1CF60105911491802F6F |
:10FAC0000E941E7C812F99270E941E7C82E090E07C |
:10FAD000A0E0B0E0C80ED91EEA1EFB1E229761F717 |
:10FAE000F694E794D794C79456012ACF853409F049 |
:10FAF00027CF7501E1BC8F2D992782BDF89A089414 |
:10FB0000E11CF11C80B50E941E7C0894C108D1083C |
:10FB1000C114D10479F7570113CF853601F594E765 |
:10FB2000791651F580E090E0A0E0B0E023E0FC0120 |
:10FB300020935700E89507B600FCFDCF80509F4FFB |
:10FB4000AF4FBF4F8F3FE7EF9E07E0E0AE07E0E02B |
:10FB5000BE0768F381E180935700E8950DC08534B6 |
:10FB600069F488E190E02CE00FB6F894A8958093B2 |
:10FB700060000FBE209360008DE02FC08035E1F360 |
:10FB80008C34D1F3803711F483E527C0843721F416 |
:10FB90000E941E7C80E021C0843521F40E94267CD6 |
:10FBA000782EEACF8B3109F4C9CE8A3A09F4C6CE51 |
:10FBB000863529F480E30E941E7C81E30EC08337E2 |
:10FBC00041F489E00E941E7C86E90E941E7C8EE141 |
:10FBD00004C08B3109F4B4CE8FE30E941E7CB0CEFA |
:040000030000F80001 |
:00000001FF |
/branches/thjac/V1_11/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/thjac/V1_11/Settings.h |
---|
--- V1_11/_Settings.h (nonexistent) |
+++ V1_11/_Settings.h (revision 1134) |
@@ -0,0 +1,44 @@ |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Abstimmung |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define ACC_AMPLIFY 12 |
+#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/thjac/V1_11/altcon.c |
---|
0,0 → 1,133 |
/* altcon.c |
* |
* Copyright 2009 Thomas Jachmann |
*/ |
#include "main.h" |
#include "altcon.h" |
#include "parameter.h" |
#include "fc.h" |
char enabled = 0; |
int pressureOffset = 0; |
int accZOffset = 0; |
int lastError = 0; |
int lastN = 0; // Zuletzt errechneter Fehlerwert |
int averageN = 0; |
long altIntegral = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
extern unsigned char Notlandung; // aus fc.c |
extern int airPressure; |
/* |
* Höhenregler initialisieren |
*/ |
void altcon_init( void ) { |
altcon_stop(); |
} |
/* |
* Speichert die aktuelle Höhe als Sollhöhe |
*/ |
void altcon_lock( void ) { |
pressureOffset = airPressure; |
} |
/* |
* Startet den Höhenregler |
*/ |
void altcon_start( void ) { |
accZOffset = Mess_Integral_Hoch / 128; |
lastError = 0; |
lastN = 0; |
averageN = 0; |
altIntegral = 0L; |
// Einschalten der Höhenregelung signalisieren |
beeptime = 500; |
} |
/* |
* Stoppt den Höhenregler |
*/ |
void altcon_stop( void ) { |
enabled = 0; |
// Ausschalten der Höhenregelung signalisieren |
beeptime = 500; |
} |
/* |
* Berechnet den Korrekturwert für die Höhenregelung |
*/ |
int altcon_error( void ) { |
int register n = 0; |
int register error; |
if( enabled && !Notlandung ) { |
// Fehlerwert für Regler ermitteln |
error = airPressure - pressureOffset; |
// Proportionalanteil |
n = ( PARAM_ALT_P * error ) / 4; // dividiert durch ( 16 / STICK_GAIN ) = 16 / 4 = 4 |
// Integralanteil |
altIntegral += ( PARAM_ALT_I * error ) / 4; |
// Integral begrenzen |
if( altIntegral > PARAM_ALT_INT_MAX ) |
altIntegral = PARAM_ALT_INT_MAX; |
else if( altIntegral < -PARAM_ALT_INT_MAX ) |
altIntegral = -PARAM_ALT_INT_MAX; |
n += altIntegral / 4000; |
// Differenzialanteil |
n += ( PARAM_ALT_D * ( error - lastError ) ) / 2; |
// ACC-Z-Integral zur Dämpfung einbeziehen |
temp = ( ( ( Mess_Integral_Hoch / 128 ) - accZOffset ) * (signed long) PARAM_ALT_ACC ) / 32; |
// Dämpfung limitieren |
if( temp > ( 70 * STICK_GAIN ) ) |
temp = 70 * STICK_GAIN; |
else if( temp < -( 70 * STICK_GAIN ) ) |
temp = -( 70 * STICK_GAIN ); |
n += temp; |
// Verstärkung des Fehlerwertes zur Anpassung des Gewichtes |
n = n * PARAM_ALT_GAIN / 10; |
int altMax = PARAM_ALT_MAX * STICK_GAIN; |
// Limitierung des Korrekturwertes |
if( n > altMax ) |
n = altMax; |
else if( n < -altMax ) |
n = -altMax; |
lastN = n; |
lastError = error; |
/* Berechnung einer exponentiellen Glättung für den neuen Gaswert bei Verlassen der |
* Höhenregelung. Dies soll ein zu heftiges Reagieren mindern. */ |
averageN = averageN + PARAM_ALT_EXP_SMOOTHING_FACTOR * ( n - averageN ) / 100; |
} |
DebugOut.Analog[30] = altIntegral / 4000; |
DebugOut.Analog[27] = n; |
return n; |
} |
/branches/thjac/V1_11/altcon.d |
---|
0,0 → 1,3 |
altcon.o altcon.d : altcon.c main.h old_macros.h _Settings.h printf_P.h timer0.h \ |
uart.h analog.h twimaster.h menu.h rc.h fc.h gps.h spi.h led.h altcon.h \ |
parameter.h |
/branches/thjac/V1_11/altcon.h |
---|
0,0 → 1,19 |
/* altcon.h |
* |
* Copyright 2009 Thomas Jachmann |
*/ |
#ifndef ALTCON_H |
#define ALTCON_H |
#define altcon_avgerror() averageN |
extern int pressureOffset; |
extern int averageN; |
extern void altcon_start( void ); |
extern void altcon_lock( void ); |
extern void altcon_stop( void ); |
extern int altcon_error( void ); |
#endif // ALTCON_H |
/branches/thjac/V1_11/analog.c |
---|
0,0 → 1,191 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
volatile int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0; |
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, 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; |
int airPressure = 0; |
//####################################################################################### |
// |
void ADC_Init(void) |
//####################################################################################### |
{ |
ADMUX = 0;//Referenz ist extern |
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE); |
//Free Running Mode, Division Factor 128, Interrupt on |
} |
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; |
GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0; |
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); |
messanzahl_Druck = 0; |
ANALOG_ON; |
while(messanzahl_Druck == 0); |
if(i<10) Delay_ms_Mess(10); |
} |
Delay_ms_Mess(70); |
} |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
static unsigned int gier1, roll1, nick1; |
ANALOG_OFF; |
switch(state++) |
{ |
case 0: |
gier1 = ADC; |
kanal = 1; |
ZaehlMessungen++; |
break; |
case 1: |
roll1 = ADC; |
kanal = 2; |
break; |
case 2: |
nick1 = ADC; |
kanal = 4; |
break; |
case 3: |
UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19) / 2; |
kanal = 6; |
break; |
case 4: |
Aktuell_ay = NeutralAccY - ADC; |
AdWertAccRoll = Aktuell_ay; |
kanal = 7; |
break; |
case 5: |
Aktuell_ax = ADC - NeutralAccX; |
AdWertAccNick = Aktuell_ax; |
kanal = 0; |
break; |
case 6: |
if(PlatinenVersion == 10) AdWertGier = (ADC + gier1) / 2; |
else |
if(PlatinenVersion == 20) AdWertGier = 1023 - (ADC + gier1); |
else AdWertGier = ADC + gier1; |
kanal = 1; |
break; |
case 7: |
if(PlatinenVersion == 10) AdWertRoll = (ADC + roll1) / 2; |
else AdWertRoll = ADC + roll1; |
kanal = 2; |
break; |
case 8: |
if(PlatinenVersion == 10) AdWertNick = (ADC + nick1) / 2; |
else AdWertNick = ADC + nick1; |
//AdWertNick = 0; |
//AdWertNick += Poti2; |
kanal = 5; |
break; |
case 9: |
AdWertAccHoch = (signed int) ADC - NeutralAccZ; |
// AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4; |
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 = 3; |
break; |
case 10: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) { |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
airPressure = StartLuftdruck - Luftdruck; |
tmpLuftdruck = 0; |
DebugOut.Analog[5] = airPressure; |
} |
kanal = 0; |
state = 0; |
break; |
default: |
kanal = 0; |
state = 0; |
break; |
} |
ADMUX = kanal; |
if(state != 0) ANALOG_ON; |
} |
/branches/thjac/V1_11/analog.h |
---|
0,0 → 1,29 |
/*####################################################################################### |
#######################################################################################*/ |
extern volatile int UBat; |
extern volatile int AdWertNick, AdWertRoll, AdWertGier; |
extern volatile int AdWertAccRoll,AdWertAccNick,AdWertAccHoch; |
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 int airPressure; |
unsigned int ReadADC(unsigned char adc_input); |
void ADC_Init(void); |
void SucheLuftruckOffset(void); |
void SucheGyroOffset(void); |
#define ANALOG_OFF ADCSRA=0 |
#define ANALOG_ON ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE) |
/branches/thjac/V1_11/eeprom.c |
---|
0,0 → 1,218 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 = 80; // 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 = 10; // Wert : 0-50 |
EE_Parameter.Stick_P = 15; // Wert : 1-6 |
EE_Parameter.Stick_D = 30; // 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.UnterspannungsWarnung = 94; // Wert : 0-250 |
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.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
EE_Parameter.AchsKopplung1 = 90; |
EE_Parameter.AchsGegenKopplung1 = 5; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 16; // 1/k |
EE_Parameter.Driftkomp = 4; |
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.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 = 60; |
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 = 80; // 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 = 10; // Wert : 0-50 |
EE_Parameter.Stick_P = 12; // 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 = 80; // Wert : 0-250 |
EE_Parameter.Gyro_I = 120; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
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.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
EE_Parameter.AchsKopplung1 = 90; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt |
EE_Parameter.AchsGegenKopplung1 = 5; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 32; // 1/k |
EE_Parameter.Driftkomp = 4; |
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.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 = 60; |
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 = 80; // 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 = 10; // 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.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
EE_Parameter.I_Faktor = 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.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
EE_Parameter.ServoNickRefresh = 5; |
EE_Parameter.LoopGasLimit = 50; |
EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
EE_Parameter.LoopHysterese = 50; |
EE_Parameter.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
EE_Parameter.AchsKopplung1 = 90; // Faktor, mit dem Gier die Achsen Roll und Nick verkoppelt |
EE_Parameter.AchsGegenKopplung1 = 5; |
EE_Parameter.WinkelUmschlagNick = 85; |
EE_Parameter.WinkelUmschlagRoll = 85; |
EE_Parameter.GyroAccAbgleich = 32; // 1/k |
EE_Parameter.Driftkomp = 4; |
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.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 = 60; |
memcpy(EE_Parameter.Name, "Beginner\0", 12); |
} |
/branches/thjac/V1_11/fc.c |
---|
0,0 → 1,1277 |
/*####################################################################################### |
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 "parameter.h" |
#include "pitch.h" |
#include "altcon.h" |
#include "eeprom.c" |
unsigned char h, m, s; |
volatile unsigned int I2CTimeout = 100; |
volatile int MesswertNick, MesswertRoll, MesswertGier, MesswertGierBias; |
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; |
volatile int KompassValue = 0; |
volatile int KompassStartwert = 0; |
volatile 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; |
float GyroFaktor; |
float IntegralFaktor; |
volatile int DiffNick, DiffRoll; |
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
volatile unsigned char Motor_Vorne, Motor_Hinten, Motor_Rechts, Motor_Links, Count; |
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; |
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_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_LoopGasLimit = 70; |
unsigned char Parameter_AchsKopplung1 = 0; |
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_J16Brightness; // for the J16 Output |
unsigned char Parameter_J17Bitmask; // for the J17 Output |
unsigned char Parameter_J17Timing; // for the J17 Output |
unsigned char Parameter_J17Brightness; // 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; |
unsigned char MikroKopterFlags = 0; |
void Piep(unsigned char Anzahl) { |
while (Anzahl--) { |
if (MotorenEin) return; //auf keinen Fall im Flug! |
beeptime = 100; |
Delay_ms(250); |
} |
} |
//############################################################################ |
// Nullwerte ermitteln |
void SetNeutral(void) |
//############################################################################ |
{ |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
AdNeutralNick = 0; |
AdNeutralRoll = 0; |
AdNeutralGier = 0; |
AdNeutralGierBias = 0; |
Parameter_AchsKopplung1 = 0; |
Parameter_AchsGegenKopplung1 = 0; |
ExpandBaro = 0; |
CalibrierMittelwert(); |
Delay_ms_Mess(100); |
CalibrierMittelwert(); |
if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
AdNeutralNick = AdWertNick; |
AdNeutralRoll = AdWertRoll; |
AdNeutralGier = AdWertGier; |
AdNeutralGierBias = AdWertGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) { |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / 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]); |
} |
Mess_IntegralNick = 0; |
Mess_IntegralNick2 = 0; |
Mess_IntegralRoll = 0; |
Mess_IntegralRoll2 = 0; |
Mess_Integral_Gier = 0; |
MesswertNick = 0; |
MesswertRoll = 0; |
MesswertGier = 0; |
Delay_ms_Mess(100); |
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 = EE_Parameter.Driftkomp * 16; |
FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
} |
void LesePotis(void) { |
/* Warum 110? Knüppel geht von -125 bis 125! |
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 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125) Poti1++; |
else if (Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125 && Poti1) Poti1--; |
if (Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125) Poti2++; |
else if (Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125 && Poti2) Poti2--; |
if (Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125) Poti3++; |
else if (Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125 && Poti3) Poti3--; |
if (Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125) Poti4++; |
else if (Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125 && 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; |
} |
//############################################################################ |
// Bearbeitet die Messwerte |
void Mittelwert(void) |
//############################################################################ |
{ |
static signed long tmpl, tmpl2; |
MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
//DebugOut.Analog[26] = MesswertNick; |
// DebugOut.Analog[28] = MesswertRoll; |
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long) Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long) AdWertAccNick))) / 2L; |
Mittelwert_AccRoll = ((long) Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long) AdWertAccRoll))) / 2L; |
Mittelwert_AccHoch = ((long) Mittelwert_AccHoch * 1 + ((long) AdWertAccHoch)) / 2L; |
IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
NaviAccNick += AdWertAccNick; |
NaviAccRoll += AdWertAccRoll; |
NaviCntAcc++; |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
ErsatzKompass += MesswertGier; |
Mess_Integral_Gier += MesswertGier; |
// Mess_Integral_Gier2 += MesswertGier; |
if (ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
if (ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
if (!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) { |
tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L; |
tmpl *= Parameter_AchsKopplung1; //125 |
tmpl /= 4096L; |
tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L; |
tmpl2 *= Parameter_AchsKopplung1; |
tmpl2 /= 4096L; |
if (labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
} else tmpl = tmpl2 = 0; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
MesswertRoll += tmpl; |
MesswertRoll += (tmpl2 * Parameter_AchsGegenKopplung1) / 512L; //109 |
Mess_IntegralRoll2 += MesswertRoll; |
Mess_IntegralRoll += MesswertRoll - 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; |
} |
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 > 2020) MesswertRoll = +1000; |
if (AdWertRoll > 2034) MesswertRoll = +2000; |
} |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
MesswertNick -= tmpl2; |
MesswertNick -= (tmpl * Parameter_AchsGegenKopplung1) / 512L; |
Mess_IntegralNick2 += MesswertNick; |
Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
if (Mess_IntegralNick > Umschlag180Nick) { |
Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
if (Mess_IntegralNick <-Umschlag180Nick) { |
Mess_IntegralNick = (Umschlag180Nick - 25000L); |
Mess_IntegralNick2 = Mess_IntegralNick; |
} |
if (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 > 2020) MesswertNick = +1000; |
if (AdWertNick > 2034) MesswertNick = +2000; |
} |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
// ADC einschalten |
ANALOG_ON; |
//++++++++++++++++++++++++++++++++++++++++++++++++ |
Integral_Gier = Mess_Integral_Gier; |
IntegralNick = Mess_IntegralNick; |
IntegralRoll = Mess_IntegralRoll; |
IntegralNick2 = Mess_IntegralNick2; |
IntegralRoll2 = Mess_IntegralRoll2; |
if (EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) { |
if (MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
else if (MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
if (MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
else if (MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
} |
LesePotis(); |
} |
//############################################################################ |
// 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; |
LesePotis(); |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
} |
//############################################################################ |
// Senden der Motorwerte per I2C-Bus |
void SendMotorData(void) |
//############################################################################ |
{ |
DebugOut.Analog[12] = Motor_Vorne; |
DebugOut.Analog[13] = Motor_Hinten; |
DebugOut.Analog[14] = Motor_Links; |
DebugOut.Analog[15] = Motor_Rechts; |
if (!(MotorenEin && PARAM_ENGINE_ENABLED)) { |
Motor_Hinten = 0; |
Motor_Vorne = 0; |
Motor_Rechts = 0; |
Motor_Links = 0; |
if (MotorTest[0]) Motor_Vorne = MotorTest[0]; |
if (MotorTest[1]) Motor_Hinten = MotorTest[1]; |
if (MotorTest[2]) Motor_Links = MotorTest[2]; |
if (MotorTest[3]) Motor_Rechts = MotorTest[3]; |
MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
} else MikroKopterFlags |= FLAG_MOTOR_RUN; |
//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_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_LoopGasLimit, EE_Parameter.LoopGasLimit, 0, 255); |
CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1, 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_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250); |
CHK_POTI_MM(Parameter_J17Timing, EE_Parameter.J17Timing, 1, 255); |
CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250); |
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255); |
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255); |
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255); |
CHK_POTI(Parameter_ExternalControl, EE_Parameter.ExternalControl, 0, 255); |
Ki = (float) Parameter_I_Faktor * 0.0001; |
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
} |
//############################################################################ |
// |
void MotorRegler(void) |
//############################################################################ |
{ |
int motorwert, pd_ergebnis, 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 char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
static long ausgleichNick, ausgleichRoll; |
Mittelwert(); |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = StickGas; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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(); |
Piep(GetActiveParamSetNumber()); |
} |
} |
} 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()); |
} |
} else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35 - 125) { |
// Starten |
if (!MotorenEin && 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 = 0; |
Mess_IntegralRoll = 0; |
Mess_IntegralNick2 = IntegralNick; |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
MikroKopterFlags |= FLAG_START; |
// Beim Einschalten automatisch kalibrieren |
if (PARAM_CAL_ON_START) { |
if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) { |
if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) { |
SucheLuftruckOffset(); |
} |
} |
SetNeutral(); |
} |
} |
} 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 chanNickPrev = 0; |
static int chanRollPrev = 0; |
static int stick_nick, stick_roll; |
ParameterZuordnung(); |
#define MAX_CHAN_VAL 125L |
#define COS45 7071L // cos( -45 ) * 10000 |
long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
int chanNickDiff; |
int chanRollDiff; |
/* Über Parameter läßt sich zwischen "+" und "X" - Formations |
* umschalten (sh. parameter.h) |
*/ |
if (PARAM_X_FORMATION) { |
chanRoll = -chanRoll; |
// Stick-Koordinatensystem um -45° (rechts) drehen |
chanNick *= COS45; |
chanRoll *= COS45; |
int chanNickTemp = (chanNick - chanRoll) / 10000L; |
int chanRollTemp = (chanRoll + chanNick) / 10000L; |
chanNick = chanNickTemp; |
chanRoll = -chanRollTemp; |
if (chanNick > MAX_CHAN_VAL) |
chanNick = MAX_CHAN_VAL; |
if (chanNick < -MAX_CHAN_VAL) |
chanNick = -MAX_CHAN_VAL; |
if (chanRoll > MAX_CHAN_VAL) |
chanRoll = MAX_CHAN_VAL; |
if (chanRoll < -MAX_CHAN_VAL) |
chanRoll = -MAX_CHAN_VAL; |
} |
chanNickDiff = ((chanNick - chanNickPrev) / 3) * 3; |
chanRollDiff = ((chanRoll - chanRollPrev) / 3) * 3; |
chanNickPrev = chanNick; |
chanRollPrev = chanRoll; |
stick_nick = (stick_nick * 3 + ((int) chanNick) * EE_Parameter.Stick_P) / 4; |
stick_nick += chanNickDiff * EE_Parameter.Stick_D; |
StickNick = stick_nick - GPS_Nick; |
stick_roll = (stick_roll * 3 + ((int) chanRoll) * EE_Parameter.Stick_P) / 4; |
stick_roll += chanRollDiff * EE_Parameter.Stick_D; |
StickRoll = stick_roll - GPS_Roll; |
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
// Gaswert übernehmen |
StickGas = pitch_value(); |
GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN); |
IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN); |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ 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; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (Notlandung) { |
StickGier = 0; |
StickNick = 0; |
StickRoll = 0; |
GyroFaktor = (float) 100 / (256.0 / STICK_GAIN); |
IntegralFaktor = (float) 120 / (44000 / STICK_GAIN); |
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) { |
long tmp_long, tmp_long2; |
if (FromNaviCtrl_Value.Kalman_K != -1) { |
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; |
} |
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) { |
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 (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 * 4) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) |
#define BEWEGUNGS_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; // + labs(IntegralFehlerNick) / 4096; |
if (labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) { |
if (IntegralFehlerNick > FEHLER_LIMIT2) { |
if (last_n_p) { |
cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; |
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; |
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 (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16; |
if (IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if (IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; // + labs(IntegralFehlerNick) / 4096; |
ausgleichRoll = 0; |
if (labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) { |
if (IntegralFehlerRoll > FEHLER_LIMIT2) { |
if (last_r_p) { |
cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; |
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; |
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 (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16; |
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; |
} |
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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[29] = (MaxStickNick + MaxStickRoll); |
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 (NeueKompassRichtungMerken) { |
fehler = 0; |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
} |
if (!KompassSignalSchlecht && w < 25) { |
GierGyroFehler += fehler; |
if (NeueKompassRichtungMerken) { |
beeptime = 200; |
// KompassStartwert = KompassValue; |
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; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[2] = Mittelwert_AccNick; |
DebugOut.Analog[3] = Mittelwert_AccRoll; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[10] = SenderOkay; |
//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[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
// 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 (Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
if (Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
DebugOut.Analog[21] = MesswertNick; |
DebugOut.Analog[22] = MesswertRoll; |
// Maximalwerte abfangen |
#define MAX_SENSOR (4096*STICK_GAIN) |
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; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas-Mischanteil |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Zur besseren Auflösung hochskalieren |
GasMischanteil *= STICK_GAIN; |
// Fehlerwert der Höhenregelung einmischen |
GasMischanteil -= altcon_error(); |
// Mindestens auf Minimalgas stellen |
if( GasMischanteil < MIN_GAS ) |
GasMischanteil = MIN_GAS; |
// Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren) |
if( GasMischanteil > ( MAX_GAS - 20 ) * STICK_GAIN ) |
GasMischanteil = ( MAX_GAS - 20 ) * STICK_GAIN; |
// Mindestens auf Minimalgas stellen |
if (GasMischanteil < MIN_GAS) |
GasMischanteil = MIN_GAS; |
// Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren) |
if (GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) |
GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
DebugOut.Analog[7] = GasMischanteil; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gier-Anteil |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GierMischanteil = MesswertGier - (sollGier * STICK_GAIN); // Regler für Gier |
#define MIN_GIERGAS ( 40 * STICK_GAIN ) // unter diesem Gaswert trotzdem Gieren |
// Reduzierten Gieranteil berechnen |
if (GasMischanteil < MIN_GIERGAS) { |
GierMischanteil = (GierMischanteil * GasMischanteil) / MIN_GIERGAS; |
} |
// Gieranteil darf nicht größer als der halbe Gasanteil sein |
if (GierMischanteil > (GasMischanteil >> 1)) |
GierMischanteil = GasMischanteil >> 1; |
if (GierMischanteil < -(GasMischanteil >> 1)) |
GierMischanteil = -(GasMischanteil >> 1); |
tmp_int = MAX_GAS * STICK_GAIN; |
// Gieranteil darf die Gasreserve nicht überschreiten |
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 += IntegralNick * IntegralFaktor - 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 = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
// Motor Vorn |
tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64; |
if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if (motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Vorne = motorwert; |
// Motor Heck |
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if (motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Hinten = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
if (IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - 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 = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64; |
if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
// Motor Links |
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if (motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Links = motorwert; |
// Motor Rechts |
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
motorwert /= STICK_GAIN; |
if ((motorwert < 0)) motorwert = 0; |
else if (motorwert > MAX_GAS) motorwert = MAX_GAS; |
if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
Motor_Rechts = motorwert; |
// +++++++++++++++++++++++++++++++++++++++++++++++ |
} |
/branches/thjac/V1_11/fc.h |
---|
0,0 → 1,174 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
#ifndef _FC_H |
#define _FC_H |
//#define GIER_GRAD_FAKTOR 1450L // Abhängigkeit zwischen GyroIntegral und Winkel |
//#define GIER_GRAD_FAKTOR 1550L // Abhängigkeit zwischen GyroIntegral und Winkel |
#define GIER_GRAD_FAKTOR 1291L // 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 |
extern unsigned char MikroKopterFlags; |
extern volatile unsigned int I2CTimeout; |
extern unsigned char Sekunde,Minute; |
extern long IntegralNick,IntegralNick2; |
extern long 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 volatile int KompassValue; |
extern volatile int KompassStartwert; |
extern volatile int KompassRichtung; |
extern long ErsatzKompass; |
extern int ErsatzKompassInGrad; // Kompasswert in Grad |
extern int HoehenWert; |
extern int SollHoehe; |
extern volatile int MesswertNick,MesswertRoll,MesswertGier; |
extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier, Mittelwert_AccNick, Mittelwert_AccRoll; |
extern int NeutralAccX, NeutralAccY,Mittelwert_AccHoch; |
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); |
extern void DefaultKonstanten(void); |
void DefaultStickMapping(void); |
void DefaultKonstanten1(void); |
void DefaultKonstanten2(void); |
void DefaultKonstanten3(void); |
extern unsigned char h,m,s; |
extern volatile unsigned char Timeout ; |
extern unsigned char CosinusNickWinkel, CosinusRollWinkel; |
extern volatile int DiffNick,DiffRoll; |
extern int Poti1, Poti2, Poti3, Poti4; |
extern volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
extern volatile unsigned char SenderOkay; |
extern int StickNick,StickRoll,StickGier; |
extern char MotorenEin; |
extern void DefaultKonstanten1(void); |
extern void DefaultKonstanten2(void); |
#define STRUCT_PARAM_LAENGE 86 |
struct mk_param_struct |
{ |
unsigned char Kanalbelegung[8]; // GAS[0], GIER[1],NICK[2], ROLL[3], POTI1, POTI2, POTI3 |
unsigned char GlobalConfig; // 0x01=Höhenregler aktiv,0x02=Kompass aktiv, 0x04=GPS aktiv, 0x08=Heading Hold aktiv |
unsigned char Hoehe_MinGas; // Wert : 0-100 |
unsigned char Luftdruck_D; // Wert : 0-250 |
unsigned char MaxHoehe; // Wert : 0-32 |
unsigned char Hoehe_P; // Wert : 0-32 |
unsigned char Hoehe_Verstaerkung; // Wert : 0-50 |
unsigned char Hoehe_ACC_Wirkung; // Wert : 0-250 |
unsigned char Stick_P; // Wert : 1-6 |
unsigned char Stick_D; // Wert : 0-64 |
unsigned char Gier_P; // Wert : 1-20 |
unsigned char Gas_Min; // Wert : 0-32 |
unsigned char Gas_Max; // Wert : 33-250 |
unsigned char GyroAccFaktor; // Wert : 1-64 |
unsigned char KompassWirkung; // Wert : 0-32 |
unsigned char Gyro_P; // Wert : 10-250 |
unsigned char Gyro_I; // Wert : 0-250 |
unsigned char UnterspannungsWarnung; // Wert : 0-250 |
unsigned char NotGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
unsigned char NotGasZeit; // Wert : 0-250 // Zeitbis auf NotGas geschaltet wird, wg. Rx-Problemen |
unsigned char UfoAusrichtung; // X oder + Formation |
unsigned char I_Faktor; // Wert : 0-250 |
unsigned char UserParam1; // Wert : 0-250 |
unsigned char UserParam2; // Wert : 0-250 |
unsigned char UserParam3; // Wert : 0-250 |
unsigned char UserParam4; // Wert : 0-250 |
unsigned char ServoNickControl; // Wert : 0-250 // Stellung des Servos |
unsigned char ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
unsigned char ServoNickMin; // Wert : 0-250 // Anschlag |
unsigned char ServoNickMax; // Wert : 0-250 // Anschlag |
unsigned char ServoNickRefresh; // |
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
unsigned char LoopHysterese; // Wert: 0-250 Hysterese für Stickausschlag |
unsigned char AchsKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
unsigned char AchsGegenKopplung1; // Wert: 0-250 Faktor, mit dem Gier die Achsen Roll und Nick Gegenkoppelt (NickRollGegenkopplung) |
unsigned char WinkelUmschlagNick; // Wert: 0-250 180°-Punkt |
unsigned char WinkelUmschlagRoll; // Wert: 0-250 180°-Punkt |
unsigned char GyroAccAbgleich; // 1/k (Koppel_ACC_Wirkung) |
unsigned char Driftkomp; |
unsigned char 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 NaviGpsACC; |
unsigned char NaviGpsMinSat; |
unsigned char NaviStickThreshold; |
unsigned char NaviWindCorrection; |
unsigned char NaviSpeedCompensation; |
unsigned char NaviOperatingRadius; |
unsigned char NaviAngleLimitation; |
//---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 ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
}; |
extern struct mk_param_struct EE_Parameter; |
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_AchsKopplung1; |
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_J16Brightness; // for the J16 Output |
extern unsigned char Parameter_J17Bitmask; // for the J17 Output |
extern unsigned char Parameter_J17Timing; // for the J17 Output |
extern unsigned char Parameter_J17Brightness; // for the J17 Output |
/* |
extern unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard |
extern unsigned char Parameter_NaviGpsGain; |
extern unsigned char Parameter_NaviGpsP; |
extern unsigned char Parameter_NaviGpsI; |
extern unsigned char Parameter_NaviGpsD; |
extern unsigned char Parameter_NaviGpsACC; |
extern unsigned char Parameter_NaviOperatingRadius; |
extern unsigned char Parameter_NaviWindCorrection; |
extern unsigned char Parameter_NaviSpeedCompensation; |
*/ |
#endif //_FC_H |
/branches/thjac/V1_11/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/thjac/V1_11/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/thjac/V1_11/led.c |
---|
0,0 → 1,145 |
#include <inttypes.h> |
#include "main.h" |
#include "parameter.h" |
unsigned char J16Blinkcount = 0, J16Mask = 1, J16Brightness = 0, J16 = 0; |
unsigned char J17Blinkcount = 0, J17Mask = 1, J17Brightness = 0, J17 = 0; |
unsigned char pwmJ16 = 0, pwmJ17 = 0; |
static unsigned char pwmtable[32] ={0, 1, 2, 2, 2, 3, 3, 4, 5, 6, 7, 8, 10, 11, |
13, 16, 19, 23, 27, 32, 38, 45, 54, 64, 76, |
91, 108, 128, 152, 181, 215, 250}; |
unsigned char lightsEnabled = 0, lightsOn = 0; |
extern char MotorenEin; |
void setJ16(char enabled) { |
/* if( PARAM_LED_NEGATE ) |
enabled = !enabled; |
if( enabled && forceEnabled ) |
J16_ON; |
else |
J16_OFF;*/ |
if ((enabled && lightsOn) ^ LED_NEGATE_J16) |
J16_ON; |
else |
J16_OFF; |
} |
void setJ17(char enabled) { |
/* if( PARAM_LED_NEGATE ) |
enabled = !enabled; |
if( enabled && forceEnabled ) |
J17_ON; |
else |
J17_OFF;*/ |
if ((enabled && lightsOn) ^ LED_NEGATE_J17) |
J17_ON; |
else |
J17_OFF; |
} |
// 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); |
lightsOn = lightsEnabled = 0; |
setJ16(0); |
setJ17(0); |
J16Blinkcount = 0; |
J16Mask = 128; |
J17Blinkcount = 0; |
J17Mask = 128; |
} |
void checkLightsEnabled(void) { |
// Mit dem Gier-Stick rechts lassen sich bei stehenden Motoren die LED's ein- und mit links ausschalten |
if (!MotorenEin) { |
if (PARAM_LED_STICK_ENABLED) { |
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 125 |
&& PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] < 125 - 35) { |
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] < -75) |
lightsEnabled = 1; |
if (PPM_in[ EE_Parameter.Kanalbelegung[ K_GIER ] ] > 75) |
lightsEnabled = 0; |
} |
} else |
lightsEnabled = 1; |
} |
// Die LED's können mit den Motoren ein- ausgeschaltet werden |
if (PARAM_LED_ENGINE_ENABLED) |
lightsEnabled = MotorenEin; |
lightsOn = lightsEnabled; |
} |
// called in UpdateMotors() every 2ms |
void LED_Update(void) { |
static char delay = 0; |
checkLightsEnabled(); |
if (!delay--) { |
delay = 9; // 20ms Intervall |
// Soll die Unterspannungswarnung zu einem schnelleren Blinken führen? |
// Grenze für Unterspannungswarnung erreicht? |
if (PARAM_LED_WARNING_SPEEDUP && UBat < EE_Parameter.UnterspannungsWarnung) { |
if (PARAM_LED_FORCE_WARNING_ENABLED) // Erzwingt die Aktivierung der Ausgänge |
lightsOn = 1; |
delay /= PARAM_LED_WARNING_SPEEDUP + 1; |
} |
// J16 |
if (EE_Parameter.J16Timing > 250 && Parameter_J16Timing > 230) |
J16 = EE_Parameter.J16Bitmask & 128; |
else if (EE_Parameter.J16Timing > 250 && Parameter_J16Timing < 10) |
J16 = !(EE_Parameter.J16Bitmask & 128); |
else if (!J16Blinkcount--) { |
J16Blinkcount = Parameter_J16Timing - 1; |
J16Mask = (J16Mask == 1 ? 0x80 : J16Mask >> 1); |
J16 = EE_Parameter.J16Bitmask & J16Mask; |
if (EE_Parameter.J16Bitmask & J16Mask) { |
pwmJ16 = pwmtable[(Parameter_J16Brightness + 6) / 4]; |
J16Brightness = Parameter_J16Brightness / 23; |
} else |
pwmJ16 = J16Brightness = 0; |
} |
// J17 |
if (EE_Parameter.J17Timing > 250 && Parameter_J17Timing > 230) |
J17 = EE_Parameter.J17Bitmask & 128; |
else if (EE_Parameter.J17Timing > 250 && Parameter_J17Timing < 10) |
J17 = !(EE_Parameter.J17Bitmask & 128); |
else if (!J17Blinkcount--) { |
J17Blinkcount = Parameter_J17Timing - 1; |
J17Mask = (J17Mask == 1 ? 0x80 : J17Mask >> 1); |
J17 = EE_Parameter.J17Bitmask & J17Mask; |
if (EE_Parameter.J17Bitmask & J17Mask) { |
pwmJ17 = pwmtable[(Parameter_J17Brightness + 6) / 4]; |
J17Brightness = Parameter_J17Brightness / 23; |
} else |
pwmJ17 = J17Brightness = 0; |
} |
} |
// delay: 0...9 - BRIGHTNESS/23: 0-Aus...10-Max - Bei Unterspannung volle Leuchtkraft |
setJ16(J16 && (delay < J16Brightness || UBat < EE_Parameter.UnterspannungsWarnung)); |
setJ17(J17 && (delay < J17Brightness || UBat < EE_Parameter.UnterspannungsWarnung)); |
} |
/branches/thjac/V1_11/led.h |
---|
0,0 → 1,12 |
#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 checkLightsEnabled(void); |
extern void LED_Init(void); |
extern void LED_Update(void); |
/branches/thjac/V1_11/main.c |
---|
0,0 → 1,281 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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; |
// -- 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); |
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); |
} |
DebugOut.Analog[19] = WinkelOut.CalcState; |
} |
//############################################################################ |
//Hauptprogramm |
int main(void) |
//############################################################################ |
{ |
unsigned int timer; |
//unsigned int timer2 = 0; |
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 |
DDRD |= 0x80; // J7 -> Servo signal |
PORTD = 0x77; // LED |
MCUSR &= ~(1 << WDRF); |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
beeptime = 2000; |
StickGier = 0; |
PPM_in[K_GAS] = 0; |
StickRoll = 0; |
StickNick = 0; |
ROT_OFF; |
Timer_Init(); |
UART_Init(); |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
SPI_MasterInit(); |
sei(); |
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ", PlatinenVersion / 10, PlatinenVersion % 10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r=============================="); |
GRN_ON; |
ReadParameterSet(3, (unsigned char *) & EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes |
// valid Stick-Settings? |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) == 255 || eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) < EE_DATENREVISION || |
EE_Parameter.Kanalbelegung[0] > 9 || EE_Parameter.Kanalbelegung[1] > 9 || EE_Parameter.Kanalbelegung[2] > 9 || EE_Parameter.Kanalbelegung[3] > 9) { |
printf("\n\rInit. EEPROM: Generating Default-Parameter and Stick-Settings..."); |
DefaultStickMapping(); |
} else if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) printf("\n\rInit. EEPROM: Generating Default-Parameter using old Stick Settings"); |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) { |
DefaultKonstanten1(); |
for (unsigned char i = 1; i < 6; i++) { |
if (i == 2) DefaultKonstanten2(); // Kamera |
if (i == 3) DefaultKonstanten3(); // Beginner |
if (i > 3) DefaultKonstanten2(); // Kamera |
WriteParameterSet(i, (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
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 nicht abgeglichen!"); |
} |
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
printf("\n\rBenutze Parametersatz %d", GetActiveParamSetNumber()); |
if (EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) { |
printf("\n\rAbgleich Luftdrucksensor.."); |
timer = SetDelay(1000); |
SucheLuftruckOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
SetNeutral(); |
ROT_OFF; |
beeptime = 2000; |
ExternControl.Digital[0] = 0x55; |
printf("\n\rSteuerung: "); |
if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Neutral"); |
printf("\n\n\r"); |
LcdClear(); |
I2CTimeout = 5000; |
WinkelOut.Orientation = 1; |
while (1) { |
if (UpdateMotor) // ReglerIntervall |
{ |
UpdateMotor = 0; |
//PORTD |= 0x08; |
if (WinkelOut.CalcState) CalMk3Mag(); |
else MotorRegler(); |
//PORTD &= ~0x08; |
SendMotorData(); |
ROT_OFF; |
if (PcZugriff) PcZugriff--; |
else { |
ExternControl.Config = 0; |
ExternStickNick = 0; |
ExternStickRoll = 0; |
ExternStickGier = 0; |
} |
if (SenderOkay) SenderOkay--; |
if (!I2CTimeout) { |
I2CTimeout = 5; |
i2c_reset(); |
if ((BeepMuster == 0xffff) && MotorenEin) { |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} else { |
I2CTimeout--; |
ROT_OFF; |
} |
if (SIO_DEBUG && (!UpdateMotor || !MotorenEin)) { |
DatenUebertragung(); |
BearbeiteRxDaten(); |
} else BearbeiteRxDaten(); |
if (CheckDelay(timer)) { |
if (UBat < EE_Parameter.UnterspannungsWarnung) { |
if (BeepMuster == 0xffff) { |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
/* if(SendVersionToNavi) |
{ |
SPI_StartTransmitPacket(SPI_CMD_VERSION);//# |
SendVersionToNavi = 0; |
} |
else SPI_StartTransmitPacket(SPI_CMD_VALUE);//# |
*/ |
SPI_StartTransmitPacket(); //# |
SendSPI = 4; |
timer = SetDelay(20); |
} |
//if(UpdateMotor) DebugOut.Analog[26]++; |
LED_Update(); |
} |
if (!SendSPI) { |
SPI_TransmitByte(); |
} |
} |
return (1); |
} |
/branches/thjac/V1_11/main.h |
---|
0,0 → 1,111 |
#ifndef _MAIN_H |
#define _MAIN_H |
//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 |
//#define SYSCLK 16000000L //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 73 // wird angepasst, wenn sich die EEPROM-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_PARAM_BEGIN 100 |
#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_HIGHT_3SWITCH 0x10 |
//#define SYSCLK |
//extern unsigned long SYSCLK; |
extern volatile int i_Nick[20],i_Roll[20],DiffNick,DiffRoll; |
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" |
#define DIV_4 2 |
#define DIV_128 7 |
#define MUL_4 2 |
#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/thjac/V1_11/makefile |
---|
0,0 → 1,434 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 1 |
VERSION_MINOR = 11 |
VERSION_PATCH = 0 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol |
VERSION_SERIAL_MINOR = 0 # Serial Protocol |
NC_SPI_COMPATIBLE = 3 # 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 |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
OPT = s |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c |
SRC += twimaster.c rc.c fc.c spi.c led.c pitch.c pitch_neutral.c pitch_md.c altcon.c |
SRC += gps.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. |
sizebefore: |
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi |
sizeafter: |
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi |
# Display compiler version information. |
gccversion : |
@$(CC) --version |
# Convert ELF to COFF for use in debugging / simulating in |
# AVR Studio or VMLAB. |
COFFCONVERT=$(OBJCOPY) --debugging \ |
--change-section-address .data-0x800000 \ |
--change-section-address .bss-0x800000 \ |
--change-section-address .noinit-0x800000 \ |
--change-section-address .eeprom-0x810000 |
coff: $(TARGET).elf |
@echo |
@echo $(MSG_COFF) $(TARGET).cof |
$(COFFCONVERT) -O coff-avr $< $(TARGET).cof |
extcoff: $(TARGET).elf |
@echo |
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof |
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof |
# Program the device. |
program: $(TARGET).hex $(TARGET).eep |
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) |
# Create final output files (.hex, .eep) from ELF output file. |
%.hex: %.elf |
@echo |
@echo $(MSG_FLASH) $@ |
$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ |
%.eep: %.elf |
@echo |
@echo $(MSG_EEPROM) $@ |
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ |
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@ |
# Create extended listing file from ELF output file. |
%.lss: %.elf |
@echo |
@echo $(MSG_EXTENDED_LISTING) $@ |
$(OBJDUMP) -h -S $< > $@ |
# Create a symbol table from ELF output file. |
%.sym: %.elf |
@echo |
@echo $(MSG_SYMBOL_TABLE) $@ |
avr-nm -n $< > $@ |
# Link: create ELF output file from object files. |
.SECONDARY : $(TARGET).elf |
.PRECIOUS : $(OBJ) |
%.elf: $(OBJ) |
@echo |
@echo $(MSG_LINKING) $@ |
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS) |
# Compile: create object files from C source files. |
%.o : %.c |
@echo |
@echo $(MSG_COMPILING) $< |
$(CC) -c $(ALL_CFLAGS) $< -o $@ |
# Compile: create assembler files from C source files. |
%.s : %.c |
$(CC) -S $(ALL_CFLAGS) $< -o $@ |
# Assemble: create object files from assembler source files. |
%.o : %.S |
@echo |
@echo $(MSG_ASSEMBLING) $< |
$(CC) -c $(ALL_ASFLAGS) $< -o $@ |
# Target: clean project. |
clean: begin clean_list finished end |
clean_list : |
@echo |
@echo $(MSG_CLEANING) |
# $(REMOVE) $(TARGET).hex |
$(REMOVE) $(TARGET).eep |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).cof |
$(REMOVE) $(TARGET).elf |
$(REMOVE) $(TARGET).map |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).a90 |
$(REMOVE) $(TARGET).sym |
$(REMOVE) $(TARGET).lnk |
$(REMOVE) $(TARGET).lss |
$(REMOVE) $(OBJ) |
$(REMOVE) $(LST) |
$(REMOVE) $(SRC:.c=.s) |
$(REMOVE) $(SRC:.c=.d) |
# Automatically generate C source code dependencies. |
# (Code originally taken from the GNU make user manual and modified |
# (See README.txt Credits).) |
# |
# Note that this will work with sh (bash) and sed that is shipped with WinAVR |
# (see the SHELL variable defined above). |
# This may not work with other shells or other seds. |
# |
%.d: %.c |
set -e; $(CC) -MM $(ALL_CFLAGS) $< \ |
| sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \ |
[ -s $@ ] || rm -f $@ |
# Remove the '-' if you want to see the dependency files generated. |
-include $(SRC:.c=.d) |
# Listing of phony targets. |
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \ |
clean clean_list program |
/branches/thjac/V1_11/menu.c |
---|
0,0 → 1,150 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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" |
#include "altcon.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 = 11; |
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 ",GetActiveParamSetNumber()); |
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,"RegOffset: %5i", pressureOffset ); |
LCD_printfxy(0,1,"Luftdruck: %5i", airPressure ); |
LCD_printfxy(0,2,"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)",AdWertNick - AdNeutralNick, AdNeutralNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll); |
LCD_printfxy(0,3,"Gier %4i (%3i)",AdNeutralGier - AdWertGier, AdNeutralGier); |
} |
else |
if((PlatinenVersion == 11) || (PlatinenVersion == 20)) |
{ |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdWertNick - AdNeutralNick, AdNeutralNick/2); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll/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, AdNeutralNick/2,AnalogOffsetNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)(%3i)",AdWertRoll - AdNeutralRoll, AdNeutralRoll/2,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; |
default: MaxMenue = MenuePunkt - 1; |
MenuePunkt = 0; |
break; |
} |
RemoteKeys = 0; |
} |
/branches/thjac/V1_11/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/thjac/V1_11/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/thjac/V1_11/parameter.h |
---|
0,0 → 1,129 |
/* parameter.h |
*/ |
#ifndef PARAMETER_H |
#define PARAMETER_H |
#define PARAM_TIMER_2S 100 // 2 Sekunden |
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas |
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung |
#define PITCH_NEUTRAL_TIMER 25 |
#define PITCH_MIN2_TIMER 100 |
#define PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1 |
#define PARAM_ALT_P Parameter_Hoehe_P |
#define PARAM_ALT_I EE_Parameter.UserParam3 |
#define PARAM_ALT_D Parameter_Luftdruck_D |
#define PARAM_ALT_ACC Parameter_Hoehe_ACC_Wirkung |
#define PARAM_ALT_GAIN EE_Parameter.Hoehe_Verstaerkung |
#define PARAM_ALT_MAX EE_Parameter.MaxHoehe |
#define PARAM_ALT_INT_MAX 1000000L |
#define PARAM_ALT_EXP_SMOOTHING_FACTOR EE_Parameter.UserParam4 // Faktor für exp. Glättung |
/****************************************************************************************** |
* Allgemeine Pitch-Steuerungsparameter |
*/ |
#define PARAM_PITCH_STICK_THRESHOLD 5 |
#define PARAM_PITCH_MIN2 EE_Parameter.UserParam2 // Minimalgas |
#define PITCH_ALT_THRESHOLD PITCH_MIN2 // Schwellwert für Höhenregelung |
/****************************************************************************************** |
* Parameter für Neutral-Pitch-Steuerung |
*/ |
#define PARAM_PITCH_NEUTRAL_DIFF EE_Parameter.UserParam1 // Stick-Loslass-Erkennung |
/****************************************************************************************** |
* Parameter für MD-Pitch-Steuerung |
*/ |
#define PARAM_PITCH_MD_HOVER EE_Parameter.UserParam1 // Standgaswert |
#define PARAM_PITCH_MD_DELAY0 5 // Begrenzung der Pitch-Beschleunigung am Boden |
#define PARAM_PITCH_MD_DELAY1 0 // Begrenzung der Pitch-Beschleunigung im Flug |
/****************************************************************************************** |
* Parameter für LED-Ansteuerung |
*/ |
/* Helligkeit J16 |
*/ |
#define PARAM_LED_BRIGHTNESS_J16 ( EE_Parameter.UserParam5 ) // 0-250, 25x=PotiX, Wert wird durch 23 geteilt |
/* Helligkeit J17 |
*/ |
#define PARAM_LED_BRIGHTNESS_J17 ( EE_Parameter.UserParam6 ) // 0-250, 25x=PotiX, Wert wird durch 23 geteilt |
/* Wenn die Unterspannungswarnung aktiv wird, kann mit diesem Parameter eingestellt |
* werden, daß sich die Blinkfrequenz der LED's verdoppelt. |
*/ |
#define PARAM_LED_WARNING_SPEEDUP ( EE_Parameter.UserParam7 & 0x03 ) // 0=deaktiviert >0=Blinkenbeschleunigung |
/* Erzwingt die Aktivierung der LED-Ausgänge im Fall einer |
* Unterspannungswarnung. |
*/ |
#define PARAM_LED_FORCE_WARNING_ENABLED ( EE_Parameter.UserParam7 & 0x04 ) // 0=deaktiviert 1=aktiviert |
/* Wenn gesetzt, wird der Pegel des Ausgangs J16 vertauscht. |
*/ |
#define PARAM_LED_NEGATE_J16 ( EE_Parameter.UserParam7 & 0x08 ) // 0=normal 1=invertiert |
#define LED_NEGATE_J16 ( PARAM_LED_NEGATE_J16 >> 3 ) |
/* Wenn gesetzt, wird der Pegel des Ausgangs J17 vertauscht. |
*/ |
#define PARAM_LED_NEGATE_J17 ( EE_Parameter.UserParam7 & 0x10 ) // 0=normal 1=invertiert |
#define LED_NEGATE_J17 ( PARAM_LED_NEGATE_J17 >> 4 ) |
/* Die Ausgänge J16/J17 lassen sich wahlweise über die Motoren koppeln, so daß |
* die Ausgänge bei ausgeschalteten Motoren unabhängig vom eingestellten Blink-Muster |
* deaktiviert sind. |
*/ |
#define PARAM_LED_ENGINE_ENABLED ( EE_Parameter.UserParam7 & 0x20 ) // 0=deaktiviert 1=an Motoren gekoppelt |
/* Die Ausgänge J16/J17 lassen sich wahlweise bei stehenden Motoren üner den Gierstick schalten. |
*/ |
#define PARAM_LED_STICK_ENABLED ( EE_Parameter.UserParam7 & 0x40 ) // 0=deaktiviert 1=aktiviert, wird durch Motorkopplung übersteuert |
/* Dieser Parameter legt fest, ob in '+'-Formation (normal) oder 'X'-Formation |
* geflogen werden soll. Zur Umschaltung reicht das Setzen des Parameters. Ein |
* Umbau der FC oder Einstellungen am Sender sind nicht notwendig. |
*/ |
#define PARAM_X_FORMATION ( EE_Parameter.UserParam8 & 0x01 ) // 0=+, 1=X |
/* Steuert, ob beim bzw. vor dem Start der Motoren eine Kalibrierung erfolgen |
* soll. |
*/ |
#define PARAM_CAL_ON_START ( EE_Parameter.UserParam8 & 0x02 ) // 0=deaktiviert 1=aktiviert |
/* Setzt den zu verwendenden Pitch-Modus |
*/ |
#define PARAM_PITCH_MODE (( EE_Parameter.UserParam8 & 0x0C ) >> 2 ) // Pitch-Mode 0-3 |
#define PARAM_PITCH_MODE_NORMAL 0x00 |
#define PARAM_PITCH_MODE_NEUTRAL 0x01 |
#define PARAM_PITCH_MODE_MD 0x02 |
/* Wenn gesetzt, wird nach ca. 2s andauernder Stick-Stellung auf Minimum |
* ein Reset der Pitch-Regelung durchgeführt, so daß der MK ohne Aus- und |
* Einschalten der Motoren wieder starten kann. Die Pitch-Regelung |
* schaltet auf Leerlaufgas zurück und der Stick kann losgelassen werden, |
* ohne das Gas gegeben wird. |
*/ |
#define PARAM_PITCH_RESTART_ENABLED ( EE_Parameter.UserParam8 & 0x10 ) // 0=deaktiviert 1=aktiviert |
/* Die Motoren können über diesen Parameter aktiviert werden. Ein Wert von 0 |
* deaktiviert die Motoren und kann zum Testen verwendet werden. |
*/ |
#define PARAM_ENGINE_ENABLED ( EE_Parameter.UserParam8 & 0x20 ) // 0=deaktiviert 1=aktiviert |
/* Skalierung des Gasinkrements (Default 15) |
*/ |
#define PARAM_STICK_SCALE (( EE_Parameter.UserParam8 & 0xC0 ) >> 6) |
#endif // PARAMETER_H |
/branches/thjac/V1_11/pitch.c |
---|
0,0 → 1,74 |
/* pitch.c |
* |
* Copyright 2009 Thomas Jachmann |
* |
* Pitch-Steuerung |
*/ |
#include "main.h" |
#include "parameter.h" |
#include "pitch_neutral.h" |
#include "pitch_md.h" |
#include "pitch.h" |
// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus |
int (* pitch_value_ptr)( void ); |
// Prototyp |
int pitch_mk_value( void ); |
/* |
* Führt die Initialisierung der Pitch-Steuerung durch. Diese Funktion |
* wird nach jeder Setting-Auswahl sowie nach jeder Setting-Änderung |
* aufgerufen. |
*/ |
void pitch_init( void ) { |
// FIXME Funktioniert noch nicht |
switch( PARAM_PITCH_MODE ) { |
case PARAM_PITCH_MODE_NEUTRAL: |
pitch_value_ptr = pitch_neutral_value; |
break; |
case PARAM_PITCH_MODE_MD: |
pitch_value_ptr = pitch_md_value; |
break; |
default: |
pitch_value_ptr = pitch_mk_value; |
} |
// Hier können weitere Initialisierungen folgen |
} |
int pitch_value( void ) { |
switch( PARAM_PITCH_MODE ) { |
case PARAM_PITCH_MODE_NEUTRAL: |
return pitch_neutral_value(); |
case PARAM_PITCH_MODE_MD: |
return pitch_md_value(); |
default: |
return pitch_mk_value(); |
} |
} |
/* |
* Führt eine Pitch-Berechnung aus, die der Original-SW entspricht. |
*/ |
int pitch_mk_value( void ) { |
register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ]; |
// Warum 120? Gas= 0 ist -125 |
// register int pitchCount = stickValue + 120; |
register int pitchCount = stickValue + 125; |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = pitchCount; |
return pitchCount; |
} |
/branches/thjac/V1_11/pitch.h |
---|
0,0 → 1,18 |
/* pitch.h |
* |
* copyright 2009 Thoams Jachmann |
*/ |
#ifndef _PITCH_H |
#define _PITCH_H |
extern int (* pitch_value_ptr)( void ); |
// TODO Soll den Stick-Wert der Mittelstellung liefern |
#define pitch_stickoffset() 0 |
extern int pitch_value( void ); |
// #define pitch_value() ( pitch_value_ptr != 0 ? pitch_value_ptr() : 0 ) |
#endif // PITCH_H |
/branches/thjac/V1_11/pitch_md.c |
---|
0,0 → 1,308 |
/* pitch_md.c |
* |
* Copyright 2009 Thomas Jachmann |
* |
* Die in dieser Quelldatei enthaltenen Algorithmen ermöglichen eine MD-ähnliche Pitch-Steuerung |
* für den MK. |
*/ |
#include "main.h" |
#include "parameter.h" |
#include "fc.h" |
#include "pitch.h" |
#include "altcon.h" |
#define STATE_INITIALIZE 0x01 // Anfangszustand nach Einschalten der Motoren |
#define STATE_SETUP 0x02 // Ermittlung von PARAM_PITCH_MD_HOVER |
#define STATE_BEGIN 0x03 // Anfangszustand für Flugbetrieb |
#define STATE_BEGIN1 0x04 // Anfangszustand für Flugbetrieb |
#define STATE_READY 0x05 // Manuelle Kontrolle |
#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
int stickValue = 0; // Aktueller Stick-Wert |
int lastStickValue = 0; // Vorheriger Stick-Wert |
int actualPitchCount; // Soll-Pitch-Wert |
int targetPitchCount; // Ist-Pitch-Wert |
int pitchOffset; // Aktueller Grundgaswert in Neutralstellung |
char state; // Zustand |
int timer; |
int delay = 0; |
int delayCounter = 0; |
int temp; // Temporäre Werte; wird mehrfach verwendet |
/* |
* Berechnet den aktuellen Pitch-Wert für die Regelung |
* |
* Nachdem die Motoren eingeschaltet sind, wird der Pitch-Stick losgelassen und damit in Mittelstellung |
* gebracht. Die Motoren laufen zu dieser Zeit im Leerlaufgas. Vor dem Abheben müssen die Motoren in |
* das Standgas gebracht werden. Dies geschieht durch minimales Gasgeben. Durch weiteres Gasgeben und |
* nehmen kann abgehoben und geflogen werden. |
* |
* Erreicht der Stick während des Fluges die Neutralstellung und verbleibt dort für ca. 1 Sekunde ohne |
* bewegt zu werden, aktiviert sich die Höhenregelung und hält den MK auf der aktuellen Höhe. |
* |
* Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert |
* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun |
* über den gesamten Stick-Bereich regeln. |
* |
* Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung |
* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein |
* ungestörtes manuelles Steuern möglich. |
* |
* Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb |
* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für |
* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
*/ |
int pitch_md_value( void ) { |
int register rawStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset(); |
// Sind die Motoren eingeschaltet? |
if( MotorenEin ) { |
// Vorigen Stick-Wert merken |
lastStickValue = stickValue; |
/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
temp = rawStickValue; |
if( temp > 0 ) { |
temp = temp + ( ( temp * temp ) / 150 ); |
} else { |
temp = temp - ( ( temp * temp ) / 150 ); |
} |
stickValue = temp; |
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
* zuzüglich dem Stick-Wert. */ |
targetPitchCount = stickValue + pitchOffset; |
switch( state ) { |
/* Entscheidet über Flugbetrieb oder Setup-Betrieb. Für den Setup-Betrieb |
* muß beim Einschalten der Motoren gleichzeitig der Roll-Stick ganz |
* betätigt werden (Richtung ist egal). |
*/ |
case STATE_INITIALIZE: |
if( abs( PPM_in[ EE_Parameter.Kanalbelegung[ K_ROLL ] ] ) > 70 ) { |
state = STATE_SETUP; |
} else { |
state = STATE_BEGIN; |
} |
break; |
/* Erlaubt die Ermittlung des Parameters PARAM_PITCH_MD_HOVER. Hierzu wird soviel Gas |
* gegeben, bis der MK kurz vor dem Abheben ist, jedoch noch stabil steht. Um den |
* Gaswert dauerhaft zu speichern, muß der Stick an der Position verweilen, bis |
* der Summer die Übernahme akustisch quittiert. Dann müssen die Motoren wieder |
* ausgeschaltet werden, da dieser Modus nicht für den Flug vorgesehen ist. |
*/ |
case STATE_SETUP: |
// Im Setup-Modus soll das Gas nicht träge reagieren |
actualPitchCount = targetPitchCount; |
if( rawStickValue < 20 || ( stickValue - lastStickValue ) ) { |
timer = PARAM_TIMER_2S; |
} |
/* Der Stick befindet sich eindeutig in der oberen Hälfte und wurde |
* seit dem letzten Zyklus nicht bewegt. */ |
else { |
timer--; |
/* Die Verweilzeit ist abgelaufen und der aktuelle Pitch-Wert |
* entspricht nicht dem bereits gespeicherten Wert. */ |
if( !timer && ( PARAM_PITCH_MD_HOVER != actualPitchCount ) ) { |
// Aktuellen Pitch-Wert in Konfiguration übernehmen |
PARAM_PITCH_MD_HOVER = actualPitchCount; |
// Konfiguration dauerhaft speichern |
WriteParameterSet( |
GetActiveParamSetNumber(), |
(unsigned char *) &EE_Parameter.Kanalbelegung[0], |
STRUCT_PARAM_LAENGE ); |
// Signalisieren |
beeptime = 500; |
} |
} |
break; |
/* In diesem Zustand steht der MK am Boden und die Motoren laufen auf Leerlaufgas. Ein |
* kurzes Auslenken des Sticks nach oben schaltet in das Standgas über. |
*/ |
case STATE_BEGIN: |
// Begrenzung der Pitch-Beschleunigung am Boden |
delay = PARAM_PITCH_MD_DELAY0; |
if( rawStickValue > PARAM_PITCH_STICK_THRESHOLD ) { |
pitchOffset = PARAM_PITCH_MD_HOVER; |
} else if( pitchOffset == PARAM_PITCH_MD_HOVER ) { |
state = STATE_BEGIN1; |
} |
break; |
// MK soll erst abheben, weil sonst die Höhenregelung am Boden schon greift |
case STATE_BEGIN1: |
if( abs( rawStickValue ) > 10 ) { |
// Begrenzung der Pitch-Beschleunigung im Flug |
delay = PARAM_PITCH_MD_DELAY1; |
if( rawStickValue > 0 ) { |
state = STATE_READY; |
} |
} |
break; |
/* Die Motoren laufen jetzt mindestens mit Standgas. Der MK ist bereit zum Abheben. |
* Das Minimalgas kann jetzt nicht mehr unterschritten werden. |
*/ |
case STATE_READY: |
// Ist ein Restart zulässig? |
if( PARAM_PITCH_RESTART_ENABLED ) { |
/* Wenn der Gashebel ganz unten steht, Timer für Restart der Pitch-Regelung |
* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
timer = PITCH_MIN2_TIMER; |
} else { |
timer--; |
/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
if( !timer ) { |
state = STATE_BEGIN; |
pitchOffset = 0; |
targetPitchCount = 0; |
actualPitchCount = 0; |
// Signalisieren |
beeptime = 500; |
} |
} |
} |
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( targetPitchCount < PARAM_PITCH_MIN2 ) { |
targetPitchCount = PARAM_PITCH_MIN2; |
} |
// Stick ist innerhalb der Neutralstellung |
if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) { |
// Aktuelle Höhe festhalten (aktiviert noch nicht den Regler) |
altcon_lock(); |
// Timer neu setzen |
timer = PITCH_NEUTRAL_TIMER; |
state = STATE_WAIT; |
} |
break; |
/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich |
* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung |
* erfolgen, um die automatische Höhenregelung zu aktivieren. */ |
case STATE_WAIT: |
/* Stick ist innerhalb der Neutralstellung und |
Stick-Differenzial ist < 2 */ |
if( abs( rawStickValue ) < PARAM_PITCH_STICK_THRESHOLD && |
lastStickValue == stickValue ) { |
timer--; |
if( !timer ) { |
state = STATE_ACTIVATING; |
} |
// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
} else { |
state = STATE_READY; |
} |
break; |
/* Die automatische Höhenregelung wird jetzt aktiviert. |
*/ |
case STATE_ACTIVATING: |
// Aktivierung des Höhenreglers mit der zuvor gemerkten Sollhöhe |
altcon_start(); |
state = STATE_ACTIVE; |
break; |
/* Die automatische Höhenregelung ist aktiv. */ |
case STATE_ACTIVE: |
// Stick ist außerhalb der Neutralstellung |
if( abs( rawStickValue ) > PARAM_PITCH_STICK_THRESHOLD ) { |
// Höhenregler deaktivieren |
altcon_stop(); |
pitchOffset -= altcon_avgerror() / 4; |
targetPitchCount = stickValue + pitchOffset; |
state = STATE_READY; |
} |
break; |
} |
// Motoren sind aus |
} else { |
/* Nach dem Einschalten der Motoren wird pitchOffset auf PARAM_PITCH_OVER gesetzt. |
*/ |
actualPitchCount = 0; |
targetPitchCount = 0; |
pitchOffset = 0; |
stickValue = 0; |
state = STATE_INITIALIZE; |
} |
if( pitchOffset < 0 ) |
pitchOffset = 0; |
if( !delayCounter ) { |
/* Durch die Sollwertvorgabe kann hier eine einstellbare Trägheit auf dem Pitch-Wert |
* abgebildet werden. */ |
int pitchDelta = targetPitchCount - actualPitchCount; |
if( pitchDelta > 3 ) |
pitchDelta = 3; |
if( pitchDelta < -3 ) |
pitchDelta = -3; |
actualPitchCount += pitchDelta; |
delayCounter = delay + 1; |
} |
delayCounter--; |
// Pitch-Wert darf nicht < 0 sein |
if( actualPitchCount < 0 ) { |
actualPitchCount = 0; |
} |
// pitchCount als Debug-Wert rausschreiben |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = actualPitchCount; |
DebugOut.Analog[29] = pitchOffset; |
return actualPitchCount; |
} |
/branches/thjac/V1_11/pitch_md.h |
---|
0,0 → 1,12 |
/* pitch_md.h |
* |
* Copyright 2009 Thoams Jachmann |
*/ |
#ifndef PITCH_MD_H |
#define PITCH_MD_H |
extern void pitch_md_init( void ); |
extern int pitch_md_value( void ); |
#endif // PITCH_MD_H |
/branches/thjac/V1_11/pitch_neutral.c |
---|
0,0 → 1,259 |
/* pitch_neutral.c |
* |
* Copyright 2009 Thomas Jachmann |
*/ |
#include "main.h" |
#include "parameter.h" |
#include "fc.h" |
#include "altcon.h" |
#include "pitch.h" |
#include "pitch_neutral.h" |
#define STATE_STARTUP_WAIT 0x00 // Init-Timeout beim Einschalten abwarten |
#define STATE_STARTUP_INIT 0x01 // Initialisierung beim Einschalten |
#define STATE_BEGIN 0x02 // Anfangszustand nach Einschalten der Motoren |
#define STATE_INITIALIZING 0x03 // Initialisierungsphase |
#define STATE_MANUAL 0x04 // Manuelle Kontrolle, Höhenregelung in Konfiguration deaktiviert |
#define STATE_INACTIVE 0x05 // Manuelle Kontrolle |
#define STATE_WAIT 0x06 // Warten auf Einschalten der Höhenregelung |
#define STATE_ACTIVATING 0x07 // Aktivierung der Höhenregelung |
#define STATE_ACTIVE 0x08 // Höhenregelung ist aktiv |
static int stickValue = 0; // Aktueller Stick-Wert |
static int lastStickValue = 0; // Vorheriger Stick-Wert |
static int pitchOffset = 0; // Aktueller Grundgaswert in Neutralstellung |
static char state = STATE_STARTUP_WAIT; // Zustand |
static int temp; |
/* Wird verwendet, um das Umschalten auf automatische Höhenregelung |
* nach Erreichen der Neutralstellung zu verzögern. |
*/ |
static int pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
/* |
* Berechnet den aktuellen Pitch-Wert für die Regelung |
* |
* Nach dem Einschalten der FC wird der aktuelle Gas-Stick-Wert gelesen und als Kalibrierungswert |
* für die Neutralstellung gespeichert. Somit spielt die korrekte Trimmung des Sticks auf Senderseite |
* keine Rolle. |
* |
* Nach Einschalten der Motoren geht der Stick in Neutralstellung. Diese Stick-Bewegung wird ignoriert |
* und die Motoren drehen mit dem eingestellten MinGas2. Ausgehend von der Neutralstellung wird nun |
* durch Bewegen des Sticks im oberen Bereich das Gas geregelt. |
* |
* Das erstmalige Aktivieren der automatischen Höhenregelung erfolgt durch Loslassen des Sticks im |
* Schwebeflug. Der zuvor aktuelle Stick-Wert wird als Wert in Neutralstellung übernommen und die |
* automatische Höhenregelung sofort aktiviert. |
* |
* Sobald der Stick die Neutralstellung verläßt, wird die automatische Höhenregelung deaktiviert |
* und der vorige Pitch-Wert als Wert der Neutralstellung übernommen. Der Pitch läßt sich nun |
* über den gesamten Stick-Bereich regeln. |
* |
* Erreicht der Stick ein weiteres Mal die Neutralstellung, wird die automatische Höhenregelung |
* wieder aktiviert, jetzt jedoch immer mit einer zeitlichen Verzögerung. Nur so ist ein |
* ungestörtes manuelles Steuern möglich. |
* |
* Der Pitch-Wert ist innerhalb der Regelung durch ein konfigurierbares Minimalgas nach unten begrenzt. |
* Dieses Minimalgas kann auf einen sehr niedrigen Wert eingestellt sein. Um im Flug nicht unterhalb |
* eines Wertes zu gelangen, der die Lageregelung außer Funktion setzt, wird ein zweiter Wert für |
* Minimalgas konfiguriert, der greift, sobald erstmalig die automatische Höhenregelung aktiviert wurde. |
*/ |
int pitch_neutral_value( void ) { |
int register pitchCount = 0; |
// Sind die Motoren eingeschaltet? |
if( MotorenEin ) { |
// Vorigen Stick-Wert merken |
lastStickValue = stickValue; |
/* StickValue exponentiell angleichen, da ausgehend von der Neutralstellung |
* nur jeweils die halbe Auflösung nach oben und unten zur Verfügung steht. Bei einer |
* Multiplikation mit 2 ließe sich das Gas im Schwebebereich nicht fein genug einstellen. */ |
temp = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] - pitch_stickoffset(); |
if( temp > 0 ) { |
temp = temp + ( ( temp * temp ) / 150 ); |
} else { |
temp = temp - ( ( temp * temp ) / 150 ); |
} |
// Original-Stick-Wert holen und glätten |
stickValue = ( temp + 2 * lastStickValue ) / 3; |
/* Aktuellen Pitch-Wert berechnen. Der Wert ergibt sich aus dem Pitch-Offset |
* zuzüglich dem Stick-Wert. */ |
pitchCount = stickValue + pitchOffset; |
switch( state ) { |
case STATE_BEGIN: |
// Schnelles Bewegen aus dem oberen Bereich des Sticks in Neutralstellung |
if( ( lastStickValue > PARAM_PITCH_STICK_THRESHOLD ) && |
( lastStickValue - stickValue >= PARAM_PITCH_NEUTRAL_DIFF ) ) { |
pitchOffset = lastStickValue; |
state = STATE_INITIALIZING; |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
} |
break; |
case STATE_INITIALIZING: |
// Während der Initialisierung das Gas konstant halten |
pitchCount = pitchOffset; |
pitchNeutralTimer--; |
/* Läuft der Timer ab, bevor der Stick die Neutralstellung erreicht, |
* wird die Aktion nicht als "schnelles Bewegen in Neutralstellung" |
* gedeutet. */ |
if( !pitchNeutralTimer ) { |
pitchOffset = 0; |
state = STATE_BEGIN; |
} |
// Ist die Neutralstellung erreicht? |
if( abs( stickValue ) <= PARAM_PITCH_STICK_THRESHOLD ) { |
// Ist die Höhenregelung aktiviert? |
if( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) { |
state = STATE_ACTIVATING; |
} else { |
state = STATE_MANUAL; |
} |
} |
break; |
/* Wenn die Höhenregelung per Konfiguration deaktiviert ist, verbleibt |
* die Funktion in diesem Zustand. */ |
case STATE_MANUAL: |
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( pitchCount < PARAM_PITCH_MIN2 ) { |
pitchCount = PARAM_PITCH_MIN2; |
} |
break; |
/* Die Höhenregelung ist per Konfiguration aktiviert, jedoch befindet |
* sich der Stick außerhalb des als Neutralstellung anerkannten |
* Wertebereiches. Es wird manuell geregelt. */ |
case STATE_INACTIVE: |
// Ist ein Restart zulässig? |
if( PARAM_PITCH_RESTART_ENABLED ) { |
/* Wenn der Gashebel ganz unten steht, Timer für Reduzierung des Minimalgaswertes |
* starten. Hierfür wird die Variable pitchNeutralTimer verwendet. */ |
if( PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ] > 35 - 120 ) { |
pitchNeutralTimer = PITCH_MIN2_TIMER; |
} else { |
pitchNeutralTimer--; |
/* Gashebel steht seit PITCH_MIN2_TIMER ganz unten; jetzt erfolgt die Initialisierung. */ |
if( !pitchNeutralTimer ) { |
state = STATE_BEGIN; |
pitchOffset = 0; |
// Signalisieren |
beeptime = 500; |
} |
} |
} |
// Min2-Gas einstellen für Lageregelung bei Minimalgas |
if( pitchCount < PARAM_PITCH_MIN2 ) { |
pitchCount = PARAM_PITCH_MIN2; |
} |
// Stick ist innerhalb der Neutralstellung |
if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD ) { |
// Timer neu setzen |
pitchNeutralTimer = PITCH_NEUTRAL_TIMER; |
state = STATE_WAIT; |
} |
break; |
/* Der Stick ist in den für die Neutralstellung gültigen Wertebereich |
* gelangt. Nun darf innerhalb einer bestimmten Zeit keine Stick-Bewegung |
* erfolgen, um die automatische Höhenregelung zu aktivieren. */ |
case STATE_WAIT: |
/* Stick ist innerhalb der Neutralstellung und |
Stick-Differenzial ist < 2 */ |
if( abs( stickValue ) < PARAM_PITCH_STICK_THRESHOLD && |
lastStickValue == stickValue ) { |
pitchNeutralTimer--; |
if( !pitchNeutralTimer ) { |
state = STATE_ACTIVATING; |
} |
// Aktivierungskriterium nicht erfüllt, zurück in INACTIVE |
} else { |
state = STATE_INACTIVE; |
} |
break; |
/* Die automatische Höhenregelung wird jetzt aktiviert. Der aktuelle |
* Luftdruck wird gespeichert und notwendige Werte für den Regler |
* werden initialisiert. */ |
case STATE_ACTIVATING: |
// Höhenregler starten |
altcon_start(); |
state = STATE_ACTIVE; |
break; |
/* Die automatische Höhenregelung ist aktiv. */ |
case STATE_ACTIVE: |
// Stick ist außerhalb der Neutralstellung |
if( abs( stickValue ) > PARAM_PITCH_STICK_THRESHOLD ) { |
pitchOffset -= altcon_avgerror() / 4; |
pitchCount = stickValue + pitchOffset; |
// Höhenregler abschalten |
altcon_stop(); |
state = STATE_INACTIVE; |
} |
break; |
} |
// Motoren sind aus |
} else { |
/* Nach dem Einschalten der Motoren darf pitchOffset keinen hohen Wert haben, |
* da der Kopter sonst sofort hochschießen würde. |
*/ |
pitchCount = 0; |
pitchOffset = 0; |
stickValue = 0; |
state = STATE_BEGIN; |
} |
if( pitchOffset < 0 ) |
pitchOffset = 0; |
// Pitch-Wert darf nicht < 0 sein |
if( pitchCount < 0 ) { |
pitchCount = 0; |
} |
// pitchCount als Debug-Wert rausschreiben |
DebugOut.Analog[26] = stickValue; |
DebugOut.Analog[28] = pitchCount; |
DebugOut.Analog[29] = pitchOffset; |
return pitchCount; |
} |
/branches/thjac/V1_11/pitch_neutral.h |
---|
0,0 → 1,12 |
/* pitch_neutral.h |
* |
* copyright 2009 Thoams Jachmann |
*/ |
#ifndef _PITCH_NEUTRAL_H |
#define _PITCH_NEUTRAL_H |
extern void pitch_neutral_init( void ); |
extern int pitch_neutral_value( void ); |
#endif // PITCH_NEUTRAL_H |
/branches/thjac/V1_11/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/thjac/V1_11/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/thjac/V1_11/rc.c |
---|
0,0 → 1,85 |
/*####################################################################################### |
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 |
// PWM |
//TCCR1A = (1 << COM1B1) | (1 << WGM11) | (1 << WGM10); |
//TCCR1B |= (1 << WGM12); |
//OCR1B = 55; |
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 > (int) Parameter_UserParam2 * 10) && (signal < 8000)) |
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;} |
// tmp = (7 * (PPM_in[index]) + signal) / 8; |
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) PORTD |= 0x20; else PORTD &= ~0x20; // Servosignal an J3 anlegen |
if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10; // Servosignal an J4 anlegen |
if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08; // Servosignal an J5 anlegen |
} |
} |
} |
/branches/thjac/V1_11/rc.h |
---|
0,0 → 1,29 |
/*####################################################################################### |
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_TEILER CK64 |
#define TIMER_RELOAD_VALUE 250 |
//#define TIMER_TEILER CK256 // bei 20MHz |
//#define TIMER_RELOAD_VALUE -78 // bei 20MHz |
#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/thjac/V1_11/spi.c |
---|
0,0 → 1,290 |
// ######################## 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; |
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; |
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) |
{ |
// static unsigned char i =0; |
signed int tmp; |
cli(); |
ToNaviCtrl.IntegralNick = (int) (IntegralNick / 108); |
ToNaviCtrl.IntegralRoll = (int) (IntegralRoll / 108); |
ToNaviCtrl.GyroCompass = ErsatzKompass / GIER_GRAD_FAKTOR; |
ToNaviCtrl.AccNick = (int) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc); |
ToNaviCtrl.AccRoll = (int) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc); |
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; |
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; |
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; |
} |
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.Byte[0]; |
FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1]; |
FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2]; |
break; |
case SPI_CMD_OSD_DATA: |
FromNaviCtrl_Value.OsdBar = FromNaviCtrl.Param.Byte[0]; |
FromNaviCtrl_Value.Distance = FromNaviCtrl.Param.Int[1]; |
break; |
case SPI_CMD_GPS_POS: |
// ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude; |
// ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude; |
break; |
case SPI_CMD_GPS_TARGET: |
// ToFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude; |
// ToFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude; |
break; |
default: |
break; |
} |
} |
else |
{ |
// KompassValue = 0; |
// KompassRichtung = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
} |
} |
#endif |
/branches/thjac/V1_11/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 Byte[12]; |
int Int[6]; |
long Long[3]; |
float Float[3]; |
} Param; |
unsigned char Chksum; |
}; |
#define SPI_CMD_OSD_DATA 100 |
#define SPI_CMD_GPS_POS 101 |
#define SPI_CMD_GPS_TARGET 102 |
#define SPI_KALMAN 103 |
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 Byte[12]; |
int Int[6]; |
long Long[3]; |
float Float[3]; |
} Param; |
unsigned char Chksum; |
}; |
struct str_FromNaviCtrl_Value |
{ |
signed int OsdBar; |
signed int Distance; |
signed char Kalman_K; |
signed char Kalman_MaxDrift; |
signed char Kalman_MaxFusion; |
}; |
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; |
//#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/thjac/V1_11/timer0.c |
---|
0,0 → 1,216 |
#include "main.h" |
#include "parameter.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; |
volatile unsigned int ServoState = 40; |
unsigned int BeepMuster = 0xffff; |
unsigned int ServoValue = 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 -> ist jetzt 10kHz |
{ |
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; |
cnt_1ms = !cnt_1ms; // So ist das etwas einfacher |
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; |
} |
} |
} |
//---------------------------- |
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; |
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3; |
// TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256 |
TCCR2B = (0 << CS20) | (0 << CS21) | (1 << CS22); // clk/64 |
TIMSK2 |= _BV(OCIE2A); |
TIMSK0 |= _BV(TOIE0); |
OCR2A = 10; |
TCNT2 = 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)) |
ANALOG_ON; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Servo ansteuern |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
SIGNAL(SIG_OVERFLOW2) { |
if (ServoState > 0) |
PORTD |= 0x80; |
else |
PORTD &= ~0x80; |
TCCR2A = 3; |
TIMSK2 &= ~_BV(TOIE2); |
} |
SIGNAL(SIG_OUTPUT_COMPARE2A) { |
static unsigned char postPulse = 0x80; |
static int filterServo = 100; |
if (ServoState == 4) { |
ServoValue = 0x0030; // Offset Part1 |
filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2) >> DIV_4; |
ServoValue += filterServo; |
// Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden |
if (ServoValue < ((int) EE_Parameter.ServoNickMin * 3)) |
ServoValue = (int) EE_Parameter.ServoNickMin * 3; |
else if (ServoValue > ((int) EE_Parameter.ServoNickMax * 3)) |
ServoValue = (int) EE_Parameter.ServoNickMax * 3; |
long integral; |
/* Über Parameter läßt sich zwischen "+" und "X" - Formations |
* umschalten (sh. parameter.h) |
*/ |
if (PARAM_X_FORMATION) |
integral = IntegralNick - IntegralRoll; |
else |
integral = IntegralNick; |
if (EE_Parameter.ServoNickCompInvert & 0x01) |
ServoValue += ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4); |
else |
ServoValue -= ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4); |
DebugOut.Analog[20] = ServoValue; |
if ((ServoValue % 255) < 45) { |
ServoValue += 77; |
postPulse = 0x60 - 77; |
} else |
postPulse = 0x60; |
OCR2A = 255 - (ServoValue % 256); |
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3; |
} else if ((ServoState > 0) && (ServoState < 4)) { |
if (ServoValue > 255) { |
PORTD |= 0x80; |
TCCR2A = 3; |
ServoValue -= 255; |
} else { |
TCCR2A = (1 << COM2A1) | (0 << COM2A0) | 3; |
OCR2A = postPulse; // Offset Part2 |
ServoState = 1; |
} |
} else if (ServoState == 0) { |
ServoState = (int) EE_Parameter.ServoNickRefresh << MUL_4; |
PORTD &= ~0x80; |
TCCR2A = 3; |
} |
ServoState--; |
} |
/branches/thjac/V1_11/timer0.h |
---|
0,0 → 1,17 |
#define TIMER_TEILER CK8 |
#define TIMER_RELOAD_VALUE 250 |
void Timer_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 unsigned int ServoValue; |
extern unsigned int BeepMuster; |
extern volatile unsigned char SendSPI; |
/branches/thjac/V1_11/twimaster.c |
---|
0,0 → 1,204 |
/*############################################################################ |
############################################################################*/ |
#include "main.h" |
volatile unsigned char twi_state = 0; |
unsigned char motor = 0; |
unsigned char motorread = 0; |
unsigned char motor_rx[8]; |
//############################################################################ |
//Initzialisieren der I2C (TWI) Schnittstelle |
void i2c_init(void) |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
//############################################################################ |
//Start I2C |
char i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
return(0); |
} |
//############################################################################ |
//Start I2C |
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); |
} |
//############################################################################ |
//Start I2C |
char i2c_write_byte(char byte) |
//############################################################################ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
return(0); |
} |
//############################################################################ |
//Start I2C |
SIGNAL (TWI_vect) |
//############################################################################ |
{ |
switch (twi_state++) |
{ |
case 0: |
i2c_write_byte(0x52+(motor*2)); |
break; |
case 1: |
switch(motor++) |
{ |
case 0: |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 2: |
i2c_stop(); |
if (motor<4) twi_state = 0; |
else motor = 0; |
i2c_start(); |
break; |
//Liest Daten von Motor |
case 3: |
i2c_write_byte(0x53+(motorread*2)); |
break; |
case 4: |
switch(motorread) |
{ |
case 0: |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 5: //1 Byte vom Motor lesen |
motor_rx[motorread] = TWDR; |
case 6: |
switch(motorread) |
{ |
case 0: |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 7: //2 Byte vom Motor lesen |
motor_rx[motorread+4] = TWDR; |
motorread++; |
if (motorread>3) motorread=0; |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
break; |
case 8: // Gyro-Offset |
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; |
} |
TWCR |= 0x80; |
} |
/branches/thjac/V1_11/twimaster.h |
---|
0,0 → 1,33 |
/*############################################################################ |
############################################################################*/ |
#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; |
extern unsigned char motorread; |
extern unsigned char motor_rx[8]; |
void i2c_reset(void); |
extern void i2c_init (void); // I2C initialisieren |
extern char i2c_start (void); // Start I2C |
extern void i2c_stop (void); // Stop I2C |
extern char i2c_write_byte (char byte); // 1 Byte schreiben |
extern void i2c_reset(void); |
#endif |
/branches/thjac/V1_11/uart.c |
---|
0,0 → 1,533 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 char DebugTextAnforderung = 255; |
unsigned char PcZugriff = 100; |
unsigned char MotorTest[4] = {0,0,0,0}; |
unsigned char MeineSlaveAdresse = 1; // Flight-Ctrl |
unsigned char ConfirmFrame; |
struct str_DebugOut DebugOut; |
struct str_ExternControl ExternControl; |
struct str_VersionInfo VersionInfo; |
struct str_WinkelOut WinkelOut; |
int Debug_Timer,Kompass_Timer; |
unsigned int DebugDataIntervall = 200; |
const unsigned char ANALOG_TEXT[32][16] = |
{ |
//1234567890123456 |
"IntegralNick ", //0 |
"IntegralRoll ", |
"AccNick ", |
"AccRoll ", |
"GyroGier ", |
"HoehenWert ", //5 |
"AccZ ", |
"Gas ", |
"KompassValue ", |
"Spannung ", |
"Empfang ", //10 |
"Ersatzkompass ", |
"Motor_Vorne ", |
"Motor_Hinten ", |
"Motor_Links ", |
"Motor_Rechts ", //15 |
" ", |
"Distance ", |
"OsdBar ", |
"MK3Mag CalState ", |
"Servo ", //20 |
"Nick ", |
"Roll ", |
" ", |
" ", |
" ", //25 |
"StickValue ", |
"Altitude N ", |
"PitchCount ", |
"PitchOffset ", |
"IntegralAlt ", //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 |
} |
} |
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 |
memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
//while(!UebertragungAbgeschlossen); |
//SendOutData('T', MeineSlaveAdresse, 0); |
PcZugriff = 255; |
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); |
} |
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 |
memcpy(&MotorTest[0], (unsigned char *)pRxData, sizeof(MotorTest)); |
while(!UebertragungAbgeschlossen); |
SendOutData('T', MeineSlaveAdresse, 0); |
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 '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; |
} |
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 / 108); // etwa in 0,1 Grad |
WinkelOut.Winkel[1] = (int) (IntegralRoll / 108); // 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(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/thjac/V1_11/uart.h |
---|
0,0 → 1,112 |
#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 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 DecodeNMEA(void); |
extern void BearbeiteRxDaten(void); |
extern unsigned char MotorTest[4]; |
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_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_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 |
#endif //_UART_H |
/branches/thjac/V1_11/version.txt |
---|
0,0 → 1,230 |
------- |
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 |
Baseline V0.71h |
V1_10a T. Jachmann |
- Neutralstellung des Gasknüppels mit autom. Aktivierung der Höhenregelung in Neutralstellung |
- Automatische Kalibrierung (Ermittlung der Neutralstellung) des Gas-Sticks beim Einschalten der FC |
- Bleibt der Knüppel für ca. 0,5 in Neutralstellung, wird die autom. Höhenregelung aktiviert |
- UserParam1 regelt die Empfindlichkeit der initialen 'Loslasserkennung' des Gasknüppels (Achtung: Wert mittels 'MK in der Hand' ermitteln!!!) |
- Höhenbegrenzung entfernt und neue Höhenregelung implementiert |
- MaxHoehe dient jetzt der Begrenzung des Fehlerwertes des Reglers |
- Looping-Funktion entfernt |
- Rotation des Nick/Roll-Sticks um 45°, um ohne Hardware-Anpassung in X-Formation zu fliegen |
- Anpassung der Servo-Nick-Kompensation auf X-Formation |
- Einschalten der LED-Ausgänge beim Starten der Motoren |
- Einführung eines zweiten MinGas-Parameters für Leerlaufgas nach Einschalten der Motoren |
V1_10b T. Jachmann |
- MK wird bei eingeschalteten Motoren mit Stick rechts unten nicht kalibriert |
- Die J16/J17-Frequenz kann bei Unterspannungswarnung erhöht werden |
- Die Pegel von J16/J17 lassen sich invertieren |
- Auch bei ausgeschalteten LED's können diese im Unterspannungsfall zwanghaft aktiviert werden |
V1_10c K. Rheinwald |
- User Parameter Defaults werden gem. Doku gesetzt |
- Die Pegel von J16/J17 werden auch im 'Aus'-Zustand invertiert |
- J16/J17 können getrennt voneinander invertiert werden |
- Einfache PWM auf J16/17 (0,20,40,60,80,100%, getrennt voneinander einstellbar) |
V1_10d K. Rheinwald |
- PWM feiner abgestuft (0-100% in 10% Schritten, getrennt voneinander einstellbar) |
- Timing von J16/17 von 10 auf 20ms verdoppelt -> Timings im MK-Tool müssen halbiert werden |
- PWM funktioniert auch für Timing über PotiX |
- Reihenfolge der User Parameters and Bitfields aufgeräumt - Settings checken! |
- User Parameter Defaults werden bei Initialisierung auf 0 gesetzt |
- Einige Warnings bereinigt |
--- |
V1_11a K. Rheinwald - Developer Version only! Do not fly! |
- Poti-Offset auf +-125 korrigiert |
- Gas-Offset auf 125 korrigiert |
- Einige Module gePrettyPrinted |
- Gas Modus wird wirklich automatisch geprüft und nicht forciert. |
- Merge with altcon.c added by T. Jachmann |
- picth_XX ausgelagert |