/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/Flight-Ctrl_MEGA644_V0_64.aps |
---|
0,0 → 1,0 |
<AVRStudio><MANAGEMENT><ProjectName>Flight-Ctrl_MEGA644_V0_64</ProjectName><Created>19-Aug-2007 14:38:13</Created><LastEdit>15-Oct-2007 19:44:55</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>19-Aug-2007 14:38:13</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>Flight-Ctrl_MEGA644_V0_64.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>E:\hubi\ufo\mickrokopter\eigene versionen\Kompass + GPS\Flight-Ctrl_V0_64_3_GPS_work_Jochenaccvonhand\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega644.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>compass.c</SOURCEFILE><HEADERFILE>_Settings.h</HEADERFILE><HEADERFILE>analog.h</HEADERFILE><HEADERFILE>fc.h</HEADERFILE><HEADERFILE>gps.h</HEADERFILE><HEADERFILE>main.h</HEADERFILE><HEADERFILE>menu.h</HEADERFILE><HEADERFILE>old_macros.h</HEADERFILE><HEADERFILE>printf_P.h</HEADERFILE><HEADERFILE>rc.h</HEADERFILE><HEADERFILE>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE><HEADERFILE>uart.h</HEADERFILE><HEADERFILE>compass.h</HEADERFILE><OTHERFILE>makefile</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>YES</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE>makefile</EXTERNALMAKEFILE><PART>atmega644</PART><HEX>1</HEX><LIST>0</LIST><MAP>0</MAP><OUTPUTFILENAME>Flight-Ctrl_MEGA644_V0_60.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS/><INCDIRS/><LIBDIRS/><LIBS/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -O0 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\WinAVR-20070525\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\WinAVR-20070525\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>main.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>compass.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>compass.h</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>fc.c</FileName><Status>1</Status></File00003></Files><Workspace><File00000><Position>346 108 878 484</Position><LineCol>155 3</LineCol></File00000><File00001><Position>355 114 901 482</Position><LineCol>0 0</LineCol></File00001><File00002><Position>377 136 923 504</Position><LineCol>0 0</LineCol></File00002><File00003><Position>336 71 1048 624</Position><LineCol>834 19</LineCol><State>Maximized</State></File00003></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio> |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/GPS.c |
---|
0,0 → 1,300 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 10.2007 by Jochen Kromayer |
// + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl. |
// + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
#include "math.h" |
//Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
//verwendet werden kann. d.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
const int c_sin[] = {0,17,34,52,69,87,104,121,139,156,173,190, |
207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,559,573,587,601,615,629, |
642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,882,891,898,906,913,920, |
927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999,1000,999,999,998,997,996, |
994,992,990,987,984,981,978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838, |
829,819,809,798,788,777,766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484, |
469,453,438,422,406,390,374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17, |
-34,-52,-69,-87,-104,-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406, |
-422,-438,-453,-469,-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719, |
-731,-743,-754,-766,-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927, |
-933,-939,-945,-951,-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000, |
-999,-999,-998,-997,-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927, |
-920,-913,-906,-898,-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719, |
-707,-694,-682,-669,-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406, |
-390,-374,-358,-342,-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17}; |
//Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
//verwendet werden kann. D.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
const int c_cos[] = {1000,999,999,998,997,996,994,992,990,987,984,981, |
978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,829,819,809,798,788,777, |
766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,469,453,438,422,406,390, |
374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,-34,-52,-69,-87,-104, |
-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,-422,-438,-453,-469, |
-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,-731,-743,-754,-766, |
-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,-933,-939,-945,-951, |
-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,-999,-999,-998,-997, |
-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,-920,-913,-906,-898, |
-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,-707,-694,-682,-669, |
-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,-390,-374,-358,-342, |
-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17,0,17,34,52,69,87, |
104,121,139,156,173,190,207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544, |
559,573,587,601,615,629,642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874, |
882,891,898,906,913,920,927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999}; |
long Soll_Position_North = 0; |
long Soll_Position_East = 0; |
long GPS_Positionsabweichung_North = 0; |
long GPS_Positionsabweichung_East = 0; |
long GPS_Geschwindigkeit_North = 0; |
long GPS_Geschwindigkeit_East = 0; |
long P_Einfluss_North = 0; |
long D_Einfluss_North = 0; |
long P_Einfluss_East = 0; |
long D_Einfluss_East = 0; |
signed int GPS_North = 0; |
signed int GPS_East = 0; |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
volatile char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr) |
long GPS_Home_North = 0; |
long GPS_Home_East = 0; |
unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr) |
//**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG |
// Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet |
#define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert |
volatile char NeuerMittelwert = 0; //Flag ob ein neuer Mittelwert vorliegt, der mit 4 Hz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen |
volatile unsigned char FilterPos = 0; |
volatile long NORTH_MITTEL = 0; |
volatile long EAST_MITTEL = 0; |
volatile long long NORTH_LONG_LONG = 0; |
volatile long long EAST_LONG_LONG = 0; |
volatile int filterfilled = 0; |
typedef struct { |
long NORTH[MITTELWERTZAHL]; |
} FILTER_NORTH; // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger |
typedef struct { |
long EAST[MITTELWERTZAHL]; |
} FILTER_EAST; // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger |
FILTER_NORTH filter_north; |
FILTER_EAST filter_east; |
//**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG |
void gps_main(void) |
{ |
//**************************** MIRCOS FILTERUNG ANFANG |
// if(actualPos.GPSFix == 3 && actualPos.newData == 1) //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen |
if(actualPos.GPSFix == 3) // hier auskommentieren, wenn dauernd (also nicht nur alle 250ms) die GPS-Berechnung durchgeführt werden soll |
{ |
filter_north.NORTH[FilterPos] = actualPos.northing; |
filter_east.EAST[FilterPos] = actualPos.easting; |
FilterPos++; |
if (FilterPos==MITTELWERTZAHL) FilterPos=0; |
if (filterfilled<MITTELWERTZAHL) filterfilled++; |
NORTH_LONG_LONG = 0; |
EAST_LONG_LONG = 0; |
for (int i=0; i<filterfilled; i++) |
{ |
NORTH_LONG_LONG += filter_north.NORTH[i]; |
EAST_LONG_LONG += filter_east.EAST[i]; |
} |
NORTH_LONG_LONG /= filterfilled; |
EAST_LONG_LONG /= filterfilled; |
NORTH_MITTEL = NORTH_LONG_LONG; |
EAST_MITTEL = EAST_LONG_LONG; |
NeuerMittelwert = 1; |
//actualPos.newData = 0; // hier auskommentieren, wenn dauernd (also nicht nur alle 250ms) die GPS-Berechnung durchgeführt werden soll |
} |
//******************************* MIRCOS FILTERUNG ENDE |
//wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle |
//auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert |
//(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird |
if (actualPos.GPSFix < 3 && Poti3 > 70) |
{ |
GPS_Nick = 0; |
GPS_Roll = 0; |
LED2_OFF; |
beeptime = 50; |
} |
// speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde |
// (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen |
// irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist |
if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000) |
{ |
GPS_Home_North = NORTH_MITTEL; |
GPS_Home_East = EAST_MITTEL; |
// Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in |
// Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde |
// GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde |
Soll_Position_North = NORTH_MITTEL; |
Soll_Position_East = EAST_MITTEL; |
beeptime = 50; |
gethome = 1; |
} |
//löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr) |
if (MotorenEin == 0) |
{ |
GPS_Home_North = 0; |
GPS_Home_East = 0; |
gethome = 0; |
modell_fliegt_gps = 0; |
} |
// wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht |
if(Poti3 < 70) |
{ |
// Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion |
// über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen. |
Soll_Position_North = NORTH_MITTEL; |
Soll_Position_East = EAST_MITTEL; |
GPS_Nick = 0; |
GPS_Roll = 0; |
LED2_OFF; |
} |
// wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt |
if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > 12 || StickNick < -12 || StickRoll > 12 || StickRoll < -12)) |
{ |
GPS_Nick = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
GPS_Roll = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
Soll_Position_North = NORTH_MITTEL; // Soll-Position wird geschrieben |
Soll_Position_East = EAST_MITTEL; // Soll-Position wird geschrieben |
//++++++++++++++++++++++++++++++++++++ |
//Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt |
//++++++++++++++++++++++++++++++++++++ |
if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char |
{ |
LED2_FLASH; |
blinkcount_LED2 = 0; |
} |
blinkcount_LED2++; |
//++++++++++++++++++++++++++++++++++++ |
} |
// wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert |
if (actualPos.GPSFix == 3 && Poti3 > 70 && NeuerMittelwert == 1 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= 12 && StickNick >= -12 && StickRoll <= 12 && StickRoll >= -12) |
{ |
//Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING) |
//DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen |
if(Poti3 < 150) |
{ |
GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
LED2_ON; |
} |
//andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt |
//werden konnten, wird zur GPS_Home_Position geflogen |
if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0) |
{ |
GPS_Positionsabweichung_North = (GPS_Home_North - NORTH_MITTEL); |
GPS_Positionsabweichung_East = (GPS_Home_East - EAST_MITTEL); |
// durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten |
// von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt |
Soll_Position_North = NORTH_MITTEL; |
Soll_Position_East = EAST_MITTEL; |
LED2_ON; |
} |
//verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition |
//die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen |
//jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet. |
if (Poti3 >= 150 && GPS_Home_North == 0 && GPS_Home_East == 0) |
{ |
GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
LED2_ON; |
beeptime = 50; |
} |
//GPS-Geschwindigkeiten |
GPS_Geschwindigkeit_North = actualPos.velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung |
GPS_Geschwindigkeit_East = actualPos.velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung |
//Berechnung der Einzelkomponenten des PD-Reglers |
P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North)*0.0005; //entspricht einer Skalierung von 1/2000 |
D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North)*0.00333; //entspricht einer Skalierung von 1/300 |
P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East)*0.0005; //entspricht einer Skalierung von 1/2000 |
D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East)*0.00333; //entspricht einer Skalierung von 1/300 |
// Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden, |
// dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat. |
// Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab. |
// Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>2,5m) von der Home-Position weg befindet. |
if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 250 || abs(GPS_Positionsabweichung_East) > 250)) |
{ |
if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil; |
if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil; |
} |
// PD-Regler |
GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
GPS_East = (P_Einfluss_East - D_Einfluss_East); |
//Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem |
GPS_Nick = (-c_sin[KompassValue]*0.001*GPS_East + c_cos[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/0.01745329251) ersetzt werden |
GPS_Roll = (c_cos[KompassValue]*0.001*GPS_East + c_sin[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/180*3.1415926535) ersetzt werden |
// Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
if (GPS_Nick > 35) GPS_Nick = 35; |
if (GPS_Roll > 35) GPS_Roll = 35; |
if (GPS_Nick < -35) GPS_Nick = -35; |
if (GPS_Roll < -35) GPS_Roll = -35; |
//Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
//FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |
//GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT |
NeuerMittelwert = 0; |
} |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/MIKROKOPTER_TO_DO.txt |
---|
0,0 → 1,11 |
. Patricks neigungskompensierten Kompass |
. Aufruf der GPS.c innerhalb der fc.c nur wenn neue Daten vorliegen |
. innerhalb der GPS.c den PD-Regler nicht mit "NeuerMittelwert", sondern mit "actualpos.newdata" alle 250ms aufrufen |
. GPS-Empfänger auf 5Hz einstellen |
. Begrenzung von 35 auf 50 anheben, damit auch bei stärkerem Wind geflogen werden kann |
. Waypoints |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/Parameter.txt |
---|
0,0 → 1,15 |
bei leichtem, konstantem Wind |
P-Faktor(Userparam_1): 120 |
D-Faktor(Userparam_2): 90 |
D-Anteil_Limit(Userparam_3): 45 |
bei stärkerem, böhigem Wind: |
P-Faktor(Userparam_1): 150 |
D-Faktor(Userparam_2): 95 |
D-Anteil_Limit(Userparam_3): 45 |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/Setting_1.mkp |
---|
0,0 → 1,51 |
[Setup] |
Name=GPS |
GlobalConfig=43 |
[Channels] |
Gas=7 |
Gier=6 |
Nick=5 |
Roll=8 |
Poti_1=1 |
Poti_2=2 |
Poti_3=3 |
Poti_4=4 |
[Stick] |
Nick_Roll-P=4 |
Nick_Roll-D=0 |
Gier-P=16 |
[Altitude] |
Setpoint=254 |
MinGas=30 |
P=10 |
Barometric-D=50 |
Z-ACC-Effect=50 |
Gain=2 |
[Gyro] |
P=120 |
I=150 |
ACC_Gyro-Factor=26 |
Main-I=0 |
[Others] |
MinGas=5 |
MaxGas=250 |
Compass-Effect=64 |
UnderVoltage=95 |
NotGas=66 |
NotGasTime=60 |
[User] |
Parameter_1=251 |
Parameter_2=252 |
Parameter_3=35 |
Parameter_4=60 |
[Camera] |
ServoNickControl=100 |
ServoNickCompensation=40 |
ServoNickInvert=0 |
ServoNickMin=50 |
ServoNickMax=150 |
ServoNickRefreshRate=5 |
[Loop] |
Config=0 |
GasLimit=50 |
StickThreshold=90 |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/Settings.h |
---|
--- _Settings.h (nonexistent) |
+++ _Settings.h (revision 308) |
@@ -0,0 +1,50 @@ |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Testmodi |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define MOTOR_OFF 0 |
+#define MOTOR_TEST 0 |
+ |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Abstimmung |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define ACC_AMPLIFY 16 |
+#define FAKTOR_P 1 |
+#define FAKTOR_I 0.0001 |
+ |
+ |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Debug-Interface |
+// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define SIO_DEBUG 1 // Soll der Debugger aktiviert sein? |
+#define MIN_DEBUG_INTERVALL 500 // 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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/analog.c |
---|
0,0 → 1,172 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0; |
volatile int accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 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; |
volatile int HoeheD = 0; |
volatile char messanzahl_Druck; |
volatile int tmpLuftdruck; |
volatile unsigned int ZaehlMessungen = 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; |
Delay_ms(100); |
if(MessLuftdruck < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms(50); |
printf("."); |
if(MessLuftdruck < 900) break; |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off); |
DruckOffsetSetting = off; |
Delay_ms(300); |
} |
//####################################################################################### |
// |
SIGNAL(SIG_ADC) |
//####################################################################################### |
{ |
static unsigned char kanal=0,state = 0; |
signed int wert; |
ANALOG_OFF; |
switch(state++) |
{ |
case 0: |
wert = (signed int) AdNeutralGier - ADC; |
if(PlatinenVersion != 10) wert *= 2; |
AccumulateGier += wert; // |
MessanzahlGier++; |
Mess_Integral_Gier += wert;// / 16; |
Mess_Integral_Gier2 += wert; |
kanal = 1; |
ZaehlMessungen++; |
break; |
case 1: |
wert = (signed int) ADC - AdNeutralRoll; |
if(PlatinenVersion != 10) wert *= 2; |
Mess_IntegralRoll += wert; |
Mess_IntegralRoll2 += wert; |
if(Mess_IntegralRoll > 310000L) Mess_IntegralRoll = -290000L; |
if(Mess_IntegralRoll <-310000L) Mess_IntegralRoll = 290000L; |
if(ADC < 15) wert = -1000; |
if(ADC < 7) wert = -2000; |
if(ADC > 1010) wert = +1000; |
if(ADC > 1017) wert = +2000; |
AccumulateRoll += wert; |
MessanzahlRoll++; |
kanal = 2; |
break; |
case 2: |
wert = (signed int) ADC - AdNeutralNick; |
if(PlatinenVersion != 10) wert *= 2; |
Mess_IntegralNick += wert; |
Mess_IntegralNick2 += wert; |
if(Mess_IntegralNick > 310000L) Mess_IntegralNick = -290000L; |
if(Mess_IntegralNick <-310000L) Mess_IntegralNick = 290000L; |
if(ADC < 15) wert = -1000; |
if(ADC < 7) wert = -2000; |
if(ADC > 1010) wert = +1000; |
if(ADC > 1017) wert = +2000; |
AccumulateNick += wert; |
MessanzahlNick++; |
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; |
accumulate_AccRoll += Aktuell_ay; |
messanzahl_AccRoll++; |
kanal = 7; |
break; |
case 5: |
Aktuell_ax = ADC - NeutralAccX; |
accumulate_AccNick += Aktuell_ax; |
messanzahl_AccNick++; |
kanal = 5; |
state = 6; |
break; |
case 6: |
accumulate_AccHoch = (signed int) ADC - NeutralAccZ; |
accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4; |
if(accumulate_AccHoch > 1) |
{ |
if(NeutralAccZ < 800) NeutralAccZ+= 0.02; |
} |
else if(accumulate_AccHoch < -1) |
{ |
if(NeutralAccZ > 600) NeutralAccZ-= 0.02; |
} |
messanzahl_AccHoch = 1; |
Aktuell_az = ADC; |
Mess_Integral_Hoch += accumulate_AccHoch; // Integrieren |
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen |
// Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen |
/* if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
{ |
kanal = 3; |
state = 7; |
} |
else |
{ |
kanal = 0; |
state = 0; |
}*/ |
kanal = 3; |
state = 7; |
break; |
case 7: |
tmpLuftdruck += ADC; |
if(++messanzahl_Druck >= 5) |
{ |
MessLuftdruck = ADC; |
messanzahl_Druck = 0; |
HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert); // D-Anteil = neuerWert - AlterWert |
Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4; |
HoehenWert = StartLuftdruck - Luftdruck; |
tmpLuftdruck = 0; |
} |
kanal = 0; |
state = 0; |
break; |
default: |
kanal = 0; |
state = 0; |
break; |
} |
ADMUX = kanal; |
ANALOG_ON; |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/analog.h |
---|
0,0 → 1,23 |
/*####################################################################################### |
#######################################################################################*/ |
extern volatile int UBat; |
extern volatile int AccumulateNick, AccumulateRoll, AccumulateGier,accumulate_AccRoll,accumulate_AccNick,accumulate_AccHoch; |
extern volatile char MessanzahlNick, MessanzahlRoll, MessanzahlGier,messanzahl_AccNick, messanzahl_AccRoll,messanzahl_AccHoch; |
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 volatile int HoeheD; |
extern volatile unsigned int MessLuftdruck; |
extern volatile int StartLuftdruck; |
extern unsigned int ReadADC(unsigned char adc_input); |
extern void ADC_Init(void); |
extern void SucheLuftruckOffset(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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/compass.c |
---|
0,0 → 1,150 |
/* |
Copyright 2007, Niklas Nold |
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify |
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
either version 3 of the License, or (at your option) any later version. |
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
*/ |
#include "main.h" |
MM3_struct MM3; |
//############################################################################ |
//Initialisierung der SPI-Schnittstelle |
void init_spi(void) |
//############################################################################ |
{ |
SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
//SPSR = (1<<SPI2X); |
DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
PORTD &= ~(1<<PD3); // J5 auf Low |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
} |
//############################################################################ |
//Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen |
void MM3_timer0(void) |
//############################################################################ |
{ |
switch (MM3.STATE) |
{ |
case MM3_RESET: |
PORTB |= (1<<PB2); // J8 auf High, MM3 Reset |
MM3.STATE = MM3_START_TRANSFER; |
return; |
case MM3_START_TRANSFER: |
PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High) |
if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
MM3.STATE = MM3_WAIT_DRDY; |
return; |
case MM3_WAIT_DRDY: |
if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
MM3.NickGrad = asin_i((float)Aktuell_ax/EE_Parameter.UserParam1*200); |
MM3.RollGrad = asin_i((float)Aktuell_ay/EE_Parameter.UserParam2*200); |
MM3.AXIS = MM3_X; |
MM3.STATE = MM3_RESET; |
return; |
} |
} |
//############################################################################ |
//SPI byte ready |
SIGNAL (SIG_SPI) |
//############################################################################ |
{ |
switch (MM3.STATE) |
{ |
case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken |
if (MM3.AXIS == MM3_X) |
{ |
MM3.x_axis = SPDR; |
MM3.x_axis <<= 8; |
} |
else if (MM3.AXIS == MM3_Y) |
{ |
MM3.y_axis = SPDR; |
MM3.y_axis <<= 8; |
} |
else // if (MM3.AXIS == MM3_Z) |
{ |
MM3.z_axis = SPDR; |
MM3.z_axis <<= 8; |
} |
SPDR=0x00; // Übertragung von 2. Byte auslösen |
MM3.STATE=MM3_BYTE2; |
return; |
case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
if (MM3.AXIS == MM3_X) |
{ |
MM3.x_axis |= SPDR; |
MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
MM3.AXIS = MM3_Y; |
MM3.STATE = MM3_RESET; |
} |
else if (MM3.AXIS == MM3_Y) |
{ |
MM3.y_axis |= SPDR; |
MM3.y_axis -= OFF_Y; |
MM3.AXIS = MM3_Z; |
MM3.STATE = MM3_RESET; |
} |
else // if (MM3.AXIS == MM3_Z) |
{ |
MM3.z_axis |= SPDR; |
MM3.z_axis -= OFF_Z; |
MM3.STATE = MM3_TILT; |
} |
return; |
} |
} |
signed int MM3_heading(void) |
{ |
float sin_nick, cos_nick, sin_roll, cos_roll; |
signed int x_corr, y_corr; |
signed int heading; |
// Berechung von sinus und cosinus |
sin_nick = sin_f(MM3.NickGrad); |
cos_nick = cos_f(MM3.NickGrad); |
sin_roll = sin_f(MM3.RollGrad); |
cos_roll = cos_f(MM3.RollGrad); |
// Neigungskompensation |
x_corr = (cos_nick * MM3.x_axis) + (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick); |
y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
// Winkelberechnung |
heading = atan2_i(x_corr, y_corr); |
return (heading); |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/compass.h |
---|
0,0 → 1,40 |
typedef struct |
{ |
uint8_t STATE; |
unsigned int DRDY; |
uint8_t AXIS; |
signed int x_axis; |
signed int y_axis; |
signed int z_axis; |
signed int NickGrad; |
signed int RollGrad; |
}MM3_struct; |
extern MM3_struct MM3; |
void init_spi(void); |
void MM3_timer0(void); |
signed int MM3_heading(void); |
#define Int2Grad_Faktor 1024 |
// Die Werte der Statemachine |
#define MM3_RESET 0 |
#define MM3_START_TRANSFER 1 |
#define MM3_WAIT_DRDY 2 |
#define MM3_DRDY 4 |
#define MM3_BYTE2 8 |
#define MM3_X 16 |
#define MM3_Y 32 |
#define MM3_Z 64 |
#define MM3_TILT 128 |
// Die Werte der Kompasskalibrierung |
// OFFSET: (Maximaler Wert + minimaler Wert) / 2 |
#define OFF_X 0 |
#define OFF_Y 0 |
#define OFF_Z 0 |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/eeprom.c |
---|
--- fc.c (nonexistent) |
+++ fc.c (revision 308) |
@@ -0,0 +1,1034 @@ |
+/*####################################################################################### |
+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" |
+ |
+unsigned char h,m,s; |
+volatile unsigned int I2CTimeout = 100; |
+volatile int MesswertNick,MesswertRoll,MesswertGier; |
+volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
+volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
+volatile float NeutralAccZ = 0; |
+unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
+volatile long IntegralNick = 0,IntegralNick2 = 0; |
+volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
+volatile long Integral_Gier = 0; |
+volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
+volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
+volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
+volatile long Mess_Integral_Hoch = 0; |
+volatile int KompassValue = 0; |
+volatile int KompassStartwert = 0; |
+volatile int KompassRichtung = 0; |
+unsigned char MAX_GAS,MIN_GAS; |
+unsigned char Notlandung = 0; |
+unsigned char HoehenReglerAktiv = 0; |
+ |
+unsigned char blinkcount_LED1 = 0;//Hilfszähler für die blinkende LED (310807Kr) |
+unsigned int modell_fliegt_gps = 0;//(030907Kr) |
+unsigned char Limit_D_Anteil = 0;//(210907Kr) |
+ |
+float GyroFaktor; |
+float IntegralFaktor; |
+ |
+volatile int DiffNick,DiffRoll; |
+int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; //PPM24-Erweiterung (121007Kr) |
+volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
+unsigned char MotorWert[5]; |
+volatile unsigned char SenderOkay = 0; |
+int StickNick = 0,StickRoll = 0,StickGier = 0; |
+char MotorenEin = 0; |
+int HoehenWert = 0; |
+int SollHoehe = 0; |
+unsigned char Looping_Nick = 0,Looping_Roll = 0; |
+ |
+float Kp = FAKTOR_P; |
+float Ki = FAKTOR_I; |
+ |
+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_ServoNickControl = 100; |
+unsigned char Parameter_LoopGasLimit = 70; |
+struct mk_param_struct EE_Parameter; |
+ |
+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) |
+//############################################################################ |
+{ |
+ unsigned int timer; |
+ NeutralAccX = 0; |
+ NeutralAccY = 0; |
+ NeutralAccZ = 0; |
+ AdNeutralNick = 0; |
+ AdNeutralRoll = 0; |
+ AdNeutralGier = 0; |
+ CalibrierMittelwert(); |
+ timer = SetDelay(5); |
+ while (!CheckDelay(timer)); |
+ CalibrierMittelwert(); |
+ if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
+ { |
+ if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
+ } |
+ |
+ if(PlatinenVersion == 10) |
+ { |
+ AdNeutralNick= abs(MesswertNick); |
+ AdNeutralRoll= abs(MesswertRoll); |
+ AdNeutralGier= abs(MesswertGier); |
+ } |
+ else |
+ { |
+ AdNeutralNick= abs(MesswertNick) / 2; |
+ AdNeutralRoll= abs(MesswertRoll) / 2; |
+ AdNeutralGier= abs(MesswertGier) / 2; |
+ } |
+ NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
+ NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
+ NeutralAccZ = Aktuell_az; |
+ |
+ Mess_IntegralNick = 0; |
+ Mess_IntegralNick2 = 0; |
+ Mess_IntegralRoll = 0; |
+ Mess_IntegralRoll2 = 0; |
+ Mess_Integral_Gier = 0; |
+ MesswertNick = 0; |
+ MesswertRoll = 0; |
+ MesswertGier = 0; |
+ StartLuftdruck = Luftdruck; |
+ HoeheD = 0; |
+ Mess_Integral_Hoch = 0; |
+ KompassStartwert = KompassValue; |
+ beeptime = 50; |
+} |
+ |
+//############################################################################ |
+// Bildet den Mittelwert aus den Messwerten |
+void Mittelwert(void) |
+//############################################################################ |
+{ |
+ // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
+ ANALOG_OFF; |
+ if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
+ if(MessanzahlRoll) (MesswertRoll = AccumulateRoll / MessanzahlRoll); |
+ if(MessanzahlGier) (MesswertGier = AccumulateGier / MessanzahlGier); |
+ if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L; |
+ if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L; |
+ if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L; |
+ AccumulateNick = 0; MessanzahlNick = 0; |
+ AccumulateRoll = 0; MessanzahlRoll = 0; |
+ AccumulateGier = 0; MessanzahlGier = 0; |
+ accumulate_AccRoll = 0;messanzahl_AccRoll = 0; |
+ accumulate_AccNick = 0;messanzahl_AccNick = 0; |
+ accumulate_AccHoch = 0;messanzahl_AccHoch = 0; |
+ Integral_Gier = Mess_Integral_Gier; |
+// Integral_Gier2 = Mess_Integral_Gier2; |
+ IntegralNick = Mess_IntegralNick; |
+ IntegralRoll = Mess_IntegralRoll; |
+ IntegralNick2 = Mess_IntegralNick2; |
+ IntegralRoll2 = Mess_IntegralRoll2; |
+ // ADC einschalten |
+ ANALOG_ON; |
+ |
+/* |
+//------------------------------------------------------------------------------ |
+ 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); |
+//------------------------------------------------------------------------------ |
+*/ |
+ 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--; |
+ //PPM24-Erweiterung (121007Kr) |
+ if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
+ if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
+ if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
+ if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
+ |
+ |
+ if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
+ if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
+ if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
+ if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
+ //PPM24-Erweiterung (121007Kr) |
+ if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
+ if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
+ if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
+ if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
+} |
+ |
+//############################################################################ |
+// Messwerte beim Ermitteln der Nullage |
+void CalibrierMittelwert(void) |
+//############################################################################ |
+{ |
+ // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
+ ANALOG_OFF; |
+ if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
+ if(MessanzahlRoll) (MesswertRoll = AccumulateRoll / MessanzahlRoll); |
+ if(MessanzahlGier) (MesswertGier = AccumulateGier / MessanzahlGier); |
+ if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick); |
+ if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll; |
+ if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch; |
+ AccumulateNick = 0; MessanzahlNick = 0; |
+ AccumulateRoll = 0; MessanzahlRoll = 0; |
+ AccumulateGier = 0; MessanzahlGier = 0; |
+ accumulate_AccRoll = 0;messanzahl_AccRoll = 0; |
+ accumulate_AccNick = 0;messanzahl_AccNick = 0; |
+ accumulate_AccHoch = 0;messanzahl_AccHoch = 0; |
+ // ADC einschalten |
+ ANALOG_ON; |
+ if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
+ if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
+ if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
+ if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
+ //PPM24-Erweiterung (121007Kr) |
+ if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
+ if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
+ if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
+ if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
+ |
+ |
+ |
+ |
+ if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
+ if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
+ if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
+ if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
+ //PPM24-Erweiterung (121007Kr) |
+ if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
+ if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
+ if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
+ if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
+} |
+ |
+//############################################################################ |
+// Senden der Motorwerte per I2C-Bus |
+void SendMotorData(void) |
+//############################################################################ |
+{ |
+ if(MOTOR_OFF || !MotorenEin) |
+ { |
+ Motor_Hinten = 0; |
+ Motor_Vorne = 0; |
+ Motor_Rechts = 0; |
+ Motor_Links = 0; |
+ if(MotorTest[0]) Motor_Vorne = MotorTest[0]; |
+ if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
+ if(MotorTest[2]) Motor_Links = MotorTest[2]; |
+ if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
+ } |
+ |
+ // DebugOut.Analog[12] = Motor_Vorne; |
+ // DebugOut.Analog[13] = Motor_Hinten; |
+ // DebugOut.Analog[14] = Motor_Links; |
+ // DebugOut.Analog[15] = Motor_Rechts; |
+ |
+ // DebugOut.Analog[12] = motor_rx[0]; //Motorstrom vorne |
+ // DebugOut.Analog[13] = motor_rx[1]; //Motorstrom hinten |
+ // DebugOut.Analog[14] = motor_rx[2]; //Motorstrom links |
+ // DebugOut.Analog[15] = motor_rx[3]; //Motorstrom rechts |
+ |
+ |
+ //Start I2C Interrupt Mode |
+ twi_state = 0; |
+ motor = 0; |
+ i2c_start(); |
+} |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// + Konstanten |
+// + 0-250 -> normale Werte |
+// + 251 -> Poti1 |
+// + 252 -> Poti2 |
+// + 253 -> Poti3 |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+void DefaultKonstanten1(void) |
+{ |
+ EE_Parameter.Kanalbelegung[K_NICK] = 1; |
+ EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
+ EE_Parameter.Kanalbelegung[K_GAS] = 3; |
+ EE_Parameter.Kanalbelegung[K_GIER] = 4; |
+ EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
+ EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
+ EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
+ EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
+ EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01; |
+ EE_Parameter.Hoehe_MinGas = 30; |
+ EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
+ EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
+ EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
+ EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
+ EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
+ EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
+ EE_Parameter.Stick_D = 8; //8 // Wert : 0-64 |
+ EE_Parameter.Gier_P = 16; // Wert : 1-20 |
+ EE_Parameter.Gas_Min = 5; // Wert : 0-32 // (160607Kr) |
+ EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
+ EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
+ EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
+ EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
+ EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
+ EE_Parameter.UnterspannungsWarnung = 95; // Wert : 0-250 // (160607Kr) |
+ EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
+ EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
+ EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
+ EE_Parameter.I_Faktor = 0; |
+ EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
+ EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
+ EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
+ EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
+ EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
+ EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
+ EE_Parameter.ServoNickRefresh = 5; |
+ EE_Parameter.LoopGasLimit = 50; |
+ EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
+ EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
+ memcpy(EE_Parameter.Name, "Sport\0", 12); |
+} |
+ |
+void DefaultKonstanten2(void) |
+{ |
+ EE_Parameter.Kanalbelegung[K_NICK] = 1; |
+ EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
+ EE_Parameter.Kanalbelegung[K_GAS] = 3; |
+ EE_Parameter.Kanalbelegung[K_GIER] = 4; |
+ EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
+ EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
+ EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
+ EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
+ EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
+ EE_Parameter.Hoehe_MinGas = 30; |
+ EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
+ EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
+ EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
+ EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
+ EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
+ EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
+ EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
+ EE_Parameter.Gier_P = 16; // Wert : 1-20 |
+ EE_Parameter.Gas_Min = 5; // Wert : 0-32 // (160607Kr) |
+ EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
+ EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
+ EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
+ EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
+ EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
+ EE_Parameter.UnterspannungsWarnung = 95; // Wert : 0-250 // (160607Kr) |
+ EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
+ EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
+ EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
+ EE_Parameter.I_Faktor = 0; |
+ EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
+ EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
+ EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
+ EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
+ EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
+ EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
+ EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
+ EE_Parameter.ServoNickRefresh = 5; |
+ EE_Parameter.LoopGasLimit = 50; |
+ EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
+ EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
+ memcpy(EE_Parameter.Name, "Kamera\0", 12); |
+} |
+ |
+ |
+//############################################################################ |
+// Trägt ggf. das Poti als Parameter ein |
+void ParameterZuordnung(void) |
+//############################################################################ |
+{ |
+ |
+ #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; if(b <= min) b = min; else if(b >= max) b = max;} |
+ CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
+ CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
+ CHK_POTI(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(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_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
+ CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
+ CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
+ CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,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,h,tmp_int; |
+ int GierMischanteil,GasMischanteil; |
+ static long SummeNick=0,SummeRoll=0; |
+ static long sollGier = 0,tmp_long,tmp_long2; |
+ long IntegralFehlerNick = 0; |
+ long IntegralFehlerRoll = 0; |
+ static unsigned int RcLostTimer; |
+ static unsigned char delay_neutral = 0; |
+ static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
+ static unsigned int modell_fliegt = 0; |
+ static int hoehenregler = 0; |
+ static char TimerWerteausgabe = 0; |
+ static char NeueKompassRichtungMerken = 0; |
+ Mittelwert(); |
+ |
+ GRN_ON; |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Gaswert ermitteln |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
+ if(GasMischanteil < 0) GasMischanteil = 0; |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Emfang schlecht |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(SenderOkay < 100) |
+ { |
+ if(!PcZugriff) |
+ { |
+ if(BeepMuster == 0xffff) |
+ { |
+ beeptime = 15000; |
+ BeepMuster = 0x0c00; |
+ } |
+ } |
+ if(RcLostTimer) RcLostTimer--; |
+ else |
+ { |
+ MotorenEin = 0; |
+ Notlandung = 0; |
+ } |
+ ROT_ON; |
+ if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
+ { |
+ GasMischanteil = EE_Parameter.NotGas; |
+ Notlandung = 1; |
+ 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) |
+ { |
+ if(modell_fliegt < 0xffff) modell_fliegt++; |
+ modell_fliegt_gps = modell_fliegt;// (030907Kr) |
+ } |
+ if((modell_fliegt < 200) || (GasMischanteil < 40)) |
+ { |
+ SummeNick = 0; |
+ SummeRoll = 0; |
+ Mess_Integral_Gier = 0; |
+ Mess_Integral_Gier2 = 0; |
+ } |
+ if((GasMischanteil > 200) && MotorenEin == 0) |
+ { |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// auf Nullwerte kalibrieren |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
+ { |
+ unsigned char setting; |
+ 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) |
+ { |
+ 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; |
+ eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
+ } |
+ if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
+ { |
+ if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
+ } |
+ ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
+ SetNeutral(); |
+ Piep(GetActiveParamSetNumber()); |
+ } |
+ } |
+ else delay_neutral = 0; |
+ } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Gas ist unten |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(GasMischanteil < 35) |
+ { |
+ // Starten |
+ if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
+ { |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Einschalten |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(++delay_einschalten > 200) |
+ { |
+ delay_einschalten = 200; |
+ modell_fliegt = 1; |
+ MotorenEin = 1; |
+ sollGier = 0; |
+ Mess_Integral_Gier = 0; |
+ Mess_Integral_Gier2 = 0; |
+ Mess_IntegralNick = 0; |
+ Mess_IntegralRoll = 0; |
+ Mess_IntegralNick2 = IntegralNick; |
+ Mess_IntegralRoll2 = IntegralRoll; |
+ SummeNick = 0; |
+ SummeRoll = 0; |
+ } |
+ } |
+ 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) |
+ { |
+ ParameterZuordnung(); |
+ StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P; |
+ StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
+ StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P; |
+ StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
+ StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
+ GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
+ IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
+ |
+ if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
+ if(GyroFaktor < 0) GyroFaktor = 0; |
+ if(IntegralFaktor < 0) IntegralFaktor = 0; |
+ // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
+ if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
+ { |
+ if(IntegralNick > 60000) |
+ { |
+ StickNick -= 8 * EE_Parameter.Stick_P; |
+ if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
+ } |
+ else |
+ if(IntegralNick < -60000) |
+ { |
+ StickNick += 8 * EE_Parameter.Stick_P; |
+ if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
+ } |
+ if(IntegralRoll > 60000) |
+ { |
+ StickRoll -= 8 * EE_Parameter.Stick_P; |
+ if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
+ } |
+ else |
+ if(IntegralRoll < -60000) |
+ { |
+ StickRoll += 8 * EE_Parameter.Stick_P; |
+ if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
+ } |
+ } |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// LED Stuff |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+LED1_OFF; |
+ |
+ if(blinkcount_LED1 >= Parameter_UserParam4 && Parameter_UserParam4 > 0) |
+ { |
+ LED1_FLASH; |
+ blinkcount_LED1 = 0; |
+ } |
+ |
+ blinkcount_LED1++; |
+ |
+ //if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] > 75) LED1_ON; else LED1_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
+ //if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 75) LED2_ON; else LED2_OFF; //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
+ } |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Looping? |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) || |
+ ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
+ { |
+ Looping_Roll = 1; |
+ if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
+ } |
+ else Looping_Roll = 0; |
+ |
+ if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) || |
+ ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN)) |
+ { |
+ Looping_Nick = 1; |
+ Looping_Roll = 0; |
+ if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
+ } |
+ else Looping_Nick = 0; |
+ |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Bei Empfangsausfall im Flug |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(Notlandung) |
+ { |
+ StickGier = 0; |
+ StickNick = 0; |
+ StickRoll = 0; |
+ GyroFaktor = 0.1; |
+ IntegralFaktor = 0.005; |
+ Looping_Roll = 0; |
+ Looping_Nick = 0; |
+ } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Gyro-Drift kompensieren |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define DRIFT_FAKTOR 3 |
+ if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
+ { |
+ IntegralFehlerNick = IntegralNick2 - IntegralNick; |
+ IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
+ ZaehlMessungen = 0; |
+ if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
+ if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
+ if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
+ if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
+// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
+// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
+ ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
+ Mess_IntegralNick2 = IntegralNick; |
+ Mess_IntegralRoll2 = IntegralRoll; |
+ Mess_Integral_Gier2 = Integral_Gier; |
+ ANALOG_ON; // ADC einschalten |
+ } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Integrale auf ACC-Signal abgleichen |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+ |
+ if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) |
+ { |
+ tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16; |
+ tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16; |
+ #define AUSGLEICH 500 |
+ 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; |
+ ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
+ Mess_IntegralNick -= tmp_long; |
+ Mess_IntegralRoll -= tmp_long2; |
+ } |
+ |
+ else |
+ { |
+ if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
+ { |
+ tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
+ if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8; |
+ else tmp_long /= 16; |
+ tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
+ if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8; |
+ else tmp_long2 /= 16; |
+ |
+ #define AUSGLEICH 500 |
+ 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; |
+ } |
+ |
+ else |
+ { |
+ tmp_long = 0; |
+ tmp_long2 = 0; |
+ } |
+ ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
+ Mess_IntegralNick -= tmp_long; |
+ Mess_IntegralRoll -= tmp_long2; |
+ } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Gieren |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(abs(StickGier) > 20) // war 35 |
+ { |
+ if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
+ } |
+ tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
+ tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
+ sollGier = tmp_int; |
+ Mess_Integral_Gier -= tmp_int; |
+ if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
+ if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
+ |
+ ANALOG_ON; // ADC einschalten |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Kompass |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+ if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
+ { |
+ if(NeueKompassRichtungMerken) |
+ { |
+ KompassStartwert = KompassValue; |
+ NeueKompassRichtungMerken = 0; |
+ } |
+ ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
+ Mess_Integral_Gier -= (KompassRichtung * Parameter_KompassWirkung) / 32; // nach Kompass ausrichten |
+ ANALOG_ON; // ADC einschalten |
+ } |
+ |
+ |
+// if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
+// { |
+// int w,v; |
+// static int SignalSchlecht = 0; |
+// w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
+// v = abs(IntegralRoll /512); |
+// if(v > w) w = v; // grösste Neigung ermitteln |
+// if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
+// { |
+// KompassStartwert = KompassValue; |
+// NeueKompassRichtungMerken = 0; |
+// } |
+// w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
+// w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
+// if(w > 0) |
+// { |
+// ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
+// if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
+// ANALOG_ON; // ADC einschalten |
+// if(SignalSchlecht) SignalSchlecht--; |
+// } |
+// else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
+// } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// GPS |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+if(EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) |
+{ |
+ P_GPS_Verstaerkung = Parameter_UserParam1; |
+ D_GPS_Verstaerkung = Parameter_UserParam2; |
+ Limit_D_Anteil = Parameter_UserParam3; |
+ gps_main(); |
+} |
+else |
+{ |
+ GPS_Nick = 0; |
+ GPS_Roll = 0; |
+} |
+ |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Debugwerte zuordnen |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ if(!TimerWerteausgabe--) |
+ { |
+ TimerWerteausgabe = 49; |
+ DebugOut.Analog[0] = MM3.NickGrad; |
+ DebugOut.Analog[1] = MM3.RollGrad; |
+ 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[7] = GasMischanteil; |
+ DebugOut.Analog[8] = KompassValue; |
+ |
+//DebugOut.Analog[8] = MM3_heading(); |
+DebugOut.Analog[9] = MM3.x_axis; |
+DebugOut.Analog[10] = MM3.y_axis; |
+DebugOut.Analog[11] = MM3.z_axis; |
+ |
+ |
+// DebugOut.Analog[0] = actualPos.GPSFix; |
+// DebugOut.Analog[1] = GPS_Roll; |
+// DebugOut.Analog[2] = GPS_Nick; |
+// DebugOut.Analog[3] = GPS_Positionsabweichung_North; |
+// DebugOut.Analog[4] = GPS_Positionsabweichung_East; |
+// DebugOut.Analog[5] = P_Einfluss_East; |
+// DebugOut.Analog[6] = D_Einfluss_East; |
+// DebugOut.Analog[7] = P_Einfluss_North; |
+// DebugOut.Analog[8] = D_Einfluss_North; |
+// DebugOut.Analog[9] = Soll_Position_North; |
+// DebugOut.Analog[10] = Soll_Position_East; |
+// DebugOut.Analog[11] = Parameter_UserParam1; |
+ DebugOut.Analog[12] = Parameter_UserParam2; |
+ DebugOut.Analog[13] = Poti3; |
+ DebugOut.Analog[14] = KompassValue; |
+ DebugOut.Analog[15] = GPS_Home_North; |
+ DebugOut.Analog[16] = GPS_Home_East; |
+ DebugOut.Analog[17] = NORTH_MITTEL; |
+ DebugOut.Analog[18] = EAST_MITTEL; |
+ DebugOut.Analog[19] = actualPos.northing; |
+ DebugOut.Analog[20] = actualPos.easting; |
+ DebugOut.Analog[21] = actualPos.velNorth; |
+ DebugOut.Analog[22] = actualPos.velEast; |
+ DebugOut.Analog[23] = UBat; |
+ DebugOut.Analog[24] = (motor_rx[0]+motor_rx[1]+motor_rx[2]+motor_rx[3]); //Gesamtstrom |
+ DebugOut.Analog[25] = StickRoll; |
+ DebugOut.Analog[26] = HoehenWert; |
+ DebugOut.Analog[27] = IntegralNick / EE_Parameter.GyroAccFaktor; |
+ DebugOut.Analog[28] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
+ DebugOut.Analog[29] = Mittelwert_AccNick; |
+ DebugOut.Analog[30] = Mittelwert_AccRoll; |
+ DebugOut.Analog[31] = StickNick; |
+ } |
+ |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// 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 * (GyroFaktor/2) + Integral_Gier * IntegralFaktor; |
+ // MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2; |
+ |
+ // Maximalwerte abfangen |
+ #define MAX_SENSOR 2048 |
+ 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; |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Höhenregelung |
+// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+//OCR0B = 180 - (Poti1 + 120) / 4; |
+//DruckOffsetSetting = OCR0B; |
+ if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
+ { |
+ int tmp_int; |
+ if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
+ { |
+ if(Parameter_MaxHoehe < 50) |
+ { |
+ SollHoehe = HoehenWert - 40; // Parameter_MaxHoehe ist der PPM-Wert des Schalters // (160607Kr) |
+ HoehenReglerAktiv = 0; |
+ } |
+ else |
+ HoehenReglerAktiv = 1; |
+ } |
+ else |
+ { |
+ SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 40; // (160607Kr) |
+ HoehenReglerAktiv = 1; |
+ } |
+ |
+ if(Notlandung) SollHoehe = 0; |
+ h = HoehenWert; |
+ if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
+ { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
+ h = GasMischanteil - h; // vom Gas abziehen |
+ h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
+ tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
+ if(tmp_int > 50) tmp_int = 50; |
+ else if(tmp_int < -50) tmp_int = -50; |
+ h -= tmp_int; |
+ hoehenregler = (hoehenregler*15 + h) / 16; |
+ if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN |
+ { |
+ if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas; |
+ if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil; |
+ } |
+ if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
+ GasMischanteil = hoehenregler; |
+ } |
+ } |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// + Mischer und PI-Regler |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ // DebugOut.Analog[7] = GasMischanteil; |
+ |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Gier-Anteil |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+#define MUL_G 1.0 |
+ GierMischanteil = MesswertGier - sollGier; // Regler für Gier |
+//GierMischanteil = 0; |
+ if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil; |
+ if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil); |
+ if(GierMischanteil > 100) GierMischanteil = 100; |
+ if(GierMischanteil < -100) GierMischanteil = -100; |
+ |
+ if(GasMischanteil < 20) GierMischanteil = 0; |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+// Nick-Achse |
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
+ |
+ DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
+ SummeNick += DiffNick; // I-Anteil |
+ if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
+ if(SummeNick > 16000) SummeNick = 16000; |
+ if(SummeNick < -16000) SummeNick = -16000; |
+ pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
+ // Motor Vorn |
+#define MUL 2 |
+ if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
+ if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
+ |
+ motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
+ 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; |
+ 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 = Kp * (MesswertRoll - (StickRoll - GPS_Roll)); // Differenz bestimmen |
+ SummeRoll += DiffRoll; // I-Anteil |
+ if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
+ if(SummeRoll > 16000) SummeRoll = 16000; |
+ if(SummeRoll < -16000) SummeRoll = -16000; |
+ pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
+ if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
+ if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
+ // Motor Links |
+ motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
+ if ((motorwert < 0)) motorwert = 0; |
+ else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
+ if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
+ Motor_Links = motorwert; |
+ // Motor Rechts |
+ motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
+ if ((motorwert < 0)) motorwert = 0; |
+ else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
+ if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
+ Motor_Rechts = motorwert; |
+ // +++++++++++++++++++++++++++++++++++++++++++++++ |
+} |
+ |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/fc.h |
---|
0,0 → 1,121 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
#ifndef _FC_H |
#define _FC_H |
extern volatile unsigned int I2CTimeout; |
extern unsigned char Sekunde,Minute; |
extern volatile long IntegralNick,IntegralNick2; |
extern volatile long IntegralRoll,IntegralRoll2; |
extern volatile long Mess_IntegralNick,Mess_IntegralNick2; |
extern volatile long Mess_IntegralRoll,Mess_IntegralRoll2; |
extern volatile long Mess_Integral_Hoch; |
extern volatile long Integral_Gier,Mess_Integral_Gier,Mess_Integral_Gier2; |
extern volatile int KompassValue; |
extern volatile int KompassStartwert; |
extern volatile int KompassRichtung; |
extern int HoehenWert; |
extern int SollHoehe; |
extern volatile int MesswertNick,MesswertRoll,MesswertGier; |
extern volatile int AdNeutralNick,AdNeutralRoll,AdNeutralGier, Mittelwert_AccNick, Mittelwert_AccRoll; |
extern volatile int NeutralAccX, NeutralAccY,Mittelwert_AccHoch; |
extern volatile float NeutralAccZ; |
extern unsigned int modell_fliegt_gps;//(030907Kr) |
extern unsigned char Limit_D_Anteil;//(210907Kr) |
void MotorRegler(void); |
void SendMotorData(void); |
void CalibrierMittelwert(void); |
void Mittelwert(void); |
void SetNeutral(void); |
unsigned char h,m,s; |
volatile unsigned char Timeout ; |
unsigned char CosinusNickWinkel, CosinusRollWinkel; |
volatile long IntegralNick,IntegralNick2; |
volatile long IntegralRoll,IntegralRoll2; |
volatile long Integral_Gier; |
volatile long Mess_IntegralNick,Mess_IntegralNick2; |
volatile long Mess_IntegralRoll,Mess_IntegralRoll2; |
volatile long Mess_Integral_Gier; |
volatile int DiffNick,DiffRoll; |
extern int Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; //PPM24-Erweiterung (121007Kr) |
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
unsigned char MotorWert[5]; |
volatile unsigned char SenderOkay; |
int StickNick,StickRoll,StickGier; |
char MotorenEin; |
extern void DefaultKonstanten(void); |
#define STRUCT_PARAM_LAENGE 58 |
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 LoopConfig; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
unsigned char ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
}; |
/* |
unsigned char ServoNickMax; // Wert : 0-250 |
unsigned char ServoNickRefresh; // |
unsigned char LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
unsigned char LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
//------------------------------------------------ |
unsigned char LoopConfig; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
unsigned char ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
unsigned char Reserved[4]; |
char Name[12]; |
*/ |
extern struct mk_param_struct EE_Parameter; |
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; |
#endif //_FC_H |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/flight-ctrl_mega644_v0_64.aws |
---|
0,0 → 1,0 |
<AVRWorkspace><IOSettings><CurrentRegisters/></IOSettings><part name="ATMEGA644"/><Files><File00000 Name="E:\hubi\ufo\mickrokopter\a Versions\version system\flightcontrol\branches\MergedVersionsByOsiair\alpha\v064JokoGPSNick666Acc\main.c" Position="343 108 875 484" LineCol="155 0" State="Maximized"/><File00001 Name="E:\hubi\ufo\mickrokopter\a Versions\version system\flightcontrol\branches\MergedVersionsByOsiair\alpha\v064JokoGPSNick666Acc\compass.c" Position="333 69 1045 622" LineCol="25 0" State="Maximized"/><File00002 Name="E:\hubi\ufo\mickrokopter\a Versions\version system\flightcontrol\branches\MergedVersionsByOsiair\alpha\v064JokoGPSNick666Acc\compass.h" Position="368 130 914 498" LineCol="0 0" State="Maximized"/><File00003 Name="E:\hubi\ufo\mickrokopter\a Versions\version system\flightcontrol\branches\MergedVersionsByOsiair\alpha\v064JokoGPSNick666Acc\fc.c" Position="336 71 1182 830" LineCol="834 0" State="Maximized"/></Files></AVRWorkspace> |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/flight.pnproj |
---|
0,0 → 1,0 |
<Project name="Flight-Ctrl"><File path="uart.h"></File><File path="main.c"></File><File path="main.h"></File><File path="makefile"></File><File path="uart.c"></File><File path="printf_P.h"></File><File path="printf_P.c"></File><File path="timer0.c"></File><File path="timer0.h"></File><File path="old_macros.h"></File><File path="twimaster.c"></File><File path="version.txt"></File><File path="twimaster.h"></File><File path="rc.c"></File><File path="rc.h"></File><File path="fc.h"></File><File path="fc.c"></File><File path="menu.h"></File><File path="menu.c"></File><File path="_Settings.h"></File><File path="analog.c"></File><File path="analog.h"></File><File path="GPS.c"></File><File path="gps.h"></File><File path="License.txt"></File></Project> |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/flight.pnps |
---|
0,0 → 1,0 |
<pd><ViewState><e p="Flight-Ctrl" x="true"></e></ViewState></pd> |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/gps.h |
---|
0,0 → 1,26 |
extern long P_Einfluss_North; |
extern long D_Einfluss_North; |
extern long P_Einfluss_East; |
extern long D_Einfluss_East; |
extern long GPS_Positionsabweichung_North; |
extern long GPS_Positionsabweichung_East; |
extern long GPS_Geschwindigkeit_North; |
extern long GPS_Geschwindigkeit_East; |
extern long GPS_Home_North; |
extern long GPS_Home_East; |
extern long Soll_Position_North; |
extern long Soll_Position_East; |
extern volatile long EAST_MITTEL; |
extern volatile long NORTH_MITTEL; |
extern signed int GPS_Nick; |
extern signed int GPS_Roll; |
extern void GPS_Neutral(void); |
extern void GPS_BerechneZielrichtung(void); |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/main.c |
---|
0,0 → 1,229 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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; |
// -- Parametersatz aus EEPROM lesen --- |
// number [0..5] |
void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if (number > 5) number = 5; |
eeprom_read_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
} |
// -- Parametersatz ins EEPROM schreiben --- |
// number [0..5] |
void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length) |
{ |
if(number > 5) number = 5; |
eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * number], length); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], number); // diesen Parametersatz als aktuell merken |
} |
unsigned char GetActiveParamSetNumber(void) |
{ |
unsigned char set; |
set = eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET]); |
if(set > 5) |
{ |
set = 2; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], set); // diesen Parametersatz als aktuell merken |
} |
return(set); |
} |
//############################################################################ |
//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) PlatinenVersion = 11; else PlatinenVersion = 10; |
DDRC = 0x81; // SCL |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
DDRD = 0x3E; // Speaker & TXD & J3 J4 J5 |
DDRD |=0x80; // J7 |
PORTD = 0xF7; // LED |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
beeptime = 2000; |
StickGier = 0; PPM_in[K_GAS] = 0;StickRoll = 0; StickNick = 0; |
ROT_OFF; |
Timer_Init(); |
UART_Init(); |
rc_sum_init(); |
ADC_Init(); |
i2c_init(); |
init_spi(); |
sei(); |
VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL; |
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d ",PlatinenVersion/10,PlatinenVersion%10, VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
printf("\n\r=============================="); |
GRN_ON; |
#define EE_DATENREVISION 62 // wird angepasst, wenn sich die EEPROM-Daten geändert haben |
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != EE_DATENREVISION) |
{ |
printf("\n\rInit. EEPROM: Generiere Default-Parameter..."); |
DefaultKonstanten1(); |
for (unsigned char i=0;i<6;i++) |
{ |
if(i==2) DefaultKonstanten2(); |
WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
} |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], 2); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_VALID], EE_DATENREVISION); |
} |
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; |
DebugIn.Analog[1] = 1000; |
DebugIn.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; |
while (1) |
{ |
if (UpdateMotor) // ReglerIntervall |
{ |
UpdateMotor=0; |
MotorRegler(); |
SendMotorData(); |
ROT_OFF; |
if(PcZugriff) PcZugriff--; |
if(SenderOkay) SenderOkay--; |
if(!I2CTimeout) |
{ |
I2CTimeout = 5; |
i2c_reset(); |
if((BeepMuster == 0xffff) && MotorenEin) |
{ |
beeptime = 10000; |
BeepMuster = 0x0080; |
} |
} |
else |
{ |
I2CTimeout--; |
ROT_OFF; |
} |
} |
if(SIO_DEBUG) |
{ |
DatenUebertragung(); |
BearbeiteRxDaten(); |
} |
else BearbeiteRxDaten(); |
if(CheckDelay(timer)) |
{ |
if(UBat < EE_Parameter.UnterspannungsWarnung) |
{ |
if(BeepMuster == 0xffff) |
{ |
beeptime = 6000; |
BeepMuster = 0x0300; |
} |
} |
timer = SetDelay(100); |
} |
} |
return (1); |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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) PORTB &=~0x01; else PORTB |= 0x01;} |
#define ROT_ON {if(PlatinenVersion == 10) PORTB |= 0x01; else PORTB &=~0x01;} |
#define ROT_FLASH PORTB ^= 0x01 |
#define GRN_OFF PORTB &=~0x02 |
#define GRN_ON PORTB |= 0x02 |
#define GRN_FLASH PORTB ^= 0x02 |
#define LED1_OFF PORTC &=~0x04 //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
#define LED1_ON PORTC |= 0x04 //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
#define LED1_FLASH PORTC ^= 0x04 //Erweiterung by 4ndreas aus mikrosvn branches // (280807Kr) |
#define LED2_OFF PORTC &=~0x08 //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
#define LED2_ON PORTC |= 0x08 //Erweiterung von Smartie aus dem MK-Forum vom 14.06.07. Dadurch können die Transistorausgänge J16 und J17 geschaltet werden. // (160607Kr) |
#define LED2_FLASH PORTC ^= 0x08 //Erweiterung by 4ndreas aus mikrosvn branches // (280807Kr) |
//#ifndef F_CPU |
//#error ################## F_CPU nicht definiert oder ungültig ############# |
//#endif |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//#define ANZ_MITTELWERT 4 |
#define EEPROM_ADR_VALID 1 |
#define EEPROM_ADR_ACTIVE_SET 2 |
#define EEPROM_ADR_LAST_OFFSET 3 |
#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_LOOP_OBEN 0x01 |
#define CFG_LOOP_UNTEN 0x02 |
#define CFG_LOOP_LINKS 0x04 |
#define CFG_LOOP_RECHTS 0x08 |
//#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 void ReadParameterSet (unsigned char number, unsigned char *buffer, unsigned char length); |
extern void WriteParameterSet(unsigned char number, unsigned char *buffer, unsigned char length); |
extern unsigned char GetActiveParamSetNumber(void); |
extern unsigned char EEPromArray[]; |
long durchschnitt_northing; |
long durchschnitt_easting; |
volatile int P_GPS_Verstaerkung; //P-Anteil (80-120) |
volatile int D_GPS_Verstaerkung; //D-Anteil (90) |
#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 "compass.h" |
#include "math.h" |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#define DEBUG_DISPLAY_INTERVALL 123 // in ms |
#define DELAY_US(x) ((unsigned int)( (x) * 1e-6 * F_CPU )) |
#endif //_MAIN_H |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/makefile |
---|
0,0 → 1,391 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
HAUPT_VERSION = 0 |
NEBEN_VERSION = 64 |
VERSION_KOMPATIBEL = 5 # PC-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 ($(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). |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(HAUPT_VERSION)_$(NEBEN_VERSION) |
# 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 GPS.c compass.c math.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_HAUPTVERSION=$(HAUPT_VERSION) -DVERSION_NEBENVERSION=$(NEBEN_VERSION) -DVERSION_KOMPATIBEL=$(VERSION_KOMPATIBEL) |
# 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 = stk200 |
AVRDUDE_PROGRAMMER = dt006 |
#AVRDUDE_PROGRAMMER = ponyser |
#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_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_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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/math.c |
---|
0,0 → 1,118 |
/* |
Copyright 2007, Niklas Nold |
This program (files math.c and math.h) is free software; you can redistribute it and/or modify |
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
either version 3 of the License, or (at your option) any later version. |
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
*/ |
#include "main.h" |
const uint8_t pgm_atan[270] PROGMEM = {0,1,2,3,5,6,7,8,9,10,11,12,13,15,16,17,18,19,20,21,22,23,24,25,26,27,27,28,29,30,31,32,33,33,34,35,36,37,37,38,39,39,40,41,41,42,43,43,44,44,45,46,46,47,47,48,48,49,49,50,50,51,51,52,52,52,53,53,54,54,54,55,55,56,56,56,57,57,57,58,58,58,59,59,59,60,60,60,60,61,61,61,61,62,62,62,62,63,63,63,63,64,64,64,64,65,65,65,65,65,66,66,66,66,66,67,67,67,67,67,67,68,68,68,68,68,68,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
// Arkustangens2 im Gradmaß |
signed int atan2_i(signed int x, signed int y) |
{ |
int i,angle; |
int8_t m; |
if (!x && !y) return 0; //atan2 = 0 für x und y = 0 |
if (y < 0) m=-1; |
else m=1; |
if (x==0) return (90*m); // atan2 = 90° für x = 0 |
i = abs(((float)y / x) * 50); // Berechne i für die Lookup table (Schrittweite atan(x) ist 0.02 -> *50) |
if (i<270) angle = pgm_read_byte(&pgm_atan[i]); // Lookup für 1° bis 79° |
else if (i>5750) angle = 90; // Grenzwert ist 90° |
else if (i>=1910) angle = 89; // 89° bis 80° über Wertebereiche |
else if (i>=1150) angle = 88; |
else if (i>=820) angle = 87; |
else if (i>=640) angle = 86; |
else if (i>=520) angle = 85; |
else if (i>=440) angle = 84; |
else if (i>=380) angle = 83; |
else if (i>=335) angle = 82; |
else if (i>=299) angle = 81; |
else angle = 80; // (i>=270) |
if (x > 0) return (angle*m); // Quadrant I und IV |
else if ((x < 0) && (y >= 0)) return ((angle*-1) + 180); // Quadrant II |
else return (angle - 180); // x < 0 && y < 0 Quadrant III |
} |
const float pgm_sinus_f [91] PROGMEM = {0.000,0.017,0.035,0.052,0.070,0.087,0.105,0.122,0.139,0.156,0.174,0.191,0.208,0.225,0.242,0.259,0.276,0.292,0.309,0.326,0.342,0.358,0.375,0.391,0.407,0.423,0.438,0.454,0.469,0.485,0.500,0.515,0.530,0.545,0.559,0.574,0.588,0.602,0.616,0.629,0.643,0.656,0.669,0.682,0.695,0.707,0.719,0.731,0.743,0.755,0.766,0.777,0.788,0.799,0.809,0.819,0.829,0.839,0.848,0.857,0.866,0.875,0.883,0.891,0.899,0.906,0.914,0.921,0.927,0.934,0.940,0.946,0.951,0.956,0.961,0.966,0.970,0.974,0.978,0.982,0.985,0.988,0.990,0.993,0.995,0.996,0.998,0.999,0.999,1.000,1.000}; |
inline float pgm_read_float(const float *addr) |
{ |
union |
{ |
uint16_t i[2]; // 2 16-bit-Worte |
float f; |
} u; |
u.i[0]=pgm_read_word((PGM_P)addr); |
u.i[1]=pgm_read_word((PGM_P)addr+2); |
return u.f; |
} |
// Kosinusfunktion im Gradmaß |
float cos_f(signed int winkel) |
{ |
return (sin_f(90-winkel)); |
} |
// Sinusfunktion im Gradmaß |
float sin_f(signed int winkel) |
{ |
short int m,n; |
float sinus; |
//winkel = winkel % 360; |
if (winkel < 0) |
{ |
m = -1; |
winkel = abs(winkel); |
} |
else m = +1; |
// Quadranten auswerten |
if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;} |
else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;} |
else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;} |
else n = 1; //0 - 90 Grad |
sinus = pgm_read_float(&pgm_sinus_f[winkel]); |
return (sinus*m*n); |
} |
const uint8_t pgm_asin[201] PROGMEM = {0,0,1,1,1,1,2,2,2,3,3,3,3,4,4,4,5,5,5,5,6,6,6,7,7,7,7,8,8,8,9,9,9,9,10,10,10,11,11,11,12,12,12,12,13,13,13,14,14,14,14,15,15,15,16,16,16,17,17,17,17,18,18,18,19,19,19,20,20,20,20,21,21,21,22,22,22,23,23,23,24,24,24,25,25,25,25,26,26,26,27,27,27,28,28,28,29,29,29,30,30,30,31,31,31,32,32,32,33,33,33,34,34,34,35,35,35,36,36,37,37,37,38,38,38,39,39,39,40,40,41,41,41,42,42,42,43,43,44,44,44,45,45,46,46,46,47,47,48,48,49,49,49,50,50,51,51,52,52,53,53,54,54,55,55,56,56,57,57,58,58,59,59,60,60,61,62,62,63,64,64,65,66,66,67,68,68,69,70,71,72,73,74,75,76,77,79,80,82,84,90}; |
// Akurssinusfunktion im Gradmaß |
int8_t asin_i(signed int i) |
{ |
signed char m; |
if (i < 0) {m=-1;i=abs(i);} |
else m=1; |
return (pgm_read_byte(&pgm_asin[i]) * m); |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/math.h |
---|
0,0 → 1,11 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Peter Muehlenbrock |
// Definitionen fuer Modul math |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
extern signed int atan2_i(signed int x, signed int y); |
extern float cos_f(signed int winkel); |
extern float sin_f(signed int winkel); |
extern int8_t asin_i(signed int i); |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/menu.c |
---|
0,0 → 1,118 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
unsigned int TestInt = 0; |
#define ARRAYGROESSE 10 |
unsigned char Array[ARRAYGROESSE] = {1,2,3,4,5,6,7,8,9,10}; |
char DisplayBuff[80] = "Hallo Welt"; |
unsigned char DispPtr = 0; |
unsigned char RemoteTasten = 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) |
{ |
static unsigned char MaxMenue = 10,MenuePunkt=0; |
if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); } |
if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); } |
if((RemoteTasten & KEY1) && (RemoteTasten & KEY2)) MenuePunkt = 0; |
LCD_printfxy(17,0,"[%i]",MenuePunkt); |
switch(MenuePunkt) |
{ |
case 0: |
LCD_printfxy(0,0,"++ MikroKopter ++"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d",PlatinenVersion/10,PlatinenVersion%10,VERSION_HAUPTVERSION, VERSION_NEBENVERSION); |
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,"Hoehe: %5i",HoehenWert); |
LCD_printfxy(0,1,"SollHoehe: %5i",SollHoehe); |
LCD_printfxy(0,2,"Luftdruck: %5i",MessLuftdruck); |
LCD_printfxy(0,3,"Off : %5i",DruckOffsetSetting); |
} |
else |
{ |
LCD_printfxy(0,1,"Keine "); |
LCD_printfxy(0,2,"Höhenregelung"); |
} |
break; |
case 2: |
LCD_printfxy(0,0,"akt. Lage"); |
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024); |
LCD_printfxy(0,3,"Kompass: %5i",KompassValue); |
break; |
case 3: |
LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]); |
LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]); |
LCD_printfxy(0,2,"K5:%4i K6:%4i ",PPM_in[5],PPM_in[6]); |
LCD_printfxy(0,3,"K7:%4i K8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4: |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_NICK]],PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Gi:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_GAS]],PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]],PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]]); |
break; |
case 5: |
LCD_printfxy(0,0,"Gyro - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AccumulateNick / MessanzahlNick, AdNeutralNick); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AccumulateRoll / MessanzahlRoll, AdNeutralRoll); |
LCD_printfxy(0,3,"Gier %4i (%3i)",AccumulateGier / MessanzahlGier, AdNeutralGier); |
break; |
case 6: |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",accumulate_AccNick / messanzahl_AccNick,NeutralAccX); |
LCD_printfxy(0,2,"Roll %4i (%3i)",accumulate_AccRoll / messanzahl_AccRoll,NeutralAccY); |
LCD_printfxy(0,3,"Hoch %4i (%3i)",Aktuell_az/*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,"Po1: %3i Po5: %3i" ,Poti1,Poti5); //PPM24-Erweiterung (121007Kr) |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Erweiterung (121007Kr) |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Erweiterung (121007Kr) |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Erweiterung (121007Kr) |
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; |
default: MaxMenue = MenuePunkt - 1; |
MenuePunkt = 0; |
break; |
} |
RemoteTasten = 0; |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/menu.h |
---|
0,0 → 1,5 |
extern void Menu(void); |
extern char DisplayBuff[80]; |
extern unsigned char DispPtr; |
unsigned char RemoteTasten; |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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 |
extern 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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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[15]; //PPM24-Erweiterung (121007Kr) |
volatile int PPM_diff[15]; // das diffenzierte Stick-Signal //PPM24-Erweiterung (121007Kr) |
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 > 1500) && (signal < 8000)) |
{ |
index = 1; |
NewPpmData = 0; // Null bedeutet: Neue Daten |
// OCR2A = Poti2/2 + 80; |
} |
else |
{ |
if(index < 14) //PPM24-Erweiterung (121007Kr) |
{ |
if((signal > 250) && (signal < 687)) |
{ |
signal -= 466; |
// Stabiles Signal |
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;} |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
// if(tmp > signal+1) tmp--; else |
// if(tmp < signal-1) tmp++; |
PPM_diff[index] = tmp - PPM_in[index]; |
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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/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[15]; //PPM24-Erweiterung (121007Kr) |
extern volatile int PPM_diff[15]; // das diffenzierte Stick-Signal //PPM24-Erweiterung (121007Kr) |
extern volatile unsigned char NewPpmData; |
#endif //_RC_H |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/timer0.c |
---|
0,0 → 1,173 |
#include "main.h" |
volatile unsigned int CountMilliseconds = 0; |
volatile static unsigned int tim_main; |
volatile unsigned char UpdateMotor = 0; |
volatile unsigned int cntKompass = 0; |
volatile unsigned int beeptime = 0; |
unsigned int BeepMuster = 0xffff; |
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 |
{ |
static unsigned char cnt_1ms = 1,cnt = 0; |
unsigned char pieper_ein = 0; |
// TCNT0 -= 250;//TIMER_RELOAD_VALUE; |
if(!cnt--) |
{ |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; |
CountMilliseconds++; |
} |
if(beeptime > 1) |
{ |
beeptime--; |
if(beeptime & BeepMuster) |
{ |
pieper_ein = 1; |
} |
else pieper_ein = 0; |
} |
else |
{ |
pieper_ein = 0; |
BeepMuster = 0xffff; |
} |
if(pieper_ein) |
{ |
if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2 |
else PORTC |= (1<<7); // Speaker an PORTC.7 |
} |
else |
{ |
if(PlatinenVersion == 10) PORTD &= ~(1<<2); |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV || EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) //Abfrage um GPS_AKTIV erweitert, damit auch bei nur eingeschaltetem GPS OHNE Kompass der benötigte Kompassvalue berechnet wird (200907Kr) |
{ |
// if(PINC & 0x10) |
// { |
// cntKompass++; |
// } |
// else |
// { |
// if((cntKompass) && (cntKompass < 4000)) |
// { |
// KompassValue = cntKompass; |
// } |
// if(cntKompass < 10) cntKompass = 10; |
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
// KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
// cntKompass = 0; |
// } |
// } |
// |
// if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
// { |
MM3_timer0(); // Kompass auslesen |
if (!cntKompass--) // Aufruf mit 25 Hz |
{ |
KompassValue = MM3_heading(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
cntKompass = 320; |
} |
} |
} |
//} |
void Timer_Init(void) |
{ |
tim_main = SetDelay(10); |
TCCR0B = CK8; |
TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM |
OCR0A = 0; |
OCR0B = 120; |
TCNT0 = -TIMER_RELOAD_VALUE; // reload |
//OCR1 = 0x00; |
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; |
TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); |
// TIMSK2 |= _BV(TOIE2); |
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)); |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Servo ansteuern |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
SIGNAL(SIG_OUTPUT_COMPARE2A) |
{ |
static unsigned char timer = 10; |
if(!timer--) |
{ |
TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3; |
ServoValue = Parameter_ServoNickControl; |
if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin; |
else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax; |
OCR2A = ServoValue;// + 75; |
timer = EE_Parameter.ServoNickRefresh; |
} |
else |
{ |
TCCR2A =3; |
PORTD&=~0x80; |
} |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/timer0.h |
---|
0,0 → 1,15 |
#define TIMER_TEILER CK8 |
#define TIMER_RELOAD_VALUE 250 |
void Timer_Init(void); |
void Delay_ms(unsigned int); |
unsigned int SetDelay (unsigned int t); |
char CheckDelay (unsigned int t); |
extern volatile unsigned int CountMilliseconds; |
extern volatile unsigned char UpdateMotor; |
extern volatile unsigned int beeptime; |
extern volatile unsigned int cntKompass; |
extern int ServoValue; |
extern unsigned int BeepMuster; |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/twimaster.c |
---|
0,0 → 1,152 |
/*############################################################################ |
############################################################################*/ |
#include "main.h" |
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; |
} |
TWCR |= 0x80; |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/twimaster.h |
---|
0,0 → 1,32 |
/*############################################################################ |
############################################################################*/ |
#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 unsigned char twi_state; |
extern unsigned char motor; |
extern unsigned char motorread; |
extern unsigned char motor_rx[8]; |
void i2c_init (void); // I2C initialisieren |
char i2c_start (void); // Start I2C |
void i2c_stop (void); // Stop I2C |
char i2c_write_byte (char byte); // 1 Byte schreiben |
#endif |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/uart.c |
---|
0,0 → 1,642 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// + |
// + GPS read out: |
// + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany |
// + only for non-profit use |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
#include "uart.h" |
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 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 volatile char PC_DebugTimeout = 0; |
unsigned char PcZugriff = 100; |
unsigned char MotorTest[4] = {0,0,0,0}; |
unsigned char MeineSlaveAdresse; |
struct str_DebugOut DebugOut; |
struct str_Debug DebugIn; |
struct str_VersionInfo VersionInfo; |
int Debug_Timer; |
static uint8_t gpsState; |
#define GPS_EMPTY 0 |
#define GPS_SYNC1 1 |
#define GPS_SYNC2 2 |
#define GPS_CLASS 3 |
#define GPS_LEN1 4 |
#define GPS_LEN2 5 |
#define GPS_FILLING 6 |
#define GPS_CKA 7 |
#define GPS_CKB 8 |
gpsInfo_t actualPos; // measured position (last gps record) |
#define SYNC_CHAR1 0xb5 |
#define SYNC_CHAR2 0x62 |
#define CLASS_NAV 0x01 |
#define MSGID_POSECEF 0x01 |
#define MSGID_STATUS 0x03 |
#define MSGID_VELECEF 0x11 |
#define MSGID_POSUTM 0x08 |
#define MSGID_VELNED 0x12 |
typedef struct { |
unsigned long ITOW; // time of week |
uint8_t GPSfix; // GPSfix Type, range 0..6 |
uint8_t Flags; // Navigation Status Flags |
uint8_t DiffS; // Differential Status |
uint8_t res; // reserved |
unsigned long TTFF; // Time to first fix (millisecond time tag) |
unsigned long MSSS; // Milliseconds since Startup / Reset |
uint8_t packetStatus; |
} NAV_STATUS_t; |
typedef struct { |
unsigned long ITOW; // time of week |
long LON; // longitude in 1e-07 deg |
long LAT; // lattitude |
long HEIGHT; // height in mm |
long HMSL; // height above mean sea level im mm |
unsigned long Hacc; // horizontal accuracy in mm |
unsigned long Vacc; // vertical accuracy in mm |
uint8_t packetStatus; |
} NAV_POSLLH_t; |
typedef struct { |
unsigned long ITOW; // time of week |
long EAST; // cm UTM Easting |
long NORTH; // cm UTM Nording |
long ALT; // cm altitude |
uint8_t ZONE; // UTM zone number |
uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
uint8_t packetStatus; |
} NAV_POSUTM_t; |
typedef struct { |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long VEL_N; // cm/s NED north velocity |
long VEL_E; // cm/s NED east velocity |
long VEL_D; // cm/s NED down velocity |
unsigned long Speed; // cm/s Speed (3-D) |
unsigned long GSpeed; // cm/s Ground Speed (2-D) |
long Heading; // deg (1e-05) Heading 2-D |
unsigned long SAcc; // cm/s Speed Accuracy Estimate |
unsigned long CAcc; // deg Course / Heading Accuracy Estimate |
uint8_t packetStatus; |
} NAV_VELNED_t; |
typedef struct { |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long ECEF_X; // ecef x / cm |
long ECEF_Y; // ecef y / cm |
long ECEF_Z; // ecef z / cm |
unsigned long Pacc; // Abweichung |
uint8_t packetStatus; |
} NAV_POSECEF_t ; |
typedef struct { |
unsigned long ITOW; // ms GPS Millisecond Time of Week |
long ECEFVX; // ecef x velocity cm/s |
long ECEFVY; // ecef y velocity cm/s |
long ECEFVZ; // ecef z velocity cm/s |
unsigned long SAcc; // Abweichung |
uint8_t packetStatus; |
} NAV_VELECEF_t; |
NAV_STATUS_t navStatus; |
NAV_POSECEF_t navPosECEF; |
NAV_VELECEF_t navVelECEF; |
NAV_POSUTM_t navPosUtm; |
NAV_VELNED_t navVelNed; |
volatile char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
volatile uint8_t CK_A, CK_B; // UBX checksum bytes |
volatile unsigned short msgLen; |
volatile uint8_t msgID; |
volatile uint8_t ignorePacket; // true when previous packet was not processed |
// distance to target position |
long rollOffset; // in 10cm |
long nickOffset; |
#define GPS_INTCYCLES 100 |
#define GPS_I_LIMIT (long)(40 * MAINLOOPS_PER_SEC) |
#ifdef GPS_DEBUG // if set then the GPS data is transfered to display |
extern volatile uint8_t v24state; |
char buf[200]; |
char *bp; |
char *ep; |
#endif |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++ 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; |
} |
void GPSscanData (void) |
{ |
if (navPosECEF.packetStatus == 1) // valid packet |
{ |
actualPos.x = navPosECEF.ECEF_X; //ECEF X in cm |
actualPos.y = navPosECEF.ECEF_Y; //ECEF Y in cm |
actualPos.z = navPosECEF.ECEF_Z; //ECEF Z in cm |
navPosECEF.packetStatus = 0; |
} |
if (navStatus.packetStatus == 1) // valid packet |
{ |
actualPos.GPSFix = navStatus.GPSfix; |
actualPos.newData = navStatus.packetStatus; |
navStatus.packetStatus = 0; |
} |
if (navVelECEF.packetStatus == 1) // valid packet |
{ |
actualPos.vx = navVelECEF.ECEFVX; //ECEF VEL X in cm/s |
actualPos.vy = navVelECEF.ECEFVY; //ECEF VEL Y in cm/s |
actualPos.vz = navVelECEF.ECEFVZ; //ECEF VEL Z in cm/s |
navVelECEF.packetStatus = 0; |
} |
if (navPosUtm.packetStatus == 1) // valid packet |
{ |
actualPos.northing = navPosUtm.NORTH; |
actualPos.easting = navPosUtm.EAST; |
actualPos.altitude = navPosUtm.ALT; |
navPosUtm.packetStatus = 0; |
} |
if (navVelNed.packetStatus == 1) |
{ |
actualPos.velNorth = navVelNed.VEL_N; |
actualPos.velEast = navVelNed.VEL_E; |
navVelNed.packetStatus = 0; |
} |
if (actualPos.newData != 0){ROT_ON;} //-> Rot blinkt mit 4Hz wenn Daten vom GPS Empfänger ankommen und brauchbar sind |
} |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//++ 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; |
uint8_t c; |
uint8_t re; |
re = (UCSR0A & (_B1(FE0) | _B1(DOR0))); // any error occured ? |
c = SioTmp; |
#ifdef GPS_DEBUG |
*bp++ = c; |
if (bp >= (buf+200)) bp = buf; |
if (v24state == 0) |
{ |
v24state = 1; |
UDR0 = *ep++; |
if (ep >= buf+200) |
ep = buf; |
UCSR0B |= _B1(UDRIE0); //enable further irqs |
} |
#endif |
if (re == 0) |
{ |
switch (gpsState) |
{ |
case GPS_EMPTY: |
if (c == SYNC_CHAR1) |
gpsState = GPS_SYNC1; |
break; |
case GPS_SYNC1: |
if (c == SYNC_CHAR2) |
gpsState = GPS_SYNC2; |
else if (c != SYNC_CHAR1) |
gpsState = GPS_EMPTY; |
break; |
case GPS_SYNC2: |
if (c == CLASS_NAV) |
gpsState = GPS_CLASS; |
else |
gpsState = GPS_EMPTY; |
break; |
case GPS_CLASS: // msg ID seen: init packed receive |
msgID = c; |
CK_A = CLASS_NAV + c; |
CK_B = CLASS_NAV + CK_A; |
gpsState = GPS_LEN1; |
switch (msgID) |
{ |
case MSGID_STATUS: |
ubxP = (char*)&navStatus; |
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
ubxSp = (char*)&navStatus.packetStatus; |
ignorePacket = navStatus.packetStatus; |
break; |
case MSGID_POSECEF: |
ubxP = (char*)&navPosECEF; |
ubxEp = (char*)(&navPosECEF + sizeof(NAV_POSECEF_t)); |
ubxSp = (char*)&navPosECEF.packetStatus; |
ignorePacket = navPosECEF.packetStatus; |
break; |
case MSGID_VELECEF: |
ubxP = (char*)&navVelECEF; |
ubxEp = (char*)(&navVelECEF + sizeof(NAV_VELECEF_t)); |
ubxSp = (char*)&navVelECEF.packetStatus; |
ignorePacket = navVelECEF.packetStatus; |
break; |
case MSGID_POSUTM: |
ubxP = (char*)&navPosUtm; |
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
ubxSp = (char*)&navPosUtm.packetStatus; |
ignorePacket = navPosUtm.packetStatus; |
break; |
case MSGID_VELNED: |
ubxP = (char*)&navVelNed; |
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
ubxSp = (char*)&navVelNed.packetStatus; |
ignorePacket = navVelNed.packetStatus; |
break; |
default: |
ignorePacket = 1; |
ubxSp = (char*)0; |
} |
break; |
case GPS_LEN1: // first len byte |
msgLen = c; |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_LEN2; |
break; |
case GPS_LEN2: // second len byte |
msgLen = msgLen + (c * 256); |
CK_A += c; |
CK_B += CK_A; |
gpsState = GPS_FILLING; // next data will be stored in packet struct |
break; |
case GPS_FILLING: |
CK_A += c; |
CK_B += CK_A; |
if ( !ignorePacket && ubxP < ubxEp) |
*ubxP++ = c; |
if (--msgLen == 0) |
gpsState = GPS_CKA; |
break; |
case GPS_CKA: |
if (c == CK_A) |
gpsState = GPS_CKB; |
else |
gpsState = GPS_EMPTY; |
break; |
case GPS_CKB: |
if (c == CK_B && ubxSp) // No error -> packet received successfully |
*ubxSp = 1; // set packetStatus in struct |
gpsState = GPS_EMPTY; // ready for next packet |
break; |
default: |
gpsState = GPS_EMPTY; // ready for next packet |
} |
} |
else // discard any data if error occured |
{ |
gpsState = GPS_EMPTY; |
GPSscanData (); //Test kann ggf. wieder gelöscht werden! |
} |
GPSscanData (); |
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; |
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 modul, unsigned char *snd, unsigned char len) |
{ |
unsigned int pt = 0; |
unsigned char a,b,c; |
unsigned char ptr = 0; |
SendeBuffer[pt++] = '#'; // Startzeichen |
SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...) |
SendeBuffer[pt++] = cmd; // Commando |
while(len) |
{ |
if(len) { a = snd[ptr++]; len--;} else a = 0; |
if(len) { b = snd[ptr++]; len--;} else b = 0; |
if(len) { c = snd[ptr++]; len--;} else c = 0; |
SendeBuffer[pt++] = '=' + (a >> 2); |
SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
SendeBuffer[pt++] = '=' + ( c & 0x3f); |
} |
AddCRC(pt); |
} |
// -------------------------------------------------------------------------- |
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer |
{ |
unsigned char a,b,c,d; |
unsigned char ptr = 0; |
unsigned char x,y,z; |
while(len) |
{ |
a = RxdBuffer[ptrIn++] - '='; |
b = RxdBuffer[ptrIn++] - '='; |
c = RxdBuffer[ptrIn++] - '='; |
d = RxdBuffer[ptrIn++] - '='; |
if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if(len--) ptrOut[ptr++] = x; else break; |
if(len--) ptrOut[ptr++] = y; else break; |
if(len--) ptrOut[ptr++] = z; else break; |
} |
} |
// -------------------------------------------------------------------------- |
void BearbeiteRxDaten(void) |
{ |
if(!NeuerDatensatzEmpfangen) return; |
unsigned int tmp_int_arr1[1]; |
unsigned int tmp_int_arr2[2]; |
unsigned int tmp_int_arr3[3]; |
unsigned char tmp_char_arr2[2]; |
unsigned char tmp_char_arr3[3]; |
unsigned char tmp_char_arr4[4]; |
//if(!MotorenEin) |
PcZugriff = 255; |
switch(RxdBuffer[2]) |
{ |
case 'c':// Debugdaten incl. Externe IOs usw |
Decode64((unsigned char *) &DebugIn,sizeof(DebugIn),3,AnzahlEmpfangsBytes); |
/* for(unsigned char i=0; i<4;i++) |
{ |
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2], DebugIn.Analog[i]); |
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2 + 1], DebugIn.Analog[i] >> 8); |
}*/ |
//RemoteTasten |= DebugIn.RemoteTasten; |
DebugDataAnforderung = 1; |
break; |
case 'h':// x-1 Displayzeilen |
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes); |
RemoteTasten |= tmp_char_arr2[0]; |
DebugDisplayAnforderung = 1; |
break; |
case 't':// Motortest |
Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes); |
break; |
case 'v': // Version-Anforderung und Ausbaustufe |
GetVersionAnforderung = 1; |
break; |
case 'g':// "Get"-Anforderung für Debug-Daten |
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
DebugGetAnforderung = 1; |
break; |
case 'q':// "Get"-Anforderung für Settings |
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes); |
if(tmp_char_arr2[0] != 0xff) |
{ |
if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; |
ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE); |
} |
else |
SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE); |
break; |
case 'l': |
case 'm': |
case 'n': |
case 'o': |
case 'p': // Parametersatz speichern |
Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes); |
WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1); // aktiven Datensatz merken |
Piep(GetActiveParamSetNumber()); |
break; |
} |
// DebugOut.AnzahlZyklen = Debug_Timer_Intervall; |
NeuerDatensatzEmpfangen = 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(200); |
gpsState = GPS_EMPTY; |
} |
//--------------------------------------------------------------------------------------------- |
void DatenUebertragung(void) |
{ |
static char dis_zeile = 0; |
if(!UebertragungAbgeschlossen) return; |
if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen |
{ |
SendOutData('G',MeineSlaveAdresse,(unsigned char *) &DebugIn,sizeof(DebugIn)); |
DebugGetAnforderung = 0; |
} |
if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen) |
{ |
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut)); |
DebugDataAnforderung = 0; |
Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL); |
} |
if(DebugDisplayAnforderung && UebertragungAbgeschlossen) |
{ |
Menu(); |
DebugDisplayAnforderung = 0; |
if(++dis_zeile == 4) |
{ |
SendOutData('4',0,&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen |
dis_zeile = -1; |
} |
else SendOutData('0' + dis_zeile,0,&DisplayBuff[20 * dis_zeile],20); // DisplayZeile übertragen |
} |
if(GetVersionAnforderung && UebertragungAbgeschlossen) |
{ |
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo)); |
GetVersionAnforderung = 0; |
} |
} |
/branches/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/uart.h |
---|
0,0 → 1,119 |
#ifndef _UART_H |
#define _UART_H |
#define MAX_SENDE_BUFF 150 |
#define MAX_EMPFANGS_BUFF 150 |
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 int Debug_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 unsigned char MotorTest[4]; |
struct str_DebugOut |
{ |
unsigned char Digital[2]; |
unsigned int Analog[32]; // Debugwerte |
}; |
extern struct str_DebugOut DebugOut; |
#define _B1(bit) (1 << (bit)) |
#define _B0(bit) (0 << (bit)) |
typedef struct |
{ |
long x; // in cm (+ = north) |
long y; // in cm (+ = east) |
long z; // in cm |
long vx; |
long vy; |
long vz; |
long northing; |
long easting; |
long altitude; |
long velNorth; |
long velEast; |
uint8_t newData; // status of data: 0 = invlid; 1 = valid |
uint8_t GPSFix; |
uint8_t noSV; // number of sats |
} gpsInfo_t; |
extern gpsInfo_t lockedPos; // stored position to fly to |
extern gpsInfo_t actualPos; // measured position (last gps record) |
extern void GPSscanData (void); |
extern void GPSupdate(void); |
extern void GPSsaveTarget (uint8_t mode); |
struct str_Debug |
{ |
unsigned char Digital[2]; |
unsigned char RemoteTasten; |
unsigned int Analog[4]; |
}; |
extern struct str_Debug DebugIn; |
struct str_VersionInfo |
{ |
unsigned char Hauptversion; |
unsigned char Nebenversion; |
unsigned char PCKompatibel; |
unsigned char Rserved[7]; |
}; |
extern struct str_VersionInfo VersionInfo; |
//Die Baud_Rate der Seriellen Schnittstelle ist 9600 Baud |
//#define BAUD_RATE 9600 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 14400 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 28800 //Baud Rate für die Serielle Schnittstelle |
//#define BAUD_RATE 38400 //Baud Rate für die Serielle Schnittstelle |
#define BAUD_RATE 57600 //Baud Rate für die Serielle Schnittstelle |
//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/MergedVersionsByOsiair/alpha/v064JokoGPSNick666MM3Acc/version.txt |
---|
0,0 → 1,104 |
------- |
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 |
- Höhenreglerabsenkung von -20 auf -40 (fc.c) |
- LED1 und LED2 Ansteuerung (fc.c) und (main.h) |
- geänderte Kanalbelegung K1-K8, Unterspannungsschwelle (9,5V) und StickD (fc.c) |
- P,D Anteil seperat in Variable geschrieben und auf das Analog Display Ausgegeben (Kopter Tool) |
- GPS_Nick und GPS_Roll werden nach Deaktivierung von Position-Hold auf Null gesetzt, um ein Einfrieren der GPS Steuerwerte auf den letzten Wert zu verhindern |
- Funktionsaufruf der GPS-Funktion von Chris/Hallo2 |
- UART.C für die GPS Signale im UTM Format auf Pitschu basierend |
- meine GPS.c-Routine |
- Mircos variable Filterung (funktioniert nur wenn 250ms Aufruf aktiv ist) |
- GPS wird deaktiviert und Summer eingeschaltet, wenn kein 3D-Fix (mehr) vorhanden |
- Statusanzeige auf eine LED reduziert (aus, blinkend, an) |
- blinkender LED_1 Ausgang für Positionsleuchten (einstellbar über Userparam.4) |
- Homing-Fkt. eingebaut |
- Diverse Absicherungen gegen Fehlbedienungen (z.B. nicht gelernte Home-Position, ...) |
- Rotationsmatrix mit Kompass, damit die GPS-Fkt. nicht nur nach Norden ausgerichtet funktioniert |
- dyn. Position Hold -> normaler Rundflug möglich |
- Limit_D_Anteil = Userparam3 um im Homing- bzw. Waypointmodus die Sollposition gleichmäßiger anzufliegen |
- Die Berechnung von Kompassvalue wird jetzt auch über den GPS Haken im Koptertool aktiviert |
- 12 Kanal Erweiterung (rc.c; rc.h; fc.c; fc.h; menu.c) |
- D-Anteil_Limit wegen HOMING ->> Radius von 700 cm -> 250 cm |
- ... |
(- Regelung wird nur noch alle 250 ms aufgerufen) |