Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// Peter Muehlenbrock ("Salvo")

// Definitionen fuer Modul GPS

// Stand 12.10.007

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

extern void Get_Ublox_Msg(uint8_t  rx) ;

//extern short int Get_GPS_data(void);

extern short int Get_Rel_Position(void);

extern short int GPS_CRTL(short int cmd);

typedef struct {

        unsigned long   utm_itow;       // time of week

        long                    utm_east;       // UTM Ost in cm  

        long                    utm_north;  // UTM Nord in cm    

        long                    utm_alt;        // hoehe in cm

        uint8_t                 utm_zone;       // 

        uint8_t                 utm_hem;        // Hemisphere Indicator 

        uint8_t                 status;         // 0: kein gueltiges Paket 1: alles ok

} NAV_POSUTM_t;

typedef struct {

        unsigned long   itow;   // time of week

        uint8_t                 gpsfix_type;// 3=3D Fix  

        uint8_t                 nav_status_flag;

        uint8_t                 nav_diff_status;

        uint8_t                 nav_resevd;

        long                    nav_tff;  // Time to First Fix in ms    

        long                    nav_msss;       // ms since startup

        uint8_t                 status;         // 0: kein gueltiges Paket 1: alles ok

} NAV_STATUS_t;

typedef struct {

        unsigned long   itow;            

        long                    speed_n;                // in cm/s  

        long                    speed_e;                // in cm/s  

        long                    speed_alt;              // in cm/s   

        unsigned long   speed_3d;       // in cm/s   

        unsigned long   speed_gnd;      // V ueber Grund in cm/s   

        long                    heading;        // Richtung in deg/10000 

        unsigned long   sacc;           // Speed Genauigkeit in cm/s  

        unsigned long   cacc;           // Richtungsgenauigkeit in deg 

        uint8_t                 status;         // 0: kein gueltiges Paket 1: alles ok  

} NAV_VELNED_t;

typedef struct {

        long                    utm_east;       // UTM Ost in 10 cm  

        long                    utm_north;  // UTM Nord in 10 cm    

        long                    utm_alt;        // hoehe in 10 cm

        unsigned long   speed_gnd;      // V ueber Grund in 10cm/s  

        unsigned                heading;        // Richtung in Grad   

        uint8_t                 status;         // 0: keine gueltigen Daten 1: alles ok

} 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

} 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                          GPS_REL_POSITION_t gps_rel_hold_position;

extern short int        gps_state,gps_sub_state;

extern signed int       GPS_hdng_abs_2trgt;

extern signed int       GPS_hdng_rel_2trgt;

extern signed int       GPS_dist_2trgt;

extern signed int       gps_reg_x,gps_reg_y;

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;

// Zustaende der zentralen GPS statemachine

#define GPS_CRTL_IDLE                           0   // 

#define GPS_CRTL_HOLD_ACTIVE            1       //  Lageregelung aktiv

#define GPS_CRTL_HOME_ACTIVE            2       //  Rueckflug zur Basis Aktiv

#define GPS_HOME_FAST_IN_TOL            3       //  Rueckflug: Aktuelle Position innerhalb der Toleranz

#define GPS_HOME_FAST_OUTOF_TOL         4       //  Rueckflug: Aktuelle Position ausserhalb der Toleranz

#define GPS_HOME_RMPDWN_IN_TOL          5       //  Rueckflug: beim Abbremsen Position  innerhalb der Toleranz

#define GPS_HOME_RMPDWN_OUTOF_TOL       6       //  Rueckflug: beim Abbremsen Position  ausserhalb der Toleranz

#define GPS_HOME_IN_TOL                         7       //  Rueckflug: Nahe am Ziel innerhalb der Toleranz

#define GPS_HOME_OUTOF_TOL                      8       //  Rueckflug: Nahe am Ziel ausserhalb der Toleranz

#define GPS_HOME_FINISHED                       9       //  Rueckflug zur Basis abgeschlossen

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

#define GPS_CMD_REQ_HOME                4    // Das Heimfliegen soll aktiviert werden

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

// GPS Lageregler

#define GPS_NICKROLL_MAX                40  // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll

#define GPS_DIST_MAX                    400 // 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

#define GPS_V                                   8       // Teilerfaktor Regelabweichung zu  Ausgabewert

// Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung, abhaengig vom Modus. 

#define GPS_DIFF_NRML_MAX_V     20      //maximale Verstaerkung * 10

#define GPS_DIFF_NRML_MAX_D     30      //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm

#define GPS_DIFF_FAST_MAX_V     15      //maximale Verstaerkung * 10 im Fast mode

#define GPS_DIFF_FAST_MAX_D     50      //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode

// P-Regler Verstaerkung 

#define GPS_PROP_NRML_V                 2       //maximale Verstaerkung * 2

#define GPS_PROP_FAST_V                 2       //maximale Verstaerkung * 2 im Fast mode

// GPS G2T /Go to Target Regler

#define GPS_G2T_DIST_MAX_STOP   50      // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm)

#define GPS_G2T_DIST_HOLD               30      // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt

#define GPS_G2T_V_MAX                   12      // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird.

#define GPS_G2T_V_RAMP_DWN              6       // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen

#define GPS_G2T_V_MIN                   3       // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition.

#define GPS_G2T_FAST_TOL                60      // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht

#define GPS_G2T_NRML_TOL                40      // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht