/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="{"GPS.c", 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="{"GPS.c", 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); |