Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 223 → Rev 224

/branches/salvo_gps/GPS.c
13,7 → 13,8
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Hold Modus mit PID Regler
Stand 2.10.2007
Komm heim zu Papi Funktion
Stand 5.10.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
44,7 → 45,7
short int ublox_msg_state = UBLOX_IDLE;
static uint8_t chk_a =0; //Checksum
static uint8_t chk_b =0;
short int gps_state;
short int gps_state,gps_sub_state;
short int gps_updte_flag;
signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
53,10 → 54,14
static unsigned int rx_len;
static unsigned int ptr_payload_data_end;
unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
 
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
 
long int dist_flown,dist_frm_start_east,dist_frm_start_north;
 
short int Get_GPS_data(void);
 
NAV_POSUTM_t actual_pos; // Aktuelle Nav Daten werden hier im ublox Format abgelegt
67,6 → 72,7
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)
109,7 → 115,7
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 < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert
if (gps_alive_cnt < 400) gps_alive_cnt += 300; // 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);
288,12 → 294,53
static signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert
signed int n,diff_v;
long signed int dev,n_l;
 
switch (cmd)
{
 
case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
// Noch einiges zu ueberlegen und zu tun
return(GPS_STST_PEND); // noch warten
if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
{
cnt++;
if (cnt > 500) // erst nach Verzoegerung
{
// Erst mal initialisieren
cnt = 0;
gps_tick = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
diff_east_f = 0, diff_north_f= 0;
diff_east = 0, diff_north = 0;
dist_flown = 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 Positionbezogen 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;
// ******************************
 
304,19 → 351,14
if (cnt > 500) // erst nach Verzoegerung
{
cnt = 0;
// aktuelle positionsdaten abespeichern
// aktuelle positionsdaten abspeichern
if (gps_rel_act_position.status > 0)
{
int_east = 0;
int_north = 0;
gps_reg_x = 0;
gps_reg_y = 0;
dist_east = 0;
dist_north = 0;
diff_east_f = 0;
diff_north_f = 0;
diff_east = 0;
diff_north = 0;
int_east = 0, int_north = 0;
gps_reg_x = 0, gps_reg_y = 0;
dist_east = 0, dist_north = 0;
diff_east_f = 0, diff_north_f= 0;
diff_east = 0, diff_north = 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
356,8 → 398,65
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;
d3 = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
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 );
if (d3 > GPS_G2T_DIST_MAX_STOP)
{
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
dist_flown += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
}
}
else //Schon ziemlich nahe am Ziel, deswegen abbremsen
{
if ((d1 < GPS_G2T_TOL) && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
{
if (d3 > 0)
{
dist_flown += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
dist_frm_start_east = (dist_flown * (long)sin_i(hdng_2home))/1000;
dist_frm_start_north = (dist_flown * (long)cos_i(hdng_2home))/1000;
gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
}
else //Ziel erreicht, Regelung beenden
{
gps_rel_hold_position.utm_east = 0;
gps_rel_hold_position.utm_north = 0;
gps_sub_state = GPS_HOME_FINISHED;
}
}
}
gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
return (GPS_STST_OK);
}
else return (GPS_STST_OK);
}
else
{
gps_state = GPS_CRTL_IDLE; //Abbruch
return (GPS_STST_ERR);
}
break;
 
 
case GPS_CRTL_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet
if (gps_updte_flag == 1) //nur wenn neue GPS Daten vorliegen
if (gps_updte_flag >0) // nur wenn neue GPS Daten vorliegen
{
gps_updte_flag = 0;
// ab hier wird geregelt
369,10 → 468,10
int_north += dist_north;
diff_east += dist_east; // Differenz zur vorhergehenden East Position
diff_north += dist_north; // Differenz zur vorhergehenden North Position
 
/*
diff_east_f = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
diff_north_f = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
 
*/
#define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung
if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
{
386,22 → 485,14
signed long dist;
int phi;
phi = arctan_i(abs(dist_north),abs(dist_east));
if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
{
dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
dist = abs((dist *1000) / (long) sin_i(phi));
}
else
{
dist = (long)dist_north;
dist = abs((dist *1000) / (long) cos_i(phi));
}
dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
 
diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
 
//PID Regler Werte aufsummieren
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north_f * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse
gps_reg_x = ((int_east * Parameter_UserParam2)/32) + ((dist_east * Parameter_UserParam1)/8)+ ((diff_east * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil X Achse
gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10); // I + P +D Anteil Y Achse
 
//Ziel-Richtung bezogen auf Nordpol bestimmen
GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
455,12 → 546,21
return (GPS_STST_ERR);
break;
}
else
{
if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
return (GPS_STST_OK);
}
}
else 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;
gps_state = GPS_CRTL_IDLE;
return (GPS_STST_ERR);
break;
}
/branches/salvo_gps/fc.c
52,7 → 52,7
// + 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 13.9.2007
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 4.109.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
81,7 → 81,10
unsigned char MAX_GAS,MIN_GAS;
unsigned char Notlandung = 0;
unsigned char HoehenReglerAktiv = 0;
//Salvo 4.10.2007
uint8_t magkompass_ok=0;
uint8_t gps_cmd = GPS_CMD_STOP;
//Salvo End
 
//Salvo 2.9.2007 Ersatzkompass
volatile long GyroKomp_Int,GyroKomp_Int2;
180,7 → 183,7
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
beeptime = 50;
//Salvo 2.9.2007 Ersatzkompass
//Salvo 4.9.2007 Ersatzkompass
GyroKomp_Int = 0;
// Salvo End
}
767,18 → 770,31
}
// Salvo End *************************
 
// Salvo 2.10.2007 GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
short int n;
n= GPS_CRTL(GPS_CMD_REQ_HOLD);
}
else
{
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
}
// Salvo 4.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((Parameter_MaxHoehe > 200) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
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 ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
&& ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
{
 
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
{
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
819,19 → 835,32
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 49;
DebugOut.Analog[0] = MesswertNick;
DebugOut.Analog[1] = MesswertRoll;
// DebugOut.Analog[2] = MesswertGier;
// DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
// DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
// DebugOut.Analog[0] = MesswertNick;
// DebugOut.Analog[1] = MesswertRoll;
DebugOut.Analog[0] = gps_state;
 
DebugOut.Analog[1] = dist_2home;
DebugOut.Analog[2] = hdng_2home;
 
 
DebugOut.Analog[3] = gps_rel_hold_position.utm_east;
DebugOut.Analog[4] = gps_rel_hold_position.utm_north;
 
/* DebugOut.Analog[2] = MesswertGier;
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[2] = Mittelwert_AccNick;
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 
*/
/*
DebugOut.Analog[7] = GasMischanteil;
DebugOut.Analog[8] = KompassValue;
*/
DebugOut.Analog[7] = dist_flown;
DebugOut.Analog[8] = dist_frm_start_north;
 
/* DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
DebugOut.Analog[1] = GPS_dist_2trgt;
/branches/salvo_gps/flight_ctrl.aps
1,0 → 0,0
<AVRStudio><MANAGEMENT><ProjectName>flight_ctrl</ProjectName><Created>28-Aug-2007 19:41:41</Created><LastEdit>28-Sep-2007 21:51:22</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>28-Aug-2007 19:41:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>flight_ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>C:\Mikrokopter\Flight_Crtl\svn\work\</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><modules><module></module></modules><Triggers><trigger clsid="{11A8571C-BF39-4FA7-8642-286DD19644B8}" enabled="1" variable="{&quot;GPS.c&quot;, 44} ptr_position" condition="0" access="0" value1="0" value2="0" elements="1" hitcount="1" continue="0" customType="0" customScope="0"/></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><SOURCEFILE>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>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.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>0</LIST><MAP>0</MAP><OUTPUTFILENAME>flight_ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</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>math.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/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -O0 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Programme\WinAVR-20070525\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Programme\WinAVR-20070525\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>C:\Mikrokopter\Flight_Crtl\svn\work\uart.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\_Settings.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\analog.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\fc.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\gps.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\main.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\menu.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\old_macros.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\printf_P.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\rc.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\Settings.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\timer0.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\twimaster.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\math.h</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\main.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\uart.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\analog.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\eeprom.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\fc.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\GPS.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\menu.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\printf_P.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\rc.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\timer0.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\twimaster.c</Name><Name>C:\Mikrokopter\Flight_Crtl\svn\work\math.c</Name></Files></ProjectFiles><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>GPS.c</FileName><Status>1</Status></File00000></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
<AVRStudio><MANAGEMENT><ProjectName>flight_ctrl</ProjectName><Created>28-Aug-2007 19:41:41</Created><LastEdit>04-Oct-2007 17:09:00</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>28-Aug-2007 19:41:41</Created><Version>4</Version><Build>4, 13, 0, 528</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>flight_ctrl.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>G:\Mikrokopter\Flight_Crtl\svn\work\</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><modules><module></module></modules><Triggers><trigger clsid="{11A8571C-BF39-4FA7-8642-286DD19644B8}" enabled="1" variable="{&quot;GPS.c&quot;, 44} ptr_position" condition="0" access="0" value1="0" value2="0" elements="1" hitcount="1" continue="0" customType="0" customScope="0"/></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>uart.c</SOURCEFILE><SOURCEFILE>analog.c</SOURCEFILE><SOURCEFILE>eeprom.c</SOURCEFILE><SOURCEFILE>fc.c</SOURCEFILE><SOURCEFILE>GPS.c</SOURCEFILE><SOURCEFILE>menu.c</SOURCEFILE><SOURCEFILE>printf_P.c</SOURCEFILE><SOURCEFILE>rc.c</SOURCEFILE><SOURCEFILE>timer0.c</SOURCEFILE><SOURCEFILE>twimaster.c</SOURCEFILE><SOURCEFILE>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>Settings.h</HEADERFILE><HEADERFILE>timer0.h</HEADERFILE><HEADERFILE>twimaster.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>0</LIST><MAP>0</MAP><OUTPUTFILENAME>flight_ctrl.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</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>math.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/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -O0 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\Programme\WinAVR-20070525\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\Programme\WinAVR-20070525\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>G:\Mikrokopter\Flight_Crtl\svn\work\uart.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\_Settings.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\analog.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\fc.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\gps.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\main.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\menu.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\old_macros.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\printf_P.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\rc.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\Settings.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\timer0.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\twimaster.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\math.h</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\main.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\uart.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\analog.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\eeprom.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\fc.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\GPS.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\menu.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\printf_P.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\rc.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\timer0.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\twimaster.c</Name><Name>G:\Mikrokopter\Flight_Crtl\svn\work\math.c</Name></Files></ProjectFiles><IOView><usergroups/></IOView><Files><File00000><FileId>00000</FileId><FileName>gps.h</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>fc.c</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>GPS.c</FileName><Status>1</Status></File00002></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
/branches/salvo_gps/gps.h
1,7 → 1,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Peter Muehlenbrock ("Salvo")
// Definitionen fuer Modul GPS
// Stand 2.10.007
// Stand 4.10.007
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
extern signed int GPS_Nick;
extern signed int GPS_Roll;
79,12 → 79,17
extern signed int GPS_dist_2trgt;
extern unsigned int gps_alive_cnt;
 
//nur provisorisch fuer Debugausgaben
extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
extern long int dist_flown,dist_frm_start_east,dist_frm_start_north;
 
// Zustaende der zentralen GPS statemachine
#define GPS_CRTL_IDLE 0 //
#define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckglug zur Basis Aktiv
#define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv
#define GPS_HOME_FINISHED 3 // Rueckflug zur Basis abgeschlossen
 
// Kommandokonstanten fuer die zentrale GPS statemachine
// Kommandokonstanten fuer die zentrale GPS Statemachine
#define GPS_CMD_REQ_INIT 0 // Initialisierung
#define GPS_CMD_STOP 1 // Lageregelung soll deaktiviert werden
#define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden
99,3 → 104,13
#define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll
#define GPS_DIST_MAX 300 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm)
#define GPS_STICK_HOLDOFF 25 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert
 
// GPS G2T /Go to Target Regler
//#define GPS_G2T_DIST_MAX_START 50 // Bei dieser Entfernung vom Startpunkt soll die Geschwindigkeit den maximal Wert erreichen (in 10 cm)
#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 30 // Bei dieser Entfernung vom Zielpunkt soll auf Normale Lageregelung umgeschaltet werden( in 10 cm)
#define GPS_G2T_V_MAX 4 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.
#define GPS_G2T_V_RAMP_DWN 1 // Gschwindigkeit (in 10cm/0.25ekunden) in der Nahe der Home Position um abzubremsen
#define GPS_G2T_TOL 30 // Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht
 
 
/branches/salvo_gps/math.c
105,3 → 105,20
return (winkel*m*n);
}
 
// Aus x,y und Winkel Distanz ermitteln
long get_dist(signed int x, signed int y, signed int phi)
{
long dist;
if (abs(x) > abs(y) )
{
dist = (long) x; //Groesseren Wert wegen besserer Genauigkeit nehmen
dist = abs((dist *1000) / (long) sin_i(phi));
}
else
{
dist = (long) y;
dist = abs((dist *1000) / (long) cos_i(phi));
}
return dist;
}
 
/branches/salvo_gps/math.h
8,3 → 8,4
extern signed int sin_i(signed int winkel);
extern signed int cos_i(signed int winkel);
extern signed int arctan_i(signed int x, signed int y);
extern long get_dist(signed int x, signed int y, signed int phi);