/branches/salvo_gps/Basis_v0070d/trunk/FlightCtrl.aps |
---|
1,0 → 0,0 |
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>11-Oct-2007 22:58:54</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>15-May-2007 11:20:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\Flight-Ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>F:\SVN\MikroKopter\FlightCtrl\branches\V0.64_ZeroWarnings\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega644.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><HEADERFILE>uart.h</HEADERFILE><HEADERFILE>_Settings.h</HEADERFILE><HEADERFILE>analog.h</HEADERFILE><HEADERFILE>fc.h</HEADERFILE><HEADERFILE>gps.h</HEADERFILE><HEADERFILE>main.h</HEADERFILE><HEADERFILE>menu.h</HEADERFILE><HEADERFILE>old_macros.h</HEADERFILE><HEADERFILE>printf_P.h</HEADERFILE><HEADERFILE>rc.h</HEADERFILE><HEADERFILE>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega644</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>Flight-Ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS><OPTION><FILE>GPS.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>analog.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>eeprom.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>menu.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>printf_P.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>rc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer0.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>twimaster.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS><LIB>libc.a</LIB><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -Wstrict-prototypes -std=gnu99 -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -DVERSION_HAUPTVERSION=0 -DVERSION_NEBENVERSION=64 -DVERSION_KOMPATIBEL=5</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Program Files\WinAVR\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Program Files\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>main.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>uart.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>menu.c</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>timer0.c</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>fc.c</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>fc.h</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>menu.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>TWIMASTER.C</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>twimaster.h</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>uart.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>_Settings.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>analog.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>gps.h</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>main.h</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>old_macros.h</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>printf_P.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>rc.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>Settings.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>timer0.h</FileName><Status>1</Status></File00018></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio> |
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>19-Oct-2008 09:40:51</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>15-May-2007 11:20:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\Flight-Ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>C:\Mikrokopter\Flight_Crtl\v0070d\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega644.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><SOURCEFILE>led.c</SOURCEFILE><SOURCEFILE>spi.c</SOURCEFILE><SOURCEFILE>math.c</SOURCEFILE><HEADERFILE>uart.h</HEADERFILE><HEADERFILE>_Settings.h</HEADERFILE><HEADERFILE>analog.h</HEADERFILE><HEADERFILE>fc.h</HEADERFILE><HEADERFILE>gps.h</HEADERFILE><HEADERFILE>main.h</HEADERFILE><HEADERFILE>menu.h</HEADERFILE><HEADERFILE>old_macros.h</HEADERFILE><HEADERFILE>printf_P.h</HEADERFILE><HEADERFILE>rc.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE><HEADERFILE>led.h</HEADERFILE><HEADERFILE>spi.h</HEADERFILE><HEADERFILE>math.h</HEADERFILE><OTHERFILE>makefile</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>YES</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE>makefile</EXTERNALMAKEFILE><PART>atmega644</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>Flight-Ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS><OPTION><FILE>GPS.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>analog.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>eeprom.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>menu.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>printf_P.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>rc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer0.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>twimaster.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS><LIB>libc.a</LIB><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -Wstrict-prototypes -std=gnu99 -DVERSION_HAUPTVERSION=0 -DVERSION_NEBENVERSION=64 -DVERSION_KOMPATIBEL=5 -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\WinAVR-20080610\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\WinAVR-20080610\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><IOView><usergroups/><sort sorted="0" column="0" ordername="1" orderaddress="1" ordergroup="1"/></IOView><Files><File00000><FileId>00000</FileId><FileName>fc.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>eeprom.c</FileName><Status>1</Status></File00001></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio> |
/branches/salvo_gps/Basis_v0070d/trunk/GPS.c |
---|
1,32 → 1,694 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + only for non-profit use |
// + www.MikroKopter.com |
// + see the File "License.txt" for further Informations |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/* |
This program (files gps.c and gps.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 General Public License and GNU Lesser General Public License for more details. |
You should have received a copy of GNU General Public License (License_GPL.txt) and |
GNU Lesser General Public License (License_LGPL.txt) 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 |
*/ |
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
von Peter Muehlenbrock alias Salvo |
Auswertung der Daten vom GPS im ublox Format |
Hold Modus mit PID Regler |
Rückstuerz zur Basis Funktion |
Umstellung auf NaviParameter an Flight Version 00.70d |
GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird |
Stand 10.10.2008 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
#include "math.h" |
//#include "gps.h" |
// Defines fuer ublox Messageformat um Auswertung zu steuern |
#define UBLOX_IDLE 0 |
#define UBLOX_SYNC1 1 |
#define UBLOX_SYNC2 2 |
#define UBLOX_CLASS 3 |
#define UBLOX_ID 4 |
#define UBLOX_LEN1 5 |
#define UBLOX_LEN2 6 |
#define UBLOX_CKA 7 |
#define UBLOX_CKB 8 |
#define UBLOX_PAYLOAD 9 |
// ublox Protokoll Identifier |
#define UBLOX_NAV_POSUTM 0x08 |
#define UBLOX_NAV_STATUS 0x03 |
#define UBLOX_NAV_VELED 0x12 |
#define UBLOX_NAV_CLASS 0x01 |
#define UBLOX_SYNCH1_CHAR 0xB5 |
#define UBLOX_SYNCH2_CHAR 0x62 |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
signed int GPS_Nick2 = 0; |
signed int GPS_Roll2 = 0; |
long GpsAktuell_X = 0; |
long GpsAktuell_Y = 0; |
long GpsZiel_X = 0; |
long GpsZiel_Y = 0; |
short int ublox_msg_state = UBLOX_IDLE; |
static uint8_t chk_a =0; //Checksum |
static uint8_t chk_b =0; |
short int gps_state,gps_sub_state; //Zustaende der Statemachine |
short int gps_updte_flag; |
static long signed gps_reg_x,gps_reg_y; |
static unsigned int rx_len; |
static unsigned int ptr_payload_data_end; |
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt |
static signed int hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
static signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt |
static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler |
static uint8_t *ptr_payload_data; |
static uint8_t *ptr_pac_status; |
static int dist_flown; |
//static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
static int gps_quiet_cnt; // Zaehler fuer GPS Off Time beim Kameraausloesen |
static int gps_gain; // // Teilerfaktor Regelabweichung zu Ausgabewert |
short int Get_GPS_data(void); |
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt |
NAV_STATUS_t actual_status; // Aktueller Nav Status |
NAV_VELNED_t actual_speed; // Aktueller Geschwindigkeits und Richtungsdaten |
GPS_ABS_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst |
GPS_ABS_POSITION_t gps_home_position; // Die Startposition, beim Kalibrieren ermittelt |
GPS_REL_POSITION_t gps_rel_act_position; // Die aktuelle relative Position bezogen auf Home Position |
GPS_REL_POSITION_t gps_rel_hold_position; // Die gespeicherte Sollposition fuer GPS_ Hold Mode |
GPS_REL_POSITION_t gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode |
// Initialisierung |
void GPS_Neutral(void) |
{ |
GpsZiel_X = GpsAktuell_X; |
GpsZiel_Y = GpsAktuell_Y; |
ublox_msg_state = UBLOX_IDLE; |
gps_state = GPS_CRTL_IDLE; |
gps_sub_state = GPS_CRTL_IDLE; |
actual_pos.status = 0; |
actual_speed.status = 0; |
actual_status.status = 0; |
gps_home_position.status = 0; // Noch keine gueltige Home Position |
gps_act_position.status = 0; |
gps_rel_act_position.status = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_updte_flag = 0; |
gps_alive_cnt = 0; |
} |
void GPS_BerechneZielrichtung(void) |
// Home Position sichern falls Daten verfuegbar sind. |
void GPS_Save_Home(void) |
{ |
short int n; |
n = Get_GPS_data(); |
if (n == 0) // Gueltige und aktuelle Daten ? |
{ |
// Neue GPS Daten liegen vor |
gps_home_position.utm_east = gps_act_position.utm_east; |
gps_home_position.utm_north = gps_act_position.utm_north; |
gps_home_position.utm_alt = gps_act_position.utm_alt; |
gps_home_position.status = 1; // Home Position gueltig |
} |
} |
// Relative Position zur Home Position bestimmen |
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig |
short int Get_Rel_Position(void) |
{ |
short int n = 0; |
n = Get_GPS_data(); |
if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind |
if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert |
if (gps_home_position.status > 0) //Nur wenn Home Position vorliegt |
{ |
gps_rel_act_position.utm_east = (int) (gps_act_position.utm_east - gps_home_position.utm_east); |
gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
gps_rel_act_position.utm_alt = (int) (gps_act_position.utm_alt - gps_home_position.utm_alt); |
gps_rel_act_position.status = 1; // gueltige Positionsdaten |
n = 0; |
gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen. |
} |
else |
{ |
n = 2; //keine gueltigen Daten vorhanden |
gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist. |
} |
return (n); |
} |
// Daten aus aktuellen ublox Messages extrahieren |
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig |
short int Get_GPS_data(void) |
{ |
short int n = 1; |
if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist |
gps_gain = Parameter_NaviGpsGain*8/100; //maximal Wert ist 20 |
// debug_gp_0 = (int)gps_gain; // zum Debuggen |
if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0)) |
{ |
if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind |
{ |
actual_status.status = 0; |
gps_act_position.utm_east = actual_pos.utm_east/10; |
gps_act_position.utm_north = actual_pos.utm_north/10; |
gps_act_position.utm_alt = actual_pos.utm_alt/10; |
actual_pos.status = 0; //neue ublox Messages anfordern |
gps_act_position.speed_gnd = actual_speed.speed_gnd; |
gps_act_position.speed_gnd = actual_speed.speed_gnd; |
gps_act_position.heading = actual_speed.heading/100000; |
actual_speed.status = 0; |
gps_act_position.status = 1; |
n = 0; //Daten gueltig |
} |
else |
{ |
gps_act_position.status = 0; //Keine gueltigen Daten |
actual_speed.status = 0; |
actual_status.status = 0; |
actual_pos.status = 0; //neue ublox Messages anfordern |
n = 2; |
} |
} |
return (n); |
} |
/* |
Daten vom GPS im ublox MSG Format auswerten |
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen |
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein |
*/ |
void Get_Ublox_Msg(uint8_t rx) |
{ |
switch (ublox_msg_state) |
{ |
case UBLOX_IDLE: // Zuerst Synchcharacters pruefen |
if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1; |
else ublox_msg_state = UBLOX_IDLE; |
break; |
case UBLOX_SYNC1: |
if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2; |
else ublox_msg_state = UBLOX_IDLE; |
chk_a = 0,chk_b = 0; |
break; |
case UBLOX_SYNC2: |
if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS; |
else ublox_msg_state = UBLOX_IDLE; |
break; |
case UBLOX_CLASS: // Nur NAV Meldungen auswerten |
switch (rx) |
{ |
case UBLOX_NAV_POSUTM: |
ptr_pac_status = &actual_pos.status; |
if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden |
else |
{ |
ptr_payload_data = &actual_pos; |
ptr_payload_data_end = &actual_pos.status; |
ublox_msg_state = UBLOX_LEN1; |
} |
break; |
case UBLOX_NAV_STATUS: |
ptr_pac_status = &actual_status.status; |
if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; |
else |
{ |
ptr_payload_data = &actual_status; |
ptr_payload_data_end = &actual_status.status; |
ublox_msg_state = UBLOX_LEN1; |
} |
break; |
case UBLOX_NAV_VELED: |
ptr_pac_status = &actual_speed.status; |
if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; |
else |
{ |
ptr_payload_data = &actual_speed; |
ptr_payload_data_end = &actual_speed.status; |
ublox_msg_state = UBLOX_LEN1; |
} |
break; |
default: |
ublox_msg_state = UBLOX_IDLE; |
break; |
} |
chk_a = UBLOX_NAV_CLASS + rx; |
chk_b = UBLOX_NAV_CLASS + chk_a; |
break; |
case UBLOX_LEN1: // Laenge auswerten |
rx_len = rx; |
chk_a += rx; |
chk_b += chk_a; |
ublox_msg_state = UBLOX_LEN2; |
break; |
case UBLOX_LEN2: // Laenge auswerten |
rx_len = rx_len + (rx *256); // Laenge ermitteln |
chk_a += rx; |
chk_b += chk_a; |
ublox_msg_state = UBLOX_PAYLOAD; |
break; |
case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen |
if (rx_len > 0) |
{ |
*ptr_payload_data = rx; |
chk_a += rx; |
chk_b += chk_a; |
--rx_len; |
if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end)) |
{ |
ptr_payload_data++; |
ublox_msg_state = UBLOX_PAYLOAD; |
} |
else ublox_msg_state = UBLOX_CKA; |
} |
else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler |
break; |
case UBLOX_CKA: // Checksum pruefen |
if (rx == chk_a) ublox_msg_state = UBLOX_CKB; |
else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler |
break; |
case UBLOX_CKB: // Checksum pruefen |
if (rx == chk_b) *ptr_pac_status = 1; // Paket ok |
ublox_msg_state = UBLOX_IDLE; |
break; |
default: |
ublox_msg_state = UBLOX_IDLE; |
break; |
} |
} |
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe |
short int GPS_CRTL(short int cmd) |
{ |
static unsigned int cnt; // Zaehler fuer diverse Verzoegerungen |
static long int delta_north,delta_east; // Mass fuer Distanz zur Sollposition |
signed int n; |
static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion |
signed int dist_frm_start_east,dist_frm_start_north; |
int amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil |
static signed int int_east,int_north; //Integrierer |
long int speed_east,speed_north; //Aktuelle Geschwindigkeit |
signed long int_east1,int_north1; |
int dist_east,dist_north; |
int diff_p; //Vom Modus abhaengige zusaetzliche Verstaerkung |
long ni,ro; // Nick und Roll Zwischenwerte |
switch (cmd) |
{ |
case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. |
if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE)) |
{ |
cnt++; |
if (cnt > 100) // erst nach Verzoegerung |
{ |
// Erst mal initialisieren |
cnt = 0; |
gps_tick = 0; |
hold_fast = 0; |
hold_reset_int = 0; // Integrator enablen |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
delta_east = 0, delta_north = 0; |
dist_flown = 0; |
gps_g2t_act_v = 0; |
gps_sub_state = GPS_CRTL_IDLE; |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
gps_rel_start_position.utm_east = gps_rel_act_position.utm_east; |
gps_rel_start_position.utm_north= gps_rel_act_position.utm_north; |
gps_rel_start_position.status = 1; // gueltige Positionsdaten |
gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
//Richtung zur Home Position bezogen auf Nordpol bestimmen |
hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north); |
// in Winkel 0...360 Grad umrechnen |
if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home); |
else hdng_2home = (270 - hdng_2home); |
dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen |
gps_state = GPS_CRTL_HOME_ACTIVE; |
return (GPS_STST_OK); |
} |
else |
{ |
gps_rel_start_position.status = 0; //Keine Daten verfuegbar |
gps_state = GPS_CRTL_IDLE; |
return(GPS_STST_ERR); // Keine Daten da |
} |
} |
else return(GPS_STST_PEND); // noch warten |
} |
break; |
// ****************************** |
case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
{ |
cnt++; |
if (cnt > 600) // erst nach Verzoegerung |
{ |
cnt = 0; |
// aktuelle positionsdaten abspeichern |
if (gps_rel_act_position.status > 0) |
{ |
hold_fast = 0; |
hold_reset_int = 0; // Integrator enablen |
int_east = 0, int_north = 0; |
gps_reg_x = 0, gps_reg_y = 0; |
delta_east = 0, delta_north = 0; |
speed_east = 0; speed_north= 0; |
// int_ovfl_cnt = 0; |
gps_quiet_cnt = 0; |
gps_rel_hold_position.utm_east = gps_rel_act_position.utm_east; |
gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north; |
gps_rel_hold_position.status = 1; // gueltige Positionsdaten |
gps_state = GPS_CRTL_HOLD_ACTIVE; |
return (GPS_STST_OK); |
} |
else |
{ |
gps_rel_hold_position.status = 0; //Keine Daten verfuegbar |
gps_state = GPS_CRTL_IDLE; |
return(GPS_STST_ERR); // Keine Daten da |
} |
} |
else return(GPS_STST_PEND); // noch warten |
} |
break; |
case GPS_CMD_STOP: // Lageregelung beenden |
cnt = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
gps_sub_state = GPS_CRTL_IDLE; |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_OK); |
break; |
default: |
return (GPS_STST_ERR); |
break; |
} |
switch (gps_state) |
{ |
case GPS_CRTL_IDLE: |
cnt = 0; |
return (GPS_STST_OK); |
break; |
case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis |
//Der Sollwert des Lagereglers wird der Homeposition angenaehert |
if (gps_rel_start_position.status >0) |
{ |
if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist |
{ |
gps_tick++; |
int d1,d2,d3; |
d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east ); |
d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north ); |
d3 = (dist_2home - dist_flown); // Restdistanz zum Ziel |
if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel |
{ |
if ((d1 < (GPS_G2T_FAST_TOL/2)) && (d2 < (GPS_G2T_FAST_TOL/2))) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz |
{ |
if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen |
dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
} |
else if ((d1 < GPS_G2T_FAST_TOL) && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz |
{ |
if (gps_g2t_act_v > (GPS_G2T_V_MAX/2)) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haelfte runter oder rauffahren |
else if (gps_g2t_act_v < (GPS_G2T_V_MAX/2)) gps_g2t_act_v += 1; |
dist_flown +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_FAST_IN_TOL; |
} |
else //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz |
{ |
if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren |
// dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
gps_sub_state = GPS_HOME_FAST_OUTOF_TOL; |
} |
hold_reset_int = 0; // Integrator einsschalten |
hold_fast = 1; // Regler fuer schnellen Flug |
dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
} |
else if (d3 > GPS_G2T_DIST_HOLD) //Das Ziel naehert sich, deswegen abbremsen |
{ |
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) |
{ |
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit |
gps_sub_state = GPS_HOME_RMPDWN_IN_TOL; |
} |
else |
{ |
dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen |
gps_sub_state = GPS_HOME_RMPDWN_OUTOF_TOL; |
} |
hold_reset_int = 0; // Integrator einsschalten |
hold_fast = 1; // Regler fuer schnellen Flug |
dist_frm_start_east = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000); |
dist_frm_start_north = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000); |
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt |
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt |
} |
else //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) |
{ |
if ((d1 < GPS_G2T_NRML_TOL) && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln |
{ |
gps_sub_state = GPS_HOME_IN_TOL; |
hold_fast = 0; // Wieder normal regeln |
hold_reset_int = 0; // Integrator einsschalten |
if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN; |
else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN; |
if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN; |
else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN; |
if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN)) |
{ |
gps_rel_hold_position.utm_east = 0; |
gps_rel_hold_position.utm_north = 0; |
gps_sub_state = GPS_HOME_FINISHED; |
} |
} |
else gps_sub_state = GPS_HOME_OUTOF_TOL; |
} |
} |
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung |
return (GPS_STST_OK); |
} |
else // Keine GPS Daten verfuegbar, deswegen Abbruch |
{ |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
} |
break; |
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen |
{ |
gps_quiet_cnt++; |
// ab hier wird geregelt |
delta_east = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east); |
delta_north = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north); |
int_east += (int)delta_east; |
int_north += (int)delta_north; |
speed_east = actual_speed.speed_e; |
speed_north = actual_speed.speed_n; |
gps_updte_flag = 0; // Neue Werte koennen vom GPS geholt werden |
dist_east = (int)delta_east; //merken |
dist_north = (int)delta_north; |
// #define GPSINT_MAX 3000 // Neues Verfahren ab 30.12.2007 bei Integratoroverflow |
long int gpsintmax; |
if (Parameter_NaviGpsI > 0) |
{ |
gpsintmax = (GPS_NICKROLL_MAX * gps_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen |
if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax)) |
{ |
// // = 1; // Zahl der Overflows zaehlen |
// int_ovfl_cnt -= 1; |
int_east = (int_east * 6)/8; // Wert reduzieren |
int_north = (int_north* 6)/8; |
} |
if (hold_reset_int > 0) //Im Schnellen Mode Integrator abschalten |
{ |
int_east = 0; |
int_north = 0; |
} |
} |
else // Integrator deaktiviert |
{ |
int_east = 0; |
int_north = 0; |
} |
debug_gp_4 = (int)int_east; // zum Debuggen |
debug_gp_5 = (int)int_north; // zum Debuggen |
//I Werte begrenzen |
#define INT1_MAX (GPS_NICKROLL_MAX * gps_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen |
int_east1 = ((((long)int_east) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
int_north1 = ((((long)int_north) * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT; |
if (int_east1 > INT1_MAX) int_east1 = INT1_MAX; //begrenzen |
else if (int_east1 < -INT1_MAX) int_east1 = -INT1_MAX; |
if (int_north1 > INT1_MAX) int_north1 = INT1_MAX; //begrenzen |
else if (int_north1 < -INT1_MAX) int_north1 = -INT1_MAX; |
if (hold_fast > 0) //schneller Coming Home Modus |
{ |
amplfy_speed_east = DIFF_Y_F_MAX; |
amplfy_speed_north = DIFF_Y_F_MAX; |
amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
speed_east = (speed_east * (long)amplfy_speed_east) /50; |
speed_north = (speed_north * (long)amplfy_speed_north)/50; |
// D Werte begrenzen |
#define D_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
if (speed_east > D_F_MAX) speed_east = D_F_MAX; |
else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX; |
if (speed_north > D_F_MAX) speed_north = D_F_MAX; |
else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX; |
diff_p = (Parameter_NaviGpsP * GPS_PROP_FAST_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
} |
else //langsamer Holdmodus |
{ |
amplfy_speed_east = DIFF_Y_N_MAX; |
amplfy_speed_north = DIFF_Y_N_MAX; |
amplfy_speed_east *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT); |
speed_east = (speed_east * (long)amplfy_speed_east) /25; |
speed_north = (speed_north * (long)amplfy_speed_north)/25; |
// D Werte begrenzen |
#define D_N_MAX (GPS_NICKROLL_MAX * gps_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen |
if (speed_east > D_N_MAX) speed_east = D_N_MAX; |
else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX; |
if (speed_north > D_N_MAX) speed_north = D_N_MAX; |
else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX; |
diff_p = (Parameter_NaviGpsP * GPS_PROP_NRML_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil |
} |
// debug_gp_4 = (int)speed_east; // zum Debuggen |
// debug_gp_5 = (int)speed_north; // zum Debuggen |
//P-Werte verstaerken |
delta_east = (delta_east * (long)diff_p)/(40); |
delta_north = (delta_north * (long)diff_p)/(40); |
if (hold_fast > 0) //schneller Coming Home Modus |
{ |
// P Werte begrenzen |
#define P1_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
if (delta_east > P1_F_MAX) delta_east = P1_F_MAX; |
else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX; |
if (delta_north > P1_F_MAX) delta_north = P1_F_MAX; |
else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX; |
} |
else // Hold modus |
{ |
// P Werte begrenzen |
#define P1_N_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen |
if (delta_east > P1_N_MAX) delta_east = P1_N_MAX; |
else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX; |
if (delta_north > P1_N_MAX) delta_north = P1_N_MAX; |
else if (delta_north < -P1_N_MAX) delta_north = -P1_N_MAX; |
} |
debug_gp_2 = (int)delta_east; // zum Debuggen |
debug_gp_3 = (int)delta_north; // zum Debuggen |
//PID Regler Werte aufsummieren |
gps_reg_x = -(int_east1 + delta_east + speed_east); // I + P +D Anteil X Achse |
gps_reg_y = -(int_north1 + delta_north + speed_north); // I + P +D Anteil Y Achse |
debug_gp_0 = (int)gps_reg_x; // zum Debuggen |
debug_gp_1 = (int)gps_reg_y; // zum Debuggen |
// Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen |
n = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter |
ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*gps_gain); |
ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*gps_gain); |
if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX); |
else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX ); |
if (ro > (GPS_NICKROLL_MAX )) ro = (GPS_NICKROLL_MAX ); |
else if (ro < -(GPS_NICKROLL_MAX)) ro = -(GPS_NICKROLL_MAX ); |
if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX)) // bei zu grossem Abstand abbrechen |
{ |
GPS_Roll = 0; |
GPS_Nick = 0; |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
break; |
} |
else if ((PPM_in[7] > 100) && (CAM_GPS_QUIET > 0) && (gps_quiet_cnt <=4) ) // Wenn Fotoausloeser gedruckt wird, GPS Stellwerte kurzzeitig auf 0 setzen |
{ |
gps_quiet_cnt++; |
GPS_Roll = 0; |
GPS_Nick = 0; |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
else if ((PPM_in[7] < 50) && (CAM_GPS_QUIET > 0) && (gps_quiet_cnt >= 4)) |
{ |
gps_quiet_cnt = 0; |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
else |
{ |
GPS_Roll = (int)ro; |
GPS_Nick = (int)ni; |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
} |
else |
{ |
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen |
return (GPS_STST_OK); |
} |
break; |
default: |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
break; |
} |
return (GPS_STST_ERR); |
} |
/branches/salvo_gps/Basis_v0070d/trunk/README_Gps_Deutsch.txt |
---|
0,0 → 1,88 |
********************************************************************* |
GPS Implementierung von Peter Muehlenbrock ("Salvo") für Mikrokopter/FlightCrtl |
Stand 28.10.2008 |
Verwendung der SW ohne Gewaehr. Siehe auch die Lizenzbedingungen in File Licensce_LPGL.txt und Licensce_GPL.txt |
Die Version basiert auf Holgers V00.70d. |
********************************************************************* |
Hardware-Voraussetzungen: |
Kalibrierter Kompass vom Typ CMPS03, waagrecht eingebaut |
GPS Modul vom Typ ublox, Die Meldungungen "NAV_STATUS", "NAV_POSUTM" und "NAV_VELNED" |
mussen mit 4 HZ Updaterate aktiviert sein. Alle Meldungen im NMEA Format muessen deaktivert sein. |
Anschluss an RX Port der FlightCRtl. Baudrate ist 57600 wie beim Kopter Tool. |
Software-Voraussetzungen: |
In timer0.h muss die Ausrichtung des Kompasses bezogen auf die Nordachse des Kopters |
eingetragen sein. Wenn KOMPASS_OFFSET > 0 wird dieser Wert genommen |
Wenn KOMPASS_OFFSET = 0 ist wird der UserParameter4 verwendet (=hexfile). Da die Userparameter nur ein Byte lang sind werden |
Offsets von 0...358 Grad durch einen Wert von 0...179 dargestellt. |
Betriebs-Voraussetzungen: |
Damit der Kompass sauber funktioniert, muss die waagrechte Lage (Gashebel Vollanschlag und Gier rechts) |
im Eeprom abgespeichert worden sein. |
Parametrierung: |
Der GPS Hold Regler ist ein PID Regler, der ueber die NaviCrtl Parameter gesteuert wird. |
GPS_P beschreibt den P(roportional)-Anteil, GPS_I den I(ntegral)-Anteil und GPS_D den D(ifferential)-Anteil. Mit GPS Gaimn kann die gesamtverstaerkung eingestellt worden |
Hier kann und muss gespielt werden.Alle Parameter koennen direkt im Mikrokoptertool in den Settings eingestellt werden. |
Der P-Anteil wirkt einer Lageänderung entgegen. Je größer er ist, desto geringer die Regelabweichung, desto höher aber auch die Schwingneigung. |
Der D-Anteil wirkt einer Geschwindigkeitsänderung entgegen und dient zur Reduzierung von Schwingungen, darf aber auch nicht zu gross sein. |
Der I-Anteil dient nur dazu die Lageabweichung auf Null zu bringen. |
P und D Anteil mussen gut aufeinander abgestimmt sein, damit die Einschwingzeit klein bei gleichzeitig geringer Schwingneigung ist. |
Standarddwerte für P,I,D sind 60,8,100 bzw 100 für GPS Gain |
Wenn alle 0 sind, ist der Regler deaktiviert. |
Voraussetzungen für GPS_Hold: |
Neben den genannten HW und SW Voraussetzungen muß beim Kalibrieren das GPS MOdul bereits Positionsdaten liefern. |
Nur dann wird die Home Position abgespeichert und nur dann wird GPS_Hold aktiviert. |
Kenntlich gemacht wird dies durch einen etwas längeren Piepser nach dem Kalibrieren. Die Rote Led auf der FlightCrtl blitzt bei jedem Empfang eines kompletten |
und korrekten Meldungsset ( "NAV_STATUS", "NAV_POSUTM" und "NAV_VELNED"). |
Folgende Möglichkeiten zur Aktivierung von GPS Hold gibt es: |
Das GPS Aktiv Flag sowie der Kompass im Setting sind aktiviert. |
Eine Ausrichtung des Kopters nach Nord etc. ist nicht erforderlich. |
Aktivierung GPS_Hold im Flug: |
Wenn alle genannten Voraussetzungen erfüllt sind, wird GPS Hold im Flug automatisch aktiviert, sobald der Nick und Roll Stick |
für ca. 500msec in Neutrallage sind. Weicht einer der Sticks davon ab (Parameter GPS_STICK_HOLDOFF) oder liefert das GPS Modul oder fällt der |
Kompass aus wird GPS_Hold sofort deaktiviert. |
Damit kann jederzeit die manuelle Kontrolle wieder übernommen werden. |
GPS_Hold regelt nur horizontale Abweichungen, keine Hoehe. |
Der Magnetkompass wird nur in waagrechter Lage dazu benutzt einen internen aus dem Giergyro ermittelten "Ersatz"kompasswert |
nach zu kalibrieren. Daher funktioniert die Bstimmung der Kopterausrichtung in jeder Lebenslage, sofern nur ab und zu |
der Magnetkompass in die waagrechte Lage kommt. Da der Giergyro per Kompass automatisch offsetkorrigiert wird, ist er spätestens nach |
einigen wenigen Minuten auch bei Temperatursprüngen ausreichend stabil. |
Ein 3D Kompass ist damit überflüssig. |
Die GPS Regelung benötigt zwar pro Durchlauf eine erhebliche Rechenzeit. Da sie aber nur einmal je neuem Meldungsblock vom |
GPS Modul (also alle 250ms) aufgerufen wird, wird die Gesamtbelastung der CPU nur minimal erhöht. |
Ein externes Naviboard ist daher nicht notwendig. |
GPS Rücksturz zur Basis (GPS Home) Funktion |
Voraussetzungen wie bei GPS Hold. |
Folgende Möglichkeiten zur Aktivierung von GPS Home gibt es: |
Das GPS Aktiv Flag und der Kompass im Setting sind aktiviert UND GPS Mode Control hat einen Wert > 170 |
Weitere Änderungen: |
-Der Ausgang PC6 kann benutzt werden um einen Externen Multiplexer zu schalten. Dieser schaltet entweder die GPS Daten |
(Wenn Motoren eingeschaltet sind) oder den Ausgang des Bluetoothmoduls (wenn Motoren ausgeschaltet sind) auf den RX |
Eingang der FlightCrtl. Damit kann eine zweite UART Schnittstelle bzw. die manuelle Umschaltung entfallen |
-Bei vorhandenem Kompass wird eine automatische Kompensation der Giergyrodrift durchgeführt. |
/branches/salvo_gps/Basis_v0070d/trunk/README_Gps_English.txt |
---|
0,0 → 1,66 |
********************************************************************* |
GPS Implementation by Peter Muehlenbrock ("Salvo") for Mikrokopter/FlightCrtl |
As of October 28th, 2008 |
Please note the files Licensce_LPGL.txt and Licensce_GPL.txt |
This SW is based on Holgers V00.70d. |
********************************************************************* |
Hardware-requirements: |
Calibrated compass of type CMPS03, horizontally adjusted |
GPS module of type ublox, The messages "NAV_STATUS", "NAV_POSUTM" and "NAV_VELNED" |
must be activated with 4 HZ update rate. All messages of type NMEA should be disabled. |
The TX port of the GPS module must be connected to the RX input of the Flight Crtl. Baudrate is 57600 (like Kopter Tool). |
UserParameter 4 tells the software the orientation for the compass. The difference between the "north" direction |
( = Motor "vorne" or 1 as described in Holgers Flight Crtl manual) of the kopter |
and the north direction of the compass is represented as follows: 0...360 degree offset corresponds to a value of 0...180. |
(A userparameter ist only 1 byte long and therefore can not directly represent 0..360 degree). Please check the |
correct value via the Koptertool. The "Kompass" debug value has to show the correct orientation: |
kopter Motor vorne orientated to north => value approx 0 Degree) |
Other requirements: |
Please calibrate the ACC Sensor values (pitch full, yaw full right) in horizontal orientation of the kopter |
as exactly as possible. |
Parameters: |
The GPS Hold regulator is of type PID: GPS-P =P(proportional), GPS-I = I(ntegral), |
GPS-D = D(ifferential). Standardvalues for P,I and D are 60,8,100. GPS Gain is the overall gain and set to 100 as default |
The I-Part may be set to 0 if a small deviation from hold position is acceptable. The D-Part is important to avoid |
oscillating. |
Please find out the best values yourself. |
It might be useful to put the P and D Part on potis and test the behaviour in flight. |
Requirements for GPS_Hold: |
The gps module has do provide a 3D-fix. If succesfull, you can hear a longer beep when performing a gyro calibration. |
Without a succesfull position fix at calibration the GPS functionality ist completely disabled. |
The red led on the FlightCrtl flashes with a 4 Hz rate if the gps module delivers a position fix. |
The GPS flag and Compass flag in the setting must be enabled |
Enabling GPS_Hold in flight: |
GPS Hold is automatically activated if the sticks for roll and nick are in neutral position for about 400ms. |
Moving the stick immediately disables the Hold mode. The GPS Hold function doesn not change height, pitch or yaw. |
GPS Coming-Home function |
Enabled when the GPS and compass flag in the setting are enabled and the GPS Mode Control Switch is set to a value > 170 |
Other changes: |
- Output PC6 can be used to switch TX data between a Bluetooth and the GPS Module (further hardware required) to the RX input of the FlightCrtl |
- automatic Yaw gyro compensation if compass is available. |
/branches/salvo_gps/Basis_v0070d/trunk/eeprom.c |
---|
5,6 → 5,10 |
// + 252 -> Poti2 |
// + 253 -> Poti3 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "fc.h" |
#include "_settings.h" |
#include "main.h" |
void DefaultKonstanten1(void) |
{ |
EE_Parameter.Kanalbelegung[K_NICK] = 1; |
31,7 → 35,7 |
EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
EE_Parameter.Gyro_P = 80; // Wert : 0-250 |
EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
EE_Parameter.NotGasZeit = 30; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
67,8 → 71,8 |
EE_Parameter.J17Timing = 15; |
EE_Parameter.NaviGpsModeControl = 253; |
EE_Parameter.NaviGpsGain = 100; |
EE_Parameter.NaviGpsP = 90; |
EE_Parameter.NaviGpsI = 90; |
EE_Parameter.NaviGpsP = 60; |
EE_Parameter.NaviGpsI = 0; |
EE_Parameter.NaviGpsD = 90; |
EE_Parameter.NaviGpsACC = 0; |
EE_Parameter.NaviGpsMinSat = 6; |
138,7 → 142,7 |
EE_Parameter.NaviGpsModeControl = 253; |
EE_Parameter.NaviGpsGain = 100; |
EE_Parameter.NaviGpsP = 90; |
EE_Parameter.NaviGpsI = 90; |
EE_Parameter.NaviGpsI = 16; |
EE_Parameter.NaviGpsD = 90; |
EE_Parameter.NaviGpsACC = 0; |
EE_Parameter.NaviGpsMinSat = 6; |
/branches/salvo_gps/Basis_v0070d/trunk/fc.c |
---|
51,10 → 51,17 |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 9.11.2008 |
/* |
Driftkompensation fuer Gyros verbessert |
Linearsensor optional mit fixem Neutralwert |
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion |
*/ |
#include "main.h" |
#include "eeprom.c" |
unsigned char h,m,s; |
volatile unsigned int I2CTimeout = 100; |
volatile int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias; |
85,6 → 92,25 |
long ErsatzKompass; |
int ErsatzKompassInGrad; // Kompasswert in Grad |
int GierGyroFehler = 0; |
//Salvo 12.10.2007 |
uint8_t magkompass_ok=0; |
uint8_t gps_cmd = GPS_CMD_STOP; |
static int ubat_cnt =0; |
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
int w,v; |
//Salvo End |
//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation |
long GyroKomp_Int; |
long int GyroGier_Comp; |
int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
short int cnt_stickgier_zero =0; |
int gyrogier_kompass; |
//Salvo End |
//Salvo 2.1.2008 Allgemeine Debugvariablen |
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
//Salvo End |
float GyroFaktor; |
float IntegralFaktor; |
volatile int DiffNick,DiffRoll; |
131,6 → 157,7 |
unsigned char Parameter_NaviGpsP; |
unsigned char Parameter_NaviGpsI; |
unsigned char Parameter_NaviGpsD; |
unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008 |
unsigned char Parameter_NaviGpsACC; |
unsigned char Parameter_ExternalControl; |
struct mk_param_struct EE_Parameter; |
139,6 → 166,13 |
unsigned int modell_fliegt = 0; |
unsigned char MikroKopterFlags = 0; |
//Salvo 2.1.2008 Allgemeine Debugvariablen |
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
//Salvo End |
void Piep(unsigned char Anzahl) |
{ |
while(Anzahl--) |
154,6 → 188,9 |
void SetNeutral(void) |
//############################################################################ |
{ |
// Salvo 9.12.2007 |
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
// Salvo End |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
213,6 → 250,13 |
SendVersionToNavi = 1; |
LED_Init(); |
MikroKopterFlags |= FLAG_CALIBRATE; |
//Salvo 13.10.2007 Ersatzkompass und Gas |
GyroKomp_Int = KompassValue * GIER_GRAD_FAKTOR; //Neu ab 15.10.2008 |
// gas_mittel = 30; |
// gas_actual = gas_mittel; |
// Salvo End |
} |
//############################################################################ |
227,7 → 271,7 |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
//DebugOut.Analog[26] = MesswertNick; |
DebugOut.Analog[28] = MesswertRoll; |
//DebugOut.Analog[28] = MesswertRoll; |
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
241,6 → 285,10 |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
ErsatzKompass += MesswertGier; |
//Salvo 12.11.2007 |
GyroKomp_Int += (long)MesswertGier; |
GyroGier_Comp += (long)MesswertGier; |
//Salvo End |
Mess_Integral_Gier += MesswertGier; |
// Mess_Integral_Gier2 += MesswertGier; |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
436,9 → 484,11 |
CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
CHK_POTI(Parameter_NaviStickThreshold,EE_Parameter.NaviStickThreshold,0,255); //Salvo 16.10.2008 |
CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
Ki = (float) Parameter_I_Faktor * 0.0001; |
MAX_GAS = EE_Parameter.Gas_Max; |
MIN_GAS = EE_Parameter.Gas_Min; |
467,7 → 517,15 |
static long ausgleichNick, ausgleichRoll; |
Mittelwert(); |
//****** GPS Daten holen *************** |
short int n; |
if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout) |
n = Get_Rel_Position(); |
if (n == 0) |
{ |
ROT_ON; //led blitzen lassen |
} |
//******PROVISORISCH*************** |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
541,6 → 599,7 |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetNeutral(); |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
569,9 → 628,16 |
} |
SetNeutral(); |
Piep(GetActiveParamSetNumber()); |
GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
if (gps_home_position.status > 0 ) |
{ |
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
beeptime = 1000; |
Delay_ms(500); |
} |
} |
} |
} |
else |
if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
{ |
607,6 → 673,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
int n; |
// Salvo 9.12.2007 |
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
// Salvo End |
delay_einschalten = 200; |
modell_fliegt = 1; |
MotorenEin = 1; |
619,6 → 689,8 |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
MikroKopterFlags |= FLAG_START; |
} |
} |
631,6 → 703,10 |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
// Salvo 9.12.2007 |
RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten |
// Salvo End |
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
650,13 → 726,16 |
ParameterZuordnung(); |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
// StickNick = stick_nick - (GPS_Nick*(STICK_GAIN/2) + GPS_Nick2); //Salvo 23.10.2008 |
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
// StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
// StickRoll = stick_roll - (GPS_Roll*(STICK_GAIN/2) + GPS_Roll2); //Salvo 23.10.2008 |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
// StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
885,10 → 964,15 |
if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
DebugOut.Analog[22] = MittelIntegralRoll / 26; |
//DebugOut.Analog[22] = MittelIntegralRoll / 26; |
//DebugOut.Analog[24] = GierGyroFehler; |
GierGyroFehler = 0; |
//Salvo Ersatzkompass Ueberlauf korrigieren |
if (GyroKomp_Int >= ((long)360 * GIER_GRAD_FAKTOR)) GyroKomp_Int = GyroKomp_Int - (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
ROT_OFF; |
// Salvo End |
/*DebugOut.Analog[17] = IntegralAccNick / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
938,10 → 1022,16 |
cnt = 0; |
KompassSignalSchlecht = 1000; |
} |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
//Salvo 16.10.2008 |
if ((w < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehernd waagrechter Lage nachtrimmen |
{ |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
} |
//Salvo End |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
975,11 → 1065,16 |
cnt = 0; |
KompassSignalSchlecht = 1000; |
} |
//Salvo 26.12.2007 |
if ((w < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur in annaehernd waagrechter Lage nachtrimmen |
{ |
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
} |
//Salvo End |
} |
else |
{ |
LageKorrekturRoll = 0; |
1002,7 → 1097,117 |
ZaehlMessungen = 0; |
} |
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
{ |
Kompass_Neuer_Wert = 0; |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
{ |
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
{ |
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
{ |
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da |
{ |
w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
if (v <-180) v +=360; // Uberlaufkorrektur |
if (v > 180) v -=360; // Uberlaufkorrektur |
v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler |
#define GIER_COMP_MAX 2 |
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
if (abs(w) > 1) |
{ |
GyroGier_Comp = 0; |
gyrogier_kompass = KompassValue; // Kompasswert merken |
AdNeutralGier -= v; |
} |
} |
} |
else |
{ |
gyrogier_kompass = KompassValue; // Kompasswert merken |
cnt_stickgier_zero = 0; |
GyroGier_Comp = 0; |
} |
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR; |
w = KompassValue - GyroKomp_Int; |
if ((w > 0) && (w < 180)) |
{ |
++GyroKomp_Int; |
} |
else if ((w > 0) && (w >= 180)) |
{ |
--GyroKomp_Int; |
} |
else if ((w < 0) && (w >= -180)) |
{ |
--GyroKomp_Int; |
} |
else if ((w < 0) && (w < -180)) |
{ |
++GyroKomp_Int; |
} |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L; |
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
} |
} |
else //Kompassfehler |
{ |
magkompass_ok = 0; |
GyroGier_Comp = 0; |
} |
Kompass_Value_Old = KompassValue; |
} |
// Salvo End ************************* |
// Salvo 6.10.2007 |
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
{ |
if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOME; |
n = GPS_CRTL(gps_cmd); |
} |
else if ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOLD; |
n= GPS_CRTL(gps_cmd); |
} |
else // GPS komplett aus |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
} |
else |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12) LED_J16_OFF; //led im GPS Mode schnell blinken lassen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1009,65 → 1214,41 |
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
if(abs(StickGier) > 15) // war 35 |
{ |
KompassSignalSchlecht = 1000; |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
{ |
NeueKompassRichtungMerken = 1; |
}; |
if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
} |
tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
sollGier = tmp_int; |
Mess_Integral_Gier -= tmp_int; |
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
if(Mess_Integral_Gier > GIER_INTEGRAL_MAX) Mess_Integral_Gier = GIER_INTEGRAL_MAX; // begrenzen Salvo 18.10.2008 |
if(Mess_Integral_Gier <-GIER_INTEGRAL_MAX) Mess_Integral_Gier =-GIER_INTEGRAL_MAX; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0)) |
{ |
int w,v,r,fehler,korrektur; |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
korrektur = w / 8 + 1; |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
//DebugOut.Analog[25] = KompassSignalSchlecht; |
if(!KompassSignalSchlecht && w < 25) |
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
{ |
GierGyroFehler += fehler; |
if(NeueKompassRichtungMerken) |
{ |
beeptime = 200; |
// KompassStartwert = KompassValue; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
} |
ErsatzKompass += (fehler * 8) / korrektur; |
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
// Salvo 13.9.2007 |
w=0; |
// Salvo End |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w >= 0) |
if(w > 0) |
{ |
if(!KompassSignalSchlecht) |
{ |
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
// r = KompassRichtung; |
v = (r * w) / v; // nach Kompass ausrichten |
w = 3 * Parameter_KompassWirkung; |
if(v > w) v = w; // Begrenzen |
else |
if(v < -w) v = -w; |
Mess_Integral_Gier += v; |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
// Salvo End |
} |
if(KompassSignalSchlecht) KompassSignalSchlecht--; |
} |
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1076,6 → 1257,11 |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
// Salvo 13.12.2007 Beleuchtung steuern |
if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
else if (MotorenEin) LED_J16_ON; |
else LED_J16_OFF; |
// Salvo End |
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
1083,24 → 1269,34 |
DebugOut.Analog[3] = Mittelwert_AccRoll; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
// DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR; |
//DebugOut.Analog[16] = Mittelwert_AccHoch; |
// DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
// DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
// DebugOut.Analog[19] = WinkelOut.CalcState; |
// DebugOut.Analog[20] = ServoValue; |
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
DebugOut.Analog[23] = debug_gp_0; |
DebugOut.Analog[24] = debug_gp_1; |
DebugOut.Analog[25] = debug_gp_2; |
DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |
1206,6 → 1402,8 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define MUL_G 1.0 |
GierMischanteil = MesswertGier - sollGier * STICK_GAIN; // Regler für Gier |
// GierMischanteil = 0; |
#define MIN_GIERGAS (40*STICK_GAIN) // unter diesem Gaswert trotzdem Gieren |
if(GasMischanteil > MIN_GIERGAS) |
1221,6 → 1419,7 |
tmp_int = MAX_GAS*STICK_GAIN; |
if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
DebugOut.Analog[6] = GierMischanteil; //Salvo 19.10.2008 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Achse |
/branches/salvo_gps/Basis_v0070d/trunk/fc.h |
---|
4,10 → 4,22 |
#ifndef _FC_H |
#define _FC_H |
//#define GIER_GRAD_FAKTOR 1450L // Abhängigkeit wzischen GyroIntegral und Winkel |
#define GIER_GRAD_FAKTOR 1550L // Abhängigkeit wzischen GyroIntegral und Winkel |
#define GIER_GRAD_FAKTOR 1450L // Abhängigkeit wzischen GyroIntegral und Winkel |
//#define GIER_GRAD_FAKTOR 1550L // Abhängigkeit zwischen GyroIntegral und Winkel |
#define STICK_GAIN 4 |
//Salvo 9.12.2007 Neutralwerte fuer ACC Sensor nur verwendet wenn ACC_FIXED >0 |
#define ACC_FIXED 0 // wenn > 0werden diese Werte beim ACC Kalbibrieren ins Eeprom geschrieben |
#define ACC_WAAGRECHT_LIMIT 100 // Nick und Roll kleiner als dieser Wert gelten als Kriterium fuer waagrechte Lage |
//Salvo 2.9.2007 Ersatzkompass: Gyroincrements/Grad als Defaultwert ***** |
// Laut Datenblatt sind di; Werte ueber Zeit und Temperatur sehr stabil. |
//#define GYROKOMP_INC_GRAD_DEFAULT 1300 // Gyroincrements/Grad als Defaultwert |
#define CAM_GPS_QUIET 0 // wenn dieses Flag gesetzt ist, wird GPS beim Ausloesen kurzzeitig deaktiviert um eine ruhige Lage zu bekommen |
// Salvo End |
#define GIER_INTEGRAL_MAX 50000 //Salvo 18.10.2008 Gier Integrierer macht Probleme (links /rechts unterschiedlich) Liegt an schraeg stehenden |
//Motoren und dann in die begrenzung laufenden Giergyro |
#define FLAG_MOTOR_RUN 1 |
#define FLAG_FLY 2 |
#define FLAG_CALIBRATE 4 |
61,6 → 73,17 |
extern void DefaultKonstanten1(void); |
extern void DefaultKonstanten2(void); |
//Salvo 2.1.2008 Debugvariablens |
extern int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; //Allgemeine Debugvariablen |
//Salvo End |
//Salvo 2.9.2007 Ersatzkompass |
extern long GyroKomp_Int; |
extern long int GyroGier_Comp; |
extern int GyroKomp_Inc_Grad; |
extern int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
// Salvo End |
#define STRUCT_PARAM_LAENGE 83 |
struct mk_param_struct |
{ |
/branches/salvo_gps/Basis_v0070d/trunk/gps.h |
---|
1,7 → 1,132 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Peter Muehlenbrock alias Salvo |
// Definitionen fuer Modul GPS |
// Stand 20.1.2008 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
extern signed int GPS_Nick; |
extern signed int GPS_Roll; |
extern signed int GPS_Nick2; |
extern signed int GPS_Roll2; |
extern void GPS_Neutral(void); |
extern void Get_Ublox_Msg(uint8_t rx) ; |
extern short int Get_GPS_data(void); |
extern short int Get_Rel_Position(void); |
extern void GPS_Save_Home(void); |
extern short int GPS_CRTL(short int cmd); |
void GPS_Neutral(void); |
void GPS_BerechneZielrichtung(void); |
typedef struct { |
unsigned long utm_itow; // time of week |
long utm_east; // UTM Ost in cm |
long utm_north; // UTM Nord in cm |
long utm_alt; // hoehe in cm |
uint8_t utm_zone; // |
uint8_t utm_hem; // Hemisphere Indicator |
uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
} NAV_POSUTM_t; |
typedef struct { |
unsigned long itow; // time of week |
uint8_t gpsfix_type;// 3=3D Fix |
uint8_t nav_status_flag; |
uint8_t nav_diff_status; |
uint8_t nav_resevd; |
long nav_tff; // Time to First Fix in ms |
long nav_msss; // ms since startup |
uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
} NAV_STATUS_t; |
typedef struct { |
unsigned long itow; |
long speed_n; // in cm/s |
long speed_e; // in cm/s |
long speed_alt; // in cm/s |
unsigned long speed_3d; // in cm/s |
unsigned long speed_gnd; // V ueber Grund in cm/s |
long heading; // Richtung in deg/10000 |
unsigned long sacc; // Speed Genauigkeit in cm/s |
unsigned long cacc; // Richtungsgenauigkeit in deg |
uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
} NAV_VELNED_t; |
typedef struct { |
long utm_east; // UTM Ost in 10 cm |
long utm_north; // UTM Nord in 10 cm |
long utm_alt; // hoehe in 10 cm |
unsigned long speed_gnd; // V ueber Grund in 10cm/s |
unsigned heading; // Richtung in Grad |
uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
} GPS_ABS_POSITION_t; |
typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position) |
int utm_east; // UTM Ost in 10 cm |
int utm_north; // UTM Nord in 10 cm |
int utm_alt ; // UTM Altitude in 10 cm |
uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
} GPS_REL_POSITION_t; |
extern GPS_ABS_POSITION_t gps_act_position; |
extern GPS_ABS_POSITION_t gps_home_position; |
extern GPS_REL_POSITION_t gps_rel_act_position; |
extern GPS_REL_POSITION_t gps_rel_hold_position; |
extern short int gps_state,gps_sub_state; |
extern unsigned int gps_alive_cnt; |
// Zustaende der zentralen GPS statemachine |
#define GPS_CRTL_IDLE 0 // Inaktiv |
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
#define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
#define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
#define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
#define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz |
#define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz |
#define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz |
#define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen |
// Kommandokonstanten fuer die zentrale GPS Statemachine |
#define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden |
#define GPS_CMD_REQ_INIT 1 // Initialisierung |
#define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
#define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
// Statusmeldungen der zentralen GPS statemachine |
#define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
#define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt |
#define GPS_STST_ERR 2 // Fehler |
// GPS Lageregler |
#define GPS_USR_PAR_FKT 4 //Faktor durch den die Userparameter geteilt werden |
#define GPS_NICKROLL_MAX 40 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
#define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
//#define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
// Konstanten fuer Verstaerkung fuer Speed Werte in Abhaengigkeit vom SpeedWert (cm/sek) |
// um eine exponentielle Verstaerkung zu erreichen |
#define DIFF_Y_N_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_N_MAX im Normal (Hold) Mode |
//#define DIFF_X_N_MAX 200 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_N_MAX |
#define DIFF_Y_F_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_F_MAX im Fast (Coming Home) Mode |
//#define DIFF_X_F_MAX 500 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_F_MAX |
// P-Regler Verstaerkung |
#define GPS_PROP_NRML_V 2 //maximale Verstaerkung im Normalen Holdmode |
#define GPS_PROP_FAST_V 6 //maximale Verstaerkung im Fast mode |
// GPS G2T /Go to Target Regler |
#define GPS_G2T_DIST_MAX_STOP 80 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
#define GPS_G2T_DIST_HOLD 50 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
#define GPS_G2T_FAST_TOL 200 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_NRML_TOL 100 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
#define GPS_G2T_V_MAX 20 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
#define GPS_G2T_V_RAMP_DWN 10 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
#define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
/branches/salvo_gps/Basis_v0070d/trunk/main.c |
---|
123,7 → 123,10 |
else PlatinenVersion = 11; |
} |
else PlatinenVersion = 10; |
DDRC = 0x81; // SCL |
// Salvo 9.12.2007 PC6 als Ausgang |
DDRC = 0xCD; // SCL und PC2,PC3, PC6 als Ausgang |
// Salvo End |
// DDRC = 0x81; // SCL |
PORTC = 0xff; // Pullup SDA |
DDRB = 0x1B; // LEDs und Druckoffset |
PORTB = 0x01; // LED_Rot |
/branches/salvo_gps/Basis_v0070d/trunk/main.h |
---|
19,6 → 19,15 |
#define GRN_ON {if(PlatinenVersion < 12) PORTB |= 0x02; else PORTB &=~0x02;} |
#define GRN_FLASH PORTB ^= 0x02 |
//Salvo 9.12.2007 Umschaltsignal fuer Bluetooth bzw. GPS Daten auf PC7 |
#define RX_SWTCH_ON PORTC |= 0x40 |
#define RX_SWTCH_OFF PORTC &= ~0x40 |
#define LED_J16_ON PORTC |= 0x04 |
#define LED_J16_OFF PORTC &= ~0x04 |
#define LED_J16_FLASH PORTC ^= 0x04 |
//Salvo End |
#define F_CPU SYSCLK |
//#ifndef F_CPU |
//#error ################## F_CPU nicht definiert oder ungültig ############# |
/branches/salvo_gps/Basis_v0070d/trunk/makefile |
---|
4,8 → 4,8 |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
HAUPT_VERSION = 0 |
NEBEN_VERSION = 70 |
VERSION_INDEX = 3 |
NEBEN_VERSION = 7 |
VERSION_INDEX = 4 |
VERSION_KOMPATIBEL = 8 # PC-Kompatibilität |
#------------------------------------------------------------------- |
83,7 → 83,7 |
########################################################################################################## |
# 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 = main.c uart.c printf_P.c timer0.c analog.c menu.c math.c |
SRC += twimaster.c rc.c fc.c GPS.c spi.c led.c |
########################################################################################################## |
390,6 → 390,7 |
@echo |
@echo $(MSG_CLEANING) |
# $(REMOVE) $(TARGET).hex |
$(REMOVE) $(TARGET).bak |
$(REMOVE) $(TARGET).eep |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).cof |
/branches/salvo_gps/Basis_v0070d/trunk/menu.c |
---|
38,6 → 38,7 |
case 0: |
LCD_printfxy(0,0,"+ MikroKopter +"); |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",PlatinenVersion/10,PlatinenVersion%10,VERSION_HAUPTVERSION, VERSION_NEBENVERSION,VERSION_INDEX+'a'); |
// LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",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--; |
/branches/salvo_gps/Basis_v0070d/trunk/spi.h |
---|
4,7 → 4,7 |
#include <util/delay.h> |
#define USE_SPI_COMMUNICATION |
//#define USE_SPI_COMMUNICATION //Salvo 17.10.2008 SPI deaktiviert |
#define SPI_PROTOCOL_COMP 1 |
/branches/salvo_gps/Basis_v0070d/trunk/timer0.c |
---|
10,7 → 10,13 |
unsigned int BeepMuster = 0xffff; |
unsigned int ServoValue = 0; |
//Salvo 8.9.2007 |
volatile uint8_t Kompass_Neuer_Wert= 0; |
volatile unsigned int Kompass_Value_Old = 0; |
// Salvo End |
//Salvo 21.9.2007 |
short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist |
// Salvo End |
enum { |
STOP = 0, |
CK = 1, |
22,7 → 28,10 |
T0_RISING_EDGE = 7 |
}; |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 24.12.2007 |
/* |
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion |
*/ |
SIGNAL (SIG_OVERFLOW0) // 8kHz |
{ |
static unsigned char cnt_1ms = 1,cnt = 0; |
31,6 → 40,7 |
if(SendSPI) SendSPI--; |
if(!cnt--) |
{ |
if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
65,7 → 75,7 |
else PORTC &= ~(1<<7); |
} |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) //Salvo 25.10.2008 |
{ |
if(PINC & 0x10) |
{ |
77,10 → 87,28 |
{ |
cntKompass += cntKompass / 41; |
if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0; |
// Salvo Kompassoffset 23.12.2007 *********** |
Kompass_present = 255; |
// Kompass_Value_Old = KompassValue; |
if (KOMPASS_OFFSET > 0) KompassValue = cntKompass -KOMPASS_OFFSET; |
else KompassValue = cntKompass - ((int) (Parameter_UserParam4*2)); |
if (KompassValue < 0) |
{ |
KompassValue += 360; |
} |
if (KompassValue >= 360) |
{ |
KompassValue -= 360; |
} |
// Salvo End |
} |
// if(cntKompass < 10) cntKompass = 10; |
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L; |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses |
Kompass_Neuer_Wert = 1; |
// Salvo End |
cntKompass = 0; |
} |
} |
/branches/salvo_gps/Basis_v0070d/trunk/timer0.h |
---|
2,6 → 2,12 |
#define TIMER_TEILER CK8 |
#define TIMER_RELOAD_VALUE 250 |
// Salvo Kompassoffset 24.12.2007 *********** |
#define KOMPASS_OFFSET 135// Winkel zwischen Nordachse Kopter und Nordachse Kompass |
//#define KOMPASS_OFFSET 0 // Winkel zwischen Nordachse Kopter und Nordachse Kompass |
// Wenn 0 wird der UserParameter4 fuer den Offset verwendet (0..360 Grad entspricht 0 ..180) |
// Salvo End |
void Timer_Init(void); |
void Delay_ms(unsigned int); |
void Delay_ms_Mess(unsigned int); |
15,3 → 21,8 |
extern unsigned int ServoValue; |
extern unsigned int BeepMuster; |
extern volatile unsigned char SendSPI; |
//Salvo 21.9.2007 |
extern volatile uint8_t Kompass_Neuer_Wert; |
extern volatile unsigned int Kompass_Value_Old; |
extern unsigned short int Kompass_present; |
// Salvo End |
/branches/salvo_gps/Basis_v0070d/trunk/uart.c |
---|
43,11 → 43,11 |
"AccRoll ", |
"GyroGier ", |
"HoehenWert ", //5 |
"AccZ ", |
"Giermischanteil ", |
"Gas ", |
"KompassValue ", |
"Spannung ", |
"Empfang ", //10 |
"Mess_integ_gier ", //10 |
"Ersatzkompass ", |
"Motor_Vorne ", |
"Motor_Hinten ", |
54,19 → 54,19 |
"Motor_Links ", |
"Motor_Rechts ", //15 |
" ", |
"Distance ", |
"OsdBar ", |
"MK3Mag CalState ", |
"Servo ", //20 |
"Nick ", |
"Roll ", |
" ", |
" ", |
" ", //25 |
" ", |
" ", |
" ", |
" ", |
" ", //20 |
"Nick ", |
"Roll ", |
"debug_0 ", |
"debug_1 ", |
"debug_2 ", //25 |
"utm_east ", |
"utm_north ", |
"utm_alt ", |
"GPS_State ", |
"GPS_Nick ", //30 |
"GPS_Roll " |
}; |
105,6 → 105,10 |
unsigned char CrcOkay = 0; |
SioTmp = UDR; |
//Salvo 11.9.2007 GPS Daten holen |
Get_Ublox_Msg(SioTmp); // Daten vom GPS Modul holen |
// Salvo End |
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0; |
if(SioTmp == '\r' && UartState == 2) |
{ |