Subversion Repositories FlightCtrl

Compare Revisions

Regard whitespace Rev 155 → Rev 156

/branches/salvo_gps/GPS.c
13,10 → 13,11
Peter Muehlenbrock
Auswertung der Daten vom GPS im ublox Format
Regelung fuer GPS noch nicht implementiert
Stand 11.9.2007
Stand 14.9.2007
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#include "main.h"
//#include "gps.h"
 
// Defines fuer ublox Messageformat um Auswertung zu steuern
#define UBLOX_IDLE 0
51,24 → 52,72
static uint8_t *ptr_payload_data;
static uint8_t *ptr_pac_status;
 
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_POSITION_t gps_act_position; // Alle wichtigen Daten zusammengefasst
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
 
void GPS_Neutral(void) // Initialisierung
// 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
}
 
void Get_GPS_data(void) // Daten aus aktuellen ublox Messages extrahieren
// Home Position sichern falls Daten verfuegbar sind.
void GPS_Save_Home(void)
{
if (actual_pos.status == 0) return; //damit es schnell geht, wenn nix zu tun ist
short int n;
n = Get_GPS_data();
if (n == 0) // Gueltige und aktuelle Daten ?
{
// Neue GPS Daten liegen vor
// beeptime = 500; // Piepsen um korrekte Home Position anzuzeigen
// gps_act_position.status = 0;
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_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.status = 1; // gueltige Positionsdaten
n = 0;
}
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
if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
{
cnt1++; //**** noch Rausschmeissen
81,11 → 130,18
gps_act_position.speed_gnd = actual_speed.speed_gnd/10;
gps_act_position.heading = actual_speed.heading/100000;
gps_act_position.status = 1;
n = 0; //Daten gueltig
}
actual_pos.status = 0;
else
{
gps_act_position.status = 0; //Keine gueltigen Daten
n = 2;
}
actual_pos.status = 0; //neue ublox Messages anfordern
actual_status.status = 0;
actual_speed.status = 0;
}
return (n);
}
 
 
/branches/salvo_gps/fc.c
52,6 → 52,12
// + 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
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
*/
 
#include "main.h"
 
75,7 → 81,6
unsigned char MAX_GAS,MIN_GAS;
unsigned char Notlandung = 0;
unsigned char HoehenReglerAktiv = 0;
static int SignalSchlecht = 0;
uint8_t magkompass_ok=0;
 
//Salvo 2.9.2007 Ersatzkompass
137,6 → 142,7
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
GPS_Neutral();
CalibrierMittelwert();
timer = SetDelay(5);
while (!CheckDelay(timer));
173,7 → 179,6
HoeheD = 0;
Mess_Integral_Hoch = 0;
KompassStartwert = KompassValue;
GPS_Neutral();
beeptime = 50;
//Salvo 2.9.2007 Ersatzkompass
GyroKomp_Int = 0;
201,7 → 206,6
accumulate_AccNick = 0;messanzahl_AccNick = 0;
accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
Integral_Gier = Mess_Integral_Gier;
// Integral_Gier2 = Mess_Integral_Gier2;
IntegralNick = Mess_IntegralNick;
IntegralRoll = Mess_IntegralRoll;
IntegralNick2 = Mess_IntegralNick2;
419,7 → 423,6
MIN_GAS = EE_Parameter.Gas_Min;
}
 
 
//############################################################################
//
void MotorRegler(void)
440,11 → 443,12
static char NeueKompassRichtungMerken = 0;
Mittelwert();
//******PROVISORISCH***************
Get_GPS_data();
if (gps_act_position.status > 0)
short int n;
n = Get_Rel_Position();
if (n == 0)
{
ROT_ON;
gps_act_position.status = 0;
// gps_act_position.status = 0;
}
 
//******PROVISORISCH***************
532,6 → 536,7
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
}
GPS_Save_Home();
}
else delay_neutral = 0;
}
655,19 → 660,6
}
// Salvo End
 
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist ***********************
// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert
// Ohne Driftkompensation ist die Gierachse wesentlich stabiler
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
// Salvo 13.9.2007
// if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (magkompass_ok >0))
// {
// if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--;
// if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++;
// }
// else Mess_Integral_Gier2 = 0;
// Salvo End ***********************
ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Mess_IntegralNick2 = IntegralNick;
Mess_IntegralRoll2 = IntegralRoll;
765,7 → 757,6
}
else magkompass_ok = 0;
}
 
// Salvo End *************************
 
 
777,19 → 768,11
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
// Salvo 12.9.2007 Kompassdaempfung abschalten weil Ersatzkompass verwendet wird
// w=0, v=0;
//Salvo End
// if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht)
 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
// Salvo 12.9.2007 Ersatzkompass nehmen statt Magnetkompass
// KompassStartwert = KompassValue;
// KompassStartwert = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
// Salvo End *************************
// Salvo 13.9.2007
KompassStartwert = KompassValue;
// Salvo End
NeueKompassRichtungMerken = 0;
}
// Salvo 13.9.2007
804,22 → 787,8
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
// Salvo 30.8.2007 Winkelbegrenzung **********************
/*
if ((!SignalSchlecht) )
{
if (abs(KompassRichtung) < 135 )
{
Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
}
}
*/
// Salvo End *************************
 
ANALOG_ON; // ADC einschalten
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
843,6 → 812,11
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;
 
// ******provisorisch
// DebugOut.Analog[9] = cnt1;
// DebugOut.Analog[10] = cnt1;
851,10 → 825,11
// DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
// ******provisorisch
 
/*
DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
DebugOut.Analog[10] = magkompass_ok;
DebugOut.Analog[11] = Mess_Integral_Gier2;
*/
/*
DebugOut.Analog[11] = GyroKomp_Inc_Grad;
DebugOut.Analog[12] = GyroKomp_Value;
/branches/salvo_gps/gps.h
8,7 → 8,9
extern void GPS_Neutral(void);
 
extern void Get_Ublox_Msg(uint8_t rx) ;
extern void Get_GPS_data(void);
//extern short int Get_GPS_data(void);
extern short int Get_Rel_Position(void);
extern void GPS_Save_Home(void);
 
typedef struct {
unsigned long utm_itow; // time of week
53,14 → 55,18
unsigned heading; // Richtung in Grad
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
 
} GPS_POSITION_t;
} GPS_ABS_POSITION_t;
 
 
typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position)
int utm_east; // UTM Ost in m
int utm_north; // UTM Nord in m
uint8_t status; // 0: keine gueltigen Daten 1: alles ok
 
/*
extern NAV_VELNED_t actual_speed;
extern NAV_STATUS_t actual_status;
extern NAV_POSUTM_t actual_position;
*/
extern GPS_POSITION_t gps_act_position;
} 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 unsigned int cnt0,cnt1,cnt2;
/branches/salvo_gps/main.c
149,6 → 149,7
printf("OK\n\r");
}
SetNeutral();
 
ROT_OFF;
/branches/salvo_gps/timer0.c
21,7 → 21,12
T0_RISING_EDGE = 7
};
 
 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 13.9.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem neutralwert
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
*/
SIGNAL (SIG_OVERFLOW0) // 8kHz
{
static unsigned char cnt_1ms = 1,cnt = 0;
69,16 → 74,10
}
// Salvo End
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
//Salvo 12.9.2007 Ersatzkompass als Basis nehmen, nicht den magnetkompass
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
// int i;
// i = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
// if (i < 180) KompassRichtung = ((540 + i - KompassStartwert) % 360) - 180;
// else KompassRichtung = ((540 + (i-360) - KompassStartwert) % 360) - 180;
//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses
Kompass_Neuer_Wert = 1;
// Salvo End
Kompass_Neuer_Wert = 1;
cntKompass = 0;
}
}