Rev 602 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 602 | Rev 613 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // Peter Muehlenbrock ("Salvo") |
2 | // Peter Muehlenbrock ("Salvo") |
3 | // Definitionen fuer Modul GPS |
3 | // Definitionen fuer Modul GPS |
4 | // Stand 12.10.007 |
4 | // Stand 12.10.007 |
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 signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
8 | extern signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel |
9 | extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
9 | extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
10 | extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
10 | extern signed int GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters |
11 | extern void GPS_Neutral(void); |
11 | extern void GPS_Neutral(void); |
12 | extern void Get_Ublox_Msg(uint8_t rx) ; |
12 | extern void Get_Ublox_Msg(uint8_t rx) ; |
13 | //extern short int Get_GPS_data(void); |
13 | //extern short int Get_GPS_data(void); |
14 | extern short int Get_Rel_Position(void); |
14 | extern short int Get_Rel_Position(void); |
15 | extern void GPS_Save_Home(void); |
15 | extern void GPS_Save_Home(void); |
16 | extern short int GPS_CRTL(short int cmd); |
16 | extern short int GPS_CRTL(short int cmd); |
17 | 17 | ||
18 | typedef struct { |
18 | typedef struct { |
19 | unsigned long utm_itow; // time of week |
19 | unsigned long utm_itow; // time of week |
20 | long utm_east; // UTM Ost in cm |
20 | long utm_east; // UTM Ost in cm |
21 | long utm_north; // UTM Nord in cm |
21 | long utm_north; // UTM Nord in cm |
22 | long utm_alt; // hoehe in cm |
22 | long utm_alt; // hoehe in cm |
23 | uint8_t utm_zone; // |
23 | uint8_t utm_zone; // |
24 | uint8_t utm_hem; // Hemisphere Indicator |
24 | uint8_t utm_hem; // Hemisphere Indicator |
25 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
25 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
26 | } NAV_POSUTM_t; |
26 | } NAV_POSUTM_t; |
27 | 27 | ||
28 | typedef struct { |
28 | typedef struct { |
29 | unsigned long itow; // time of week |
29 | unsigned long itow; // time of week |
30 | uint8_t gpsfix_type;// 3=3D Fix |
30 | uint8_t gpsfix_type;// 3=3D Fix |
31 | uint8_t nav_status_flag; |
31 | uint8_t nav_status_flag; |
32 | uint8_t nav_diff_status; |
32 | uint8_t nav_diff_status; |
33 | uint8_t nav_resevd; |
33 | uint8_t nav_resevd; |
34 | long nav_tff; // Time to First Fix in ms |
34 | long nav_tff; // Time to First Fix in ms |
35 | long nav_msss; // ms since startup |
35 | long nav_msss; // ms since startup |
36 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
36 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
37 | } NAV_STATUS_t; |
37 | } NAV_STATUS_t; |
38 | 38 | ||
39 | typedef struct { |
39 | typedef struct { |
40 | unsigned long itow; |
40 | unsigned long itow; |
41 | long speed_n; // in cm/s |
41 | long speed_n; // in cm/s |
42 | long speed_e; // in cm/s |
42 | long speed_e; // in cm/s |
43 | long speed_alt; // in cm/s |
43 | long speed_alt; // in cm/s |
44 | unsigned long speed_3d; // in cm/s |
44 | unsigned long speed_3d; // in cm/s |
45 | unsigned long speed_gnd; // V ueber Grund in cm/s |
45 | unsigned long speed_gnd; // V ueber Grund in cm/s |
46 | long heading; // Richtung in deg/10000 |
46 | long heading; // Richtung in deg/10000 |
47 | unsigned long sacc; // Speed Genauigkeit in cm/s |
47 | unsigned long sacc; // Speed Genauigkeit in cm/s |
48 | unsigned long cacc; // Richtungsgenauigkeit in deg |
48 | unsigned long cacc; // Richtungsgenauigkeit in deg |
49 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
49 | uint8_t status; // 0: kein gueltiges Paket 1: alles ok |
50 | } NAV_VELNED_t; |
50 | } NAV_VELNED_t; |
51 | 51 | ||
52 | 52 | ||
53 | typedef struct { |
53 | typedef struct { |
54 | long utm_east; // UTM Ost in 10 cm |
54 | long utm_east; // UTM Ost in 10 cm |
55 | long utm_north; // UTM Nord in 10 cm |
55 | long utm_north; // UTM Nord in 10 cm |
56 | long utm_alt; // hoehe in 10 cm |
56 | long utm_alt; // hoehe in 10 cm |
57 | unsigned long speed_gnd; // V ueber Grund in 10cm/s |
57 | unsigned long speed_gnd; // V ueber Grund in 10cm/s |
58 | unsigned heading; // Richtung in Grad |
58 | unsigned heading; // Richtung in Grad |
59 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
59 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
60 | 60 | ||
61 | } GPS_ABS_POSITION_t; |
61 | } GPS_ABS_POSITION_t; |
62 | 62 | ||
63 | 63 | ||
64 | typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position) |
64 | typedef struct { // Struktur fuer Relative GPS Daten (bezogen z.B. auf Home Position) |
65 | int utm_east; // UTM Ost in 10 cm |
65 | int utm_east; // UTM Ost in 10 cm |
66 | int utm_north; // UTM Nord in 10 cm |
66 | int utm_north; // UTM Nord in 10 cm |
67 | int utm_alt ; // UTM Altitude in 10 cm |
67 | int utm_alt ; // UTM Altitude in 10 cm |
68 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
68 | uint8_t status; // 0: keine gueltigen Daten 1: alles ok |
69 | 69 | ||
70 | } GPS_REL_POSITION_t; |
70 | } GPS_REL_POSITION_t; |
71 | 71 | ||
72 | 72 | ||
73 | extern GPS_ABS_POSITION_t gps_act_position; |
73 | extern GPS_ABS_POSITION_t gps_act_position; |
74 | extern GPS_ABS_POSITION_t gps_home_position; |
74 | extern GPS_ABS_POSITION_t gps_home_position; |
75 | extern GPS_REL_POSITION_t gps_rel_act_position; |
75 | extern GPS_REL_POSITION_t gps_rel_act_position; |
76 | extern GPS_REL_POSITION_t gps_rel_hold_position; |
76 | extern GPS_REL_POSITION_t gps_rel_hold_position; |
77 | extern short int gps_state,gps_sub_state; |
77 | extern short int gps_state,gps_sub_state; |
78 | extern signed int GPS_hdng_abs_2trgt; |
78 | extern signed int GPS_hdng_abs_2trgt; |
79 | extern signed int GPS_hdng_rel_2trgt; |
79 | extern signed int GPS_hdng_rel_2trgt; |
80 | extern signed int GPS_dist_2trgt; |
80 | extern signed int GPS_dist_2trgt; |
81 | extern signed int gps_reg_x,gps_reg_y; |
81 | extern signed int gps_reg_x,gps_reg_y; |
82 | extern signed int GPS_dist_2trgt; |
82 | extern signed int GPS_dist_2trgt; |
83 | extern unsigned int gps_alive_cnt; |
83 | extern unsigned int gps_alive_cnt; |
84 | extern unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
84 | extern unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators |
85 | extern signed int int_east,int_north; //Integrierer |
85 | extern signed int int_east,int_north; //Integrierer |
- | 86 | extern signed int diff_east_f,diff_north_f; // Differenzierer, gefiltert |
|
- | 87 | extern signed int diff_v; |
|
- | 88 | signed long dist; |
|
86 | 89 | ||
87 | 90 | ||
88 | //nur provisorisch fuer Debugausgaben |
91 | //nur provisorisch fuer Debugausgaben |
89 | extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
92 | extern signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position |
90 | extern long int dist_flown; |
93 | extern long int dist_flown; |
91 | 94 | ||
92 | 95 | ||
93 | #define GPS_USR_PAR_FKT 8 //Faktor durch den die Userparameter getielt werden |
96 | #define GPS_USR_PAR_FKT 8 //Faktor durch den die Userparameter getielt werden |
94 | 97 | ||
95 | 98 | ||
96 | // Zustaende der zentralen GPS statemachine |
99 | // Zustaende der zentralen GPS statemachine |
97 | #define GPS_CRTL_IDLE 0 // |
100 | #define GPS_CRTL_IDLE 0 // |
98 | #define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
101 | #define GPS_CRTL_HOLD_ACTIVE 1 // Lageregelung aktiv |
99 | #define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
102 | #define GPS_CRTL_HOME_ACTIVE 2 // Rueckflug zur Basis Aktiv |
100 | #define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
103 | #define GPS_HOME_FAST_IN_TOL 3 // Rueckflug: Aktuelle Position innerhalb der Toleranz |
101 | #define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
104 | #define GPS_HOME_FAST_OUTOF_TOL 4 // Rueckflug: Aktuelle Position ausserhalb der Toleranz |
102 | #define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
105 | #define GPS_HOME_RMPDWN_IN_TOL 5 // Rueckflug: beim Abbremsen Position innerhalb der Toleranz |
103 | #define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz |
106 | #define GPS_HOME_RMPDWN_OUTOF_TOL 6 // Rueckflug: beim Abbremsen Position ausserhalb der Toleranz |
104 | #define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz |
107 | #define GPS_HOME_IN_TOL 7 // Rueckflug: Nahe am Ziel innerhalb der Toleranz |
105 | #define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz |
108 | #define GPS_HOME_OUTOF_TOL 8 // Rueckflug: Nahe am Ziel ausserhalb der Toleranz |
106 | #define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen |
109 | #define GPS_HOME_FINISHED 9 // Rueckflug zur Basis abgeschlossen |
107 | 110 | ||
108 | // Kommandokonstanten fuer die zentrale GPS Statemachine |
111 | // Kommandokonstanten fuer die zentrale GPS Statemachine |
109 | #define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden |
112 | #define GPS_CMD_STOP 0 // Lageregelung soll deaktiviert werden |
110 | #define GPS_CMD_REQ_INIT 1 // Initialisierung |
113 | #define GPS_CMD_REQ_INIT 1 // Initialisierung |
111 | #define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
114 | #define GPS_CMD_REQ_HOLD 3 // Lageregelung soll aktiviert werden |
112 | #define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
115 | #define GPS_CMD_REQ_HOME 4 // Das Heimfliegen soll aktiviert werden |
113 | 116 | ||
114 | // Statusmeldungen der zentralen GPS statemachine |
117 | // Statusmeldungen der zentralen GPS statemachine |
115 | #define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
118 | #define GPS_STST_OK 0 // Kommando erfolgreich und abgeschlossen |
116 | #define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt |
119 | #define GPS_STST_PEND 1 // Kommando noch nicht komplett durchgefuehrt |
117 | #define GPS_STST_ERR 2 // Fehler |
120 | #define GPS_STST_ERR 2 // Fehler |
118 | 121 | ||
119 | // GPS Lageregler |
122 | // GPS Lageregler |
120 | #define GPS_NICKROLL_MAX 30 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
123 | #define GPS_NICKROLL_MAX 20 // Maximaler Einfluss des GPS Lagereglers auf Nick und Roll |
121 | #define GPS_DIST_MAX 200 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
124 | #define GPS_DIST_MAX 200 // Maximal zulaessige Distanz bevor Regelung gestoppt wird (in 10cm) |
122 | #define GPS_STICK_HOLDOFF 10 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert |
125 | #define GPS_STICK_HOLDOFF 10 // Wenn der Nick oder Roll Stickwerte groesser sind, wird GPS_HOLD deaktiviert |
123 | #define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
126 | #define GPS_V 8 // Teilerfaktor Regelabweichung zu Ausgabewert |
124 | 127 | ||
125 | // Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung, abhaengig vom Modus. |
128 | // Differenzierer Kennwerte fuer von Distanz abhaengige Verstaerkung, abhaengig vom Modus. |
126 | #define GPS_DIFF_NRML_MAX_V 20 //maximale Verstaerkung * 10 |
129 | #define GPS_DIFF_NRML_MAX_V 20 //maximale Verstaerkung * 10 |
127 | #define GPS_DIFF_NRML_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
130 | #define GPS_DIFF_NRML_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm |
128 | #define GPS_DIFF_FAST_MAX_V 15 //maximale Verstaerkung * 10 im Fast mode |
131 | #define GPS_DIFF_FAST_MAX_V 15 //maximale Verstaerkung * 10 im Fast mode |
129 | #define GPS_DIFF_FAST_MAX_D 50 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode |
132 | #define GPS_DIFF_FAST_MAX_D 50 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm im Fast mode |
- | 133 | ||
130 | // P-Regler Verstaerkung |
134 | // P-Regler Verstaerkung |
131 | #define GPS_PROP_NRML_V 2 //maximale Verstaerkung * 2 |
135 | #define GPS_PROP_NRML_V 2 //maximale Verstaerkung * 2 |
132 | #define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2 im Fast mode |
136 | #define GPS_PROP_FAST_V 2 //maximale Verstaerkung * 2 im Fast mode |
133 | 137 | ||
134 | // GPS G2T /Go to Target Regler |
138 | // GPS G2T /Go to Target Regler |
135 | #define GPS_G2T_DIST_MAX_STOP 50 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
139 | #define GPS_G2T_DIST_MAX_STOP 50 // Ab dieser Entfernung vom Zielpunkt soll die Geschwindigkeit runtergeregelt werden( in 10 cm) |
136 | #define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
140 | #define GPS_G2T_DIST_HOLD 30 // Ab dieser Entfernung vom Zielpunkt wird mit Minimaler Geschwindigkeit eingeregelt |
137 | #define GPS_G2T_V_MAX 12 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
141 | #define GPS_G2T_V_MAX 12 // Maximale Geschwindigkeit (in 10cm/0.25 Sekunden) mit der der Sollpunkt geaendert wird. |
138 | #define GPS_G2T_V_RAMP_DWN 6 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
142 | #define GPS_G2T_V_RAMP_DWN 6 // Geschwindigkeit (in 10cm/0.25ekunden) in der Naehe der Home Position um abzubremsen |
139 | #define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
143 | #define GPS_G2T_V_MIN 3 // Minimale (in 10cm/0.25 Sekunden) ganz nahe an Homeposition. |
140 | #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 |
144 | #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 |
141 | #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 |
145 | #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 |
142 | 146 | ||
143 | 147 | ||
144 | 148 |