Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 157 → Rev 158

/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")))