/branches/salvo_gps/GPS.c |
---|
13,7 → 13,7 |
Peter Muehlenbrock |
Auswertung der Daten vom GPS im ublox Format |
Regelung fuer GPS noch nicht implementiert |
Stand 15.9.2007 |
Stand 16.9.2007 |
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
*/ |
#include "main.h" |
41,9 → 41,14 |
signed int GPS_Nick = 0; |
signed int GPS_Roll = 0; |
static uint8_t ublox_msg_state = UBLOX_IDLE; |
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_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 |
signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
static unsigned int rx_len; |
unsigned int cnt0,cnt1,cnt2; //******Provisorisch |
61,18 → 66,23 |
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 |
// Initialisierung |
void GPS_Neutral(void) |
{ |
ublox_msg_state = UBLOX_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; |
short int n; |
ublox_msg_state = UBLOX_IDLE; |
gps_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; |
} |
// Home Position sichern falls Daten verfuegbar sind. |
103,6 → 113,7 |
gps_rel_act_position.utm_north = (int) (gps_act_position.utm_north - gps_home_position.utm_north); |
gps_rel_act_position.status = 1; // gueltige Positionsdaten |
n = 0; |
gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen. |
} |
else |
{ |
263,6 → 274,90 |
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 |
switch (cmd) |
{ |
case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden. |
if (gps_state != GPS_CRTL_HOLD_ACTIVE) |
{ |
cnt++; |
if (cnt > 300) // erst nach Verzoegerung |
{ |
cnt = 0; |
// aktuelle positionsdaten abespeichern |
if (gps_rel_act_position.status > 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_HOLD: // Lageregelung beenden |
cnt = 0; |
GPS_Nick = 0; |
GPS_Roll = 0; |
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_HOLD_ACTIVE: // Hier werden die Daten fuer Nick und Roll errechnet |
if (gps_updte_flag = 1) //nur wenn neue GPS Daten vorliegen |
{ |
gps_updte_flag = 0; |
// ab hier wird geregelt |
// Richtung zum Ziel ermitteln |
signed int dist_north,dist_east; |
dist_east = gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east; |
dist_north = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north; |
GPS_hdng_abs_2trgt = arctan_i((long)dist_east,(long)dist_north); |
//in Winkel 0..360 grad umrechnen |
if ((dist_east >= 0) && (dist_north < 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt); |
else if ((dist_east < 0) ) GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt); |
// Distanz zum Ziel ermitteln |
GPS_dist_2trgt = abs(dist_north) + abs(dist_east);//ACHTUNG: Noch Nicht korrekt |
return (GPS_STST_OK); |
} |
else return (GPS_STST_OK); |
break; |
default: |
gps_state = GPS_CRTL_IDLE; |
return (GPS_STST_ERR); |
break; |
} |
return (GPS_STST_ERR); |
} |
/branches/salvo_gps/fc.c |
---|
441,13 → 441,12 |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
Mittelwert(); |
//******PROVISORISCH*************** |
//****** GPS Daten holen *************** |
short int n; |
n = Get_Rel_Position(); |
if (n == 0) |
{ |
ROT_ON; |
// gps_act_position.status = 0; |
ROT_ON; //led blitzen lassen |
} |
//******PROVISORISCH*************** |
537,8 → 536,8 |
GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
if (gps_home_position.status > 0 ) |
{ |
Delay_ms(500); //akustisch verkuenden dass GPS Home Daten da sind |
beeptime = 1000; |
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
beeptime = 2000; |
Delay_ms(500); |
} |
} |
764,6 → 763,16 |
} |
// Salvo End ************************* |
// Salvo 15.9.2007 GPS Hold Aktiveren mit dem Hoehenschalter aber nur wenn Knueppel in Ruhelage sind |
if ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF)) |
{ |
short int n; |
n= GPS_CRTL(GPS_CMD_REQ_HOLD); |
} |
else |
{ |
n= GPS_CRTL(GPS_CMD_STOP_HOLD); |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
815,13 → 824,19 |
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[9] = gps_rel_act_position.utm_east; |
DebugOut.Analog[10] = gps_rel_act_position.utm_north; |
DebugOut.Analog[11] = gps_rel_act_position.status; |
DebugOut.Analog[7] = gps_rel_act_position.utm_east; |
DebugOut.Analog[8] = gps_rel_act_position.utm_north; |
DebugOut.Analog[9] = gps_rel_hold_position.utm_east; |
DebugOut.Analog[10] = gps_rel_hold_position.utm_north; |
DebugOut.Analog[11] = GPS_hdng_abs_2trgt; |
// ******provisorisch |
// DebugOut.Analog[9] = cnt1; |
// DebugOut.Analog[10] = cnt1; |
878,7 → 893,11 |
int tmp_int; |
if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
{ |
// Salvo 15.9.2007 Grenzwert fuer Hoehenschalter kleiner, damit |
// mit dem Schalter die Zusatende AUS- Hoehenregler AN -- Hoehenreger und GPS an |
//durchlaufen werden koennen |
if(Parameter_MaxHoehe < 50) |
// Salvo End |
{ |
SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
HoehenReglerAktiv = 0; |
/branches/salvo_gps/fc.h |
---|
17,9 → 17,12 |
//Salvo 2.9.2007 Ersatzkompass: Gyroincrements/Grad als Defaultwert ***** |
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil. |
#define GYROKOMP_INC_GRAD_DEFAULT 1250 // Gyroincrements/Grad als Defaultwert |
// Salvo End |
//Salvo 16.9.2007 GPS_STICK_HOLDOFF *************** |
// Laut Datenblatt sind die Werte ueber Zeit und Temperatur sehr stabil. |
#define GPS_STICK_HOLDOFF 35 // Wenn der Nick oder Roll groesser ist, wird GPS_HOLD deaktiviert |
// Salvo End |
extern volatile unsigned char Timeout; |
extern unsigned char Sekunde,Minute; |
extern volatile long IntegralNick,IntegralNick2; |
/branches/salvo_gps/gps.h |
---|
11,6 → 11,7 |
//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); |
typedef struct { |
unsigned long utm_itow; // time of week |
69,4 → 70,24 |
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 unsigned int cnt0,cnt1,cnt2; |
extern short int gps_state; |
extern signed int GPS_hdng_abs_2trgt; |
extern signed int GPS_hdng_rel_2trgt; |
extern signed int GPS_dist_2trgt; |
// Zustaende der zentralen GPS statemachine |
#define GPS_CRTL_IDLE 0 // |
// Kommandokonstanten fuer die zentrale GPS statemachine |
#define GPS_CMD_REQ_INIT 0 // Initialisierung |
#define GPS_CMD_REQ_HOLD 1 // Lageregelung soll aktiviert werden |
#define GPS_CMD_STOP_HOLD 2 // Lageregelung soll deaktiviert werden |
#define GPS_CMD_WAIT 4 // nix tun, nur Status abfragen |
// 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 |
#define GPS_CRTL_HOLD_ACTIVE 3 // Lageregelung aktiv |
/branches/salvo_gps/main.h |
---|
72,8 → 72,8 |
#include "menu.h" |
#include "rc.h" |
#include "fc.h" |
#include "math.h" |
#include "gps.h" |
#include "math.h" |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |