Rev 676 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 676 | Rev 698 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // Peter Muehlenbrock alias Salvo |
2 | // Peter Muehlenbrock alias Salvo |
3 | // Definitionen fuer Modul GPS |
3 | // Definitionen fuer Modul GPS |
4 | // Stand 20.1.2008 |
4 | // Stand 20.1.2008 |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | extern signed int GPS_Nick; |
6 | extern signed int GPS_Nick; |
7 | extern signed int GPS_Roll; |
7 | extern signed int GPS_Roll; |
8 | extern void GPS_Neutral(void); |
8 | extern void GPS_Neutral(void); |
9 | extern void Get_Ublox_Msg(uint8_t rx) ; |
9 | extern void Get_Ublox_Msg(uint8_t rx) ; |
10 | extern short int Get_GPS_data(void); |
10 | extern short int Get_GPS_data(void); |
11 | extern short int Get_Rel_Position(void); |
11 | extern short int Get_Rel_Position(void); |
12 | extern void GPS_Save_Home(void); |
12 | extern void GPS_Save_Home(void); |
13 | extern short int GPS_CRTL(short int cmd); |
13 | extern short int GPS_CRTL(short int cmd); |
14 | 14 | ||
15 | typedef struct { |
15 | typedef struct { |
16 | unsigned long utm_itow; // time of week |
16 | unsigned long utm_itow; // time of week |
17 | long utm_east; // UTM Ost in cm |
17 | long utm_east; // UTM Ost in cm |
18 | long utm_north; // UTM Nord in cm |
18 | long utm_north; // UTM Nord in cm |
19 | long utm_alt; // hoehe in cm |
19 | long utm_alt; // hoehe in cm |
20 | uint8_t utm_zone; // |
20 | uint8_t utm_zone; // |
21 | uint8_t utm_hem; // Hemisphere Indicator |
21 | uint8_t utm_hem; // Hemisphere Indicator |
22 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
22 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
23 | } NAV_POSUTM_t; |
23 | } NAV_POSUTM_t; |
24 | 24 | ||
25 | typedef struct { |
25 | typedef struct { |
26 | unsigned long itow; // time of week |
26 | unsigned long itow; // time of week |
27 | uint8_t gpsfix_type;// 3=3D Fix |
27 | uint8_t gpsfix_type;// 3=3D Fix |
28 | uint8_t nav_status_flag; |
28 | uint8_t nav_status_flag; |
29 | uint8_t nav_diff_status; |
29 | uint8_t nav_diff_status; |
30 | uint8_t nav_resevd; |
30 | uint8_t nav_resevd; |
31 | long nav_tff; // Time to First Fix in ms |
31 | long nav_tff; // Time to First Fix in ms |
32 | long nav_msss; // ms since startup |
32 | long nav_msss; // ms since startup |
33 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
33 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
34 | } NAV_STATUS_t; |
34 | } NAV_STATUS_t; |
35 | 35 | ||
36 | typedef struct { |
36 | typedef struct { |
37 | unsigned long itow; |
37 | unsigned long itow; |
38 | long speed_n; // in cm/s |
38 | long speed_n; // in cm/s |
39 | long speed_e; // in cm/s |
39 | long speed_e; // in cm/s |
40 | long speed_alt; // in cm/s |
40 | long speed_alt; // in cm/s |
41 | unsigned long speed_3d; // in cm/s |
41 | unsigned long speed_3d; // in cm/s |
42 | unsigned long speed_gnd; // V ueber Grund in cm/s |
42 | unsigned long speed_gnd; // V ueber Grund in cm/s |
43 | long heading; // Richtung in deg/10000 |
43 | long heading; // Richtung in deg/10000 |
44 | unsigned long sacc; // Speed Genauigkeit in cm/s |
44 | unsigned long sacc; // Speed Genauigkeit in cm/s |
45 | unsigned long cacc; // Richtungsgenauigkeit in deg |
45 | unsigned long cacc; // Richtungsgenauigkeit in deg |
46 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
46 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
47 | } NAV_VELNED_t; |
47 | } NAV_VELNED_t; |
48 | 48 | ||
49 | 49 | ||
50 | typedef struct { |
50 | typedef struct { |
51 | long utm_east; // UTM Ost in 10 cm |
51 | long utm_east; // UTM Ost in 10 cm |
52 | long utm_north; // UTM Nord in 10 cm |
52 | long utm_north; // UTM Nord in 10 cm |
53 | long utm_alt; // hoehe in 10 cm |
53 | long utm_alt; // hoehe in 10 cm |
54 | unsigned long speed_gnd; // V ueber Grund in 10cm/s |
54 | unsigned long speed_gnd; // V ueber Grund in 10cm/s |
55 | unsigned heading; // Richtung in Grad |
55 | unsigned heading; // Richtung in Grad |
56 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
56 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
57 | 57 | ||
58 | } GPS_ABS_POSITION_t; |
58 | } GPS_ABS_POSITION_t; |
59 | 59 | ||
60 | 60 | ||
61 | typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position) |
61 | typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position) |
62 | int utm_east; // UTM Ost in 10 cm |
62 | int utm_east; // UTM Ost in 10 cm |
63 | int utm_north; // UTM Nord in 10 cm |
63 | int utm_north; // UTM Nord in 10 cm |
64 | int utm_alt ; // UTM Altitude in 10 cm |
64 | int utm_alt ; // UTM Altitude in 10 cm |
65 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
65 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
66 | 66 | ||
67 | } GPS_REL_POSITION_t; |
67 | } GPS_REL_POSITION_t; |
68 | 68 | ||
69 | 69 | ||
70 | extern GPS_ABS_POSITION_t gps_act_position; |
70 | extern GPS_ABS_POSITION_t gps_act_position; |
71 | extern GPS_ABS_POSITION_t gps_home_position; |
71 | extern GPS_ABS_POSITION_t gps_home_position; |
72 | extern GPS_REL_POSITION_t gps_rel_act_position; |
72 | extern GPS_REL_POSITION_t gps_rel_act_position; |
73 | extern GPS_REL_POSITION_t gps_rel_hold_position; |
73 | extern GPS_REL_POSITION_t gps_rel_hold_position; |
74 | extern short int gps_state,gps_sub_state; |
74 | extern short int gps_state,gps_sub_state; |
75 | extern unsigned int gps_alive_cnt; |
75 | extern unsigned int gps_alive_cnt; |
76 | 76 | ||
77 | 77 | ||
78 | 78 | ||
79 | // Zustaende der zentralen GPS statemachine |
79 | // Zustaende der zentralen GPS statemachine |
80 | #define GPS_CRTL_IDLE 0 // Inaktiv |
80 | #define GPS_CRTL_IDLE 0 // Inaktiv |
81 | #define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
81 | #define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
82 | #define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
82 | #define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
83 | #define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
83 | #define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
84 | #define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
84 | #define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
85 | #define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
85 | #define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
86 | #define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz |
86 | #define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz |
87 | #define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz |
87 | #define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz |
88 | #define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz |
88 | #define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz |
89 | #define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen |
89 | #define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen |
90 | 90 | ||
91 | // Kommandokonstanten fuer die zentrale GPS Statemachine |
91 | // Kommandokonstanten fuer die zentrale GPS Statemachine |
92 | #define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden |
92 | #define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden |
93 | #define GPS_CMD_REQ_INIT 1 // Initialisierung |
93 | #define GPS_CMD_REQ_INIT 1 // Initialisierung |
94 | #define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
94 | #define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
95 | #define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
95 | #define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
96 | 96 | ||
97 | // Statusmeldungen der zentralen GPS statemachine |
97 | // Statusmeldungen der zentralen GPS statemachine |
98 | #define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
98 | #define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
99 | #define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt |
99 | #define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt |
100 | #define GPS_STST_ERR 2 // Fehler |
100 | #define GPS_STST_ERR 2 // Fehler |
101 | 101 | ||
102 | // GPS Lageregler |
102 | // GPS Lageregler |
103 | #define GPS_USR_PAR_FKT 4 //Faktor durch den die Userparameter geteilt werden |
103 | #define GPS_USR_PAR_FKT 4 //Faktor durch den die Userparameter geteilt werden |
104 | 104 | ||
105 | #define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
105 | #define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
106 | #define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
106 | #define GPS_DIST_MAX 400 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
107 | #define GPS_STICK_HOLDOFF 16 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert |
107 | #define GPS_STICK_HOLDOFF 16 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert |
108 | #define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
108 | #define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
109 | 109 | ||
110 | 110 | ||
111 | // Konstanten fuer Verstaerkung fuer Speed Werte in Abhaengigkeit vom SpeedWert (cm/sek) |
111 | // Konstanten fuer Verstaerkung fuer Speed Werte in Abhaengigkeit vom SpeedWert (cm/sek) |
112 | // um eine exponentielle Verstaerkung zu erreichen |
112 | // um eine exponentielle Verstaerkung zu erreichen |
113 | #define DIFF_Y_N_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_N_MAX im Normal (Hold) Mode |
113 | #define DIFF_Y_N_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_N_MAX im Normal (Hold) Mode |
114 | //#define DIFF_X_N_MAX 200 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_N_MAX |
114 | //#define DIFF_X_N_MAX 200 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_N_MAX |
115 | 115 | ||
116 | #define DIFF_Y_F_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_F_MAX im Fast (Coming Home) Mode |
116 | #define DIFF_Y_F_MAX 1 // Verstaerkung bei Eingangswert = DIFF_X_F_MAX im Fast (Coming Home) Mode |
117 | //#define DIFF_X_F_MAX 500 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_F_MAX |
117 | //#define DIFF_X_F_MAX 500 // bei diesem Eingangswert ist die Verstaerkung = DIFF_Y_F_MAX |
118 | 118 | ||
119 | // P-Regler Verstaerkung |
119 | // P-Regler Verstaerkung |
120 | #define GPS_PROP_NRML_V 2 //maximale Verstaerkung im Normalen Holdmode |
120 | #define GPS_PROP_NRML_V 2 //maximale Verstaerkung im Normalen Holdmode |
121 | #define GPS_PROP_FAST_V 4 //maximale Verstaerkung im Fast mode |
121 | #define GPS_PROP_FAST_V 6 //maximale Verstaerkung im Fast mode |
122 | 122 | ||
123 | // GPS G2T /Go to Target Regler |
123 | // GPS G2T /Go to Target Regler |
124 | #define GPS_G2T_DIST_MAX_STOP 100 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
124 | #define GPS_G2T_DIST_MAX_STOP 100 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
125 | #define GPS_G2T_DIST_HOLD 50 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
125 | #define GPS_G2T_DIST_HOLD 50 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
126 | #define GPS_G2T_FAST_TOL 200 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
126 | #define GPS_G2T_FAST_TOL 200 // Bei grosser Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
127 | #define GPS_G2T_NRML_TOL 100 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
127 | #define GPS_G2T_NRML_TOL 100 // Bei kleiner Entfernung vom Ziel: Der Sollwert wird nur geaendert wenn die aktuelle Position nicht mehr als diesem Wert vom Sollwert abweicht |
128 | #define GPS_G2T_V_MAX 16 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
128 | #define GPS_G2T_V_MAX 16 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
129 | #define GPS_G2T_V_RAMP_DWN 10 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
129 | #define GPS_G2T_V_RAMP_DWN 10 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
130 | #define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
130 | #define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
131 | 131 | ||
132 | 132 |