Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1086 → Rev 1085

/branches/salvo_gps/Basis_V0071h/trunk/README_Gps_Deutsch.txt
File deleted
/branches/salvo_gps/Basis_V0071h/trunk/README_Gps_English.txt
File deleted
/branches/salvo_gps/Basis_V0071h/trunk/Flight-Ctrl_MEGA644_V0_71h.hex
File deleted
/branches/salvo_gps/Basis_V0071h/trunk/License_LPGL.txt
File deleted
\ No newline at end of file
/branches/salvo_gps/Basis_V0071h/trunk/math.c
File deleted
/branches/salvo_gps/Basis_V0071h/trunk/math.h
File deleted
/branches/salvo_gps/Basis_V0071h/trunk/FlightCtrl.aps
1,0 → 0,0
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>28-Dec-2008 05:47:18</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\v0071h\</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><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="0" orderaddress="0" ordergroup="0"/></IOView><Files><File00000><FileId>00000</FileId><FileName>main.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>_Settings.h</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>uart.c</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>fc.c</FileName><Status>1</Status></File00003></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
<AVRStudio><MANAGEMENT><ProjectName>FlightCtrl</ProjectName><Created>15-May-2007 11:20:41</Created><LastEdit>11-Oct-2007 22:58:54</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>15-May-2007 11:20:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\Flight-Ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>F:\SVN\MikroKopter\FlightCtrl\branches\V0.64_ZeroWarnings\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega644.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><HEADERFILE>uart.h</HEADERFILE><HEADERFILE>_Settings.h</HEADERFILE><HEADERFILE>analog.h</HEADERFILE><HEADERFILE>fc.h</HEADERFILE><HEADERFILE>gps.h</HEADERFILE><HEADERFILE>main.h</HEADERFILE><HEADERFILE>menu.h</HEADERFILE><HEADERFILE>old_macros.h</HEADERFILE><HEADERFILE>printf_P.h</HEADERFILE><HEADERFILE>rc.h</HEADERFILE><HEADERFILE>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega644</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>Flight-Ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS><OPTION><FILE>GPS.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>analog.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>eeprom.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>fc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>menu.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>printf_P.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>rc.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>timer0.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>twimaster.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>uart.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS><LIB>libc.a</LIB><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -Wstrict-prototypes -std=gnu99 -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -DVERSION_HAUPTVERSION=0 -DVERSION_NEBENVERSION=64 -DVERSION_KOMPATIBEL=5</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Program Files\WinAVR\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Program Files\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>main.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>uart.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>menu.c</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>timer0.c</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>fc.c</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>fc.h</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>menu.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>TWIMASTER.C</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>twimaster.h</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>uart.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>_Settings.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>analog.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>gps.h</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>main.h</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>old_macros.h</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>printf_P.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>rc.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>Settings.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>timer0.h</FileName><Status>1</Status></File00018></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
/branches/salvo_gps/Basis_V0071h/trunk/GPS.c
1,697 → 1,32
/*
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 7.12.2008
 
Aenderung 7.12.2008: an Coming Home geschraubt
Aenderung 27.11.2008: gps_gain erhoeht
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "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;
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 long int limit_gain; // Teilerfaktor Regelabweichung zu Ausgabewert
static int gps_gain ; // Verstaerkunsgfaktor*10
 
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
signed int GPS_Nick = 0;
signed int GPS_Roll = 0;
signed int GPS_Nick2 = 0;
signed int GPS_Roll2 = 0;
long GpsAktuell_X = 0;
long GpsAktuell_Y = 0;
long GpsZiel_X = 0;
long GpsZiel_Y = 0;
void GPS_Neutral(void)
{
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;
 
GpsZiel_X = GpsAktuell_X;
GpsZiel_Y = GpsAktuell_Y;
}
 
// Home Position sichern falls Daten verfuegbar sind.
void GPS_Save_Home(void)
void GPS_BerechneZielrichtung(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
}
GPS_Nick = 0;
GPS_Roll = 0;
}
 
// 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/5;
limit_gain = 8;
// debug_gp_0 = (int)limit_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-1) gps_g2t_act_v += 2; //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 = 1; // Integrator aussschalten
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*gps_gain)/10;
int_north += (int)(delta_north*gps_gain)/10;
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 * limit_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 * limit_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*gps_gain) /400;
speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/400;
// D Werte begrenzen
#define D_F_MAX (GPS_NICKROLL_MAX * limit_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*gps_gain) /250;
speed_north = (speed_north * (long)amplfy_speed_north*gps_gain)/250;
// D Werte begrenzen
#define D_N_MAX (GPS_NICKROLL_MAX * limit_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*gps_gain)/(400);
delta_north = (delta_north * (long)diff_p*gps_gain)/(400);
 
if (hold_fast > 0) //schneller Coming Home Modus
{
// P Werte begrenzen
#define P1_F_MAX (GPS_NICKROLL_MAX * limit_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 * limit_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)))/(1000L*(long)limit_gain);
ro = ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000L*(long)limit_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_V0071h/trunk/fc.c
51,12 → 51,6
// + 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"
91,25 → 85,6
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;
157,7 → 132,6
unsigned char Parameter_NaviGpsI;
unsigned char Parameter_NaviGpsD;
unsigned char Parameter_NaviGpsACC;
unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008
unsigned char Parameter_NaviOperatingRadius;
unsigned char Parameter_NaviWindCorrection;
unsigned char Parameter_NaviSpeedCompensation;
168,13 → 142,6
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--)
190,10 → 157,7
void SetNeutral(void)
//############################################################################
{
// Salvo 9.12.2007
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
// Salvo End
NeutralAccX = 0;
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
AdNeutralNick = 0;
256,13 → 220,6
FromNaviCtrl_Value.Kalman_K = -1;
FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
//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
 
}
 
//############################################################################
277,7 → 234,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;
291,10 → 248,6
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
485,12 → 438,12
CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
 
CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
522,17 → 475,9
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
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***************
 
Mittelwert();
 
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
606,7 → 551,6
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
SetNeutral();
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
635,13 → 579,6
}
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);
}
}
}
}
680,10 → 617,6
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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;
696,8 → 629,6
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
 
MikroKopterFlags |= FLAG_START;
}
}
710,10 → 641,6
{
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;
970,15 → 897,10
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;
1093,118 → 1015,8
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 4
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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1228,27 → 1040,53
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v,r,fehler,korrektur;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
korrektur = w / 8 + 1;
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
if(NeueKompassRichtungMerken)
{
fehler = 0;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
}
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
if(NeueKompassRichtungMerken)
{
beeptime = 200;
// KompassStartwert = KompassValue;
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
}
// Salvo 13.9.2007
w=0;
// Salvo End
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
ErsatzKompass += (fehler * 8) / korrektur;
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w >= 0)
{
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
}
if(!KompassSignalSchlecht)
{
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
// r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if(v > w) v = w; // Begrenzen
else
if(v < -w) v = -w;
Mess_Integral_Gier += v;
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1255,11 → 1093,6
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;
1267,34 → 1100,25
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[10] = Mess_Integral_Gier / 128;
// DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR;
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
DebugOut.Analog[10] = SenderOkay;
//DebugOut.Analog[16] = Mittelwert_AccHoch;
// DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
// DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
// DebugOut.Analog[19] = WinkelOut.CalcState;
// DebugOut.Analog[20] = ServoValue;
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
DebugOut.Analog[20] = ServoValue;
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
 
 
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
DebugOut.Analog[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[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
/branches/salvo_gps/Basis_V0071h/trunk/fc.h
4,22 → 4,11
 
#ifndef _FC_H
#define _FC_H
#define GIER_GRAD_FAKTOR 1450L // Abhängigkeit zwischen GyroIntegral und Winkel
//#define GIER_GRAD_FAKTOR 1450L // Abhängigkeit zwischen GyroIntegral und Winkel
//#define GIER_GRAD_FAKTOR 1550L // Abhängigkeit zwischen GyroIntegral und Winkel
//#define GIER_GRAD_FAKTOR 1291L // Abhängigkeit zwischen GyroIntegral und Winkel
#define GIER_GRAD_FAKTOR 1291L // 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 die Werte ueber Zeit und Temperatur sehr stabil.
#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
73,17 → 62,6
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 86
struct mk_param_struct
{
177,7 → 155,7
extern unsigned char Parameter_J16Timing; // for the J16 Output
extern unsigned char Parameter_J17Bitmask; // for the J17 Output
extern unsigned char Parameter_J17Timing; // for the J17 Output
 
/*
extern unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
extern unsigned char Parameter_NaviGpsGain;
extern unsigned char Parameter_NaviGpsP;
187,6 → 165,6
extern unsigned char Parameter_NaviOperatingRadius;
extern unsigned char Parameter_NaviWindCorrection;
extern unsigned char Parameter_NaviSpeedCompensation;
*/
#endif //_FC_H
 
/branches/salvo_gps/Basis_V0071h/trunk/gps.h
1,132 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock alias Salvo
// Definitionen fuer Modul GPS
// Stand 7.12.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);
extern signed int GPS_Nick;
extern signed int GPS_Roll;
extern signed int GPS_Nick2;
extern signed int GPS_Roll2;
 
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 80 // 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 15 // 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.
 
void GPS_Neutral(void);
void GPS_BerechneZielrichtung(void);
/branches/salvo_gps/Basis_V0071h/trunk/main.c
133,16 → 133,10
else
{
if(PINB & 0x02) PlatinenVersion = 20;
else
{
PlatinenVersion = 10;
// Salvo 9.12.2007 PC6 als Ausgang
DDRC = 0xCD; // SCL und PC2,PC3, PC6 als Ausgang
// Salvo End
}
else PlatinenVersion = 10;
}
 
// DDRC = 0x81; // SCL
DDRC = 0x81; // SCL
DDRC |=0x40; // HEF4017 Reset
PORTC = 0xff; // Pullup SDA
DDRB = 0x1B; // LEDs und Druckoffset
/branches/salvo_gps/Basis_V0071h/trunk/main.h
19,15 → 19,6
#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_V0071h/trunk/makefile
85,7 → 85,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 math.c
SRC = main.c uart.c printf_P.c timer0.c analog.c menu.c
SRC += twimaster.c rc.c fc.c GPS.c spi.c led.c
 
##########################################################################################################
/branches/salvo_gps/Basis_V0071h/trunk/spi.c
280,11 → 280,8
// KompassValue = 0;
// KompassRichtung = 0;
 
// Salvo 28.12.2008
// GPS_Nick = 0;
// GPS_Roll = 0;
//Salvo End
 
GPS_Nick = 0;
GPS_Roll = 0;
}
}
 
/branches/salvo_gps/Basis_V0071h/trunk/spi.h
4,7 → 4,7
 
#include <util/delay.h>
 
#define USE_SPI_COMMUNICATION //Salvo 17.10.2008 SPI deaktiviert
#define USE_SPI_COMMUNICATION
 
#define SPI_PROTOCOL_COMP 1
 
/branches/salvo_gps/Basis_V0071h/trunk/timer0.c
10,13 → 10,7
 
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,
28,10 → 22,7
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;
40,7 → 31,6
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;
75,7 → 65,7
else PORTC &= ~(1<<7);
}
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) //Salvo 25.10.2008
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
if(PINC & 0x10)
{
87,29 → 77,11
{
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;
cntKompass = 0;
}
}
}
/branches/salvo_gps/Basis_V0071h/trunk/timer0.h
4,13 → 4,6
#define HEF4017R_ON PORTC ^= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
 
 
// 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);
24,8 → 17,3
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_V0071h/trunk/uart.c
62,19 → 62,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 ",
"Kalman_MaxDrift ",
" ",
"Kalman K ",
"GPS_Nick ", //30
"GPS_Roll "
};
113,10 → 113,6
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)
{