Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1002 → Rev 1003

/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"
 
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;
// 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 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)
{
GPS_Nick = 0;
GPS_Roll = 0;
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;
130,7 → 156,8
unsigned char Parameter_NaviGpsGain;
unsigned char Parameter_NaviGpsP;
unsigned char Parameter_NaviGpsI;
unsigned char Parameter_NaviGpsD;
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;
198,7 → 235,7
MesswertNick = 0;
MesswertRoll = 0;
MesswertGier = 0;
Delay_ms_Mess(100);
Delay_ms_Mess(100);
StartLuftdruck = Luftdruck;
HoeheD = 0;
Mess_Integral_Hoch = 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,8 → 484,10
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;
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,6 → 628,13
}
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);
}
}
}
}
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,12 → 726,15
ParameterZuordnung();
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
// 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 + GPS_Roll2);
// 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;
 
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;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
 
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,10 → 1065,15
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
 
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
}
else
{
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) && (Kompass_present > 0))
{
if(v > w) w = v; // grösste Neigung ermitteln
 
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;
//DebugOut.Analog[25] = KompassSignalSchlecht;
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
if(NeueKompassRichtungMerken)
{
beeptime = 200;
// KompassStartwert = KompassValue;
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
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;
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
}
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
extern signed int GPS_Nick;
extern signed int GPS_Roll;
extern signed int GPS_Nick2;
extern signed int GPS_Roll2;
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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,11 → 87,29
{
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;
cntKompass = 0;
//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 "
};
104,7 → 104,11
static unsigned char UartState = 0;
unsigned char CrcOkay = 0;
 
SioTmp = UDR;
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)
{