Rev 966 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 966 | Rev 988 | ||
---|---|---|---|
1 | /* |
1 | /* |
2 | Copyright 2008, by Michael Walter |
2 | Copyright 2008, by Michael Walter |
3 | 3 | ||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
9 | 9 | ||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
12 | unless it is stated otherwise. |
12 | unless it is stated otherwise. |
13 | */ |
13 | */ |
14 | 14 | ||
15 | /***************************************************************************** |
15 | /***************************************************************************** |
16 | INCLUDES |
16 | INCLUDES |
17 | **************************************************************************** */ |
17 | **************************************************************************** */ |
18 | #include "main.h" |
18 | #include "main.h" |
19 | #include "kafi.h" |
19 | #include "kafi.h" |
20 | #include "mymath.h" |
20 | #include "mymath.h" |
21 | #include <math.h> |
21 | #include <math.h> |
22 | 22 | ||
23 | /***************************************************************************** |
23 | /***************************************************************************** |
24 | (SYMBOLIC) CONSTANTS |
24 | (SYMBOLIC) CONSTANTS |
25 | *****************************************************************************/ |
25 | *****************************************************************************/ |
26 | 26 | ||
27 | /***************************************************************************** |
27 | /***************************************************************************** |
28 | VARIABLES |
28 | VARIABLES |
29 | *****************************************************************************/ |
29 | *****************************************************************************/ |
30 | 30 | ||
31 | int GPSTracking = 0; |
31 | int GPSTracking = 0; |
32 | int targetPosValid = 0; |
32 | int targetPosValid = 0; |
33 | int homePosValid = 0; |
33 | int homePosValid = 0; |
34 | int holdPosValid = 0; |
34 | int holdPosValid = 0; |
35 | 35 | ||
36 | volatile gpsInfo_t actualPos;// measured position (last gps record) |
36 | volatile gpsInfo_t actualPos;// measured position (last gps record) |
37 | volatile gpsInfo_t targetPos;// measured position (last gps record) |
37 | volatile gpsInfo_t targetPos;// measured position (last gps record) |
38 | volatile gpsInfo_t homePos;// measured position (last gps record) |
38 | volatile gpsInfo_t homePos;// measured position (last gps record) |
39 | volatile gpsInfo_t holdPos; // measured position (last gps record) |
39 | volatile gpsInfo_t holdPos; // measured position (last gps record) |
40 | 40 | ||
41 | NAV_STATUS_t navStatus; |
41 | NAV_STATUS_t navStatus; |
42 | NAV_POSLLH_t navPosLlh; |
42 | NAV_POSLLH_t navPosLlh; |
43 | NAV_POSUTM_t navPosUtm; |
43 | NAV_POSUTM_t navPosUtm; |
44 | NAV_VELNED_t navVelNed; |
44 | NAV_VELNED_t navVelNed; |
45 | 45 | ||
46 | uint8_t gpsState; |
46 | uint8_t gpsState; |
47 | 47 | ||
48 | signed int GPS_Nick = 0; |
48 | signed int GPS_Nick = 0; |
49 | signed int GPS_Roll = 0; |
49 | signed int GPS_Roll = 0; |
50 | 50 | ||
51 | long distanceNS = 0; |
51 | long distanceNS = 0; |
52 | long distanceEW = 0; |
52 | long distanceEW = 0; |
53 | long velocityNS = 0; |
53 | long velocityNS = 0; |
54 | long velocityEW = 0; |
54 | long velocityEW = 0; |
55 | unsigned long maxDistance = 0; |
55 | unsigned long maxDistance = 0; |
56 | 56 | ||
57 | int roll_gain = 0; |
57 | int roll_gain = 0; |
58 | int nick_gain = 0; |
58 | int nick_gain = 0; |
59 | int nick_gain_p, nick_gain_d; |
59 | int nick_gain_p, nick_gain_d; |
60 | int roll_gain_p, roll_gain_d; |
60 | int roll_gain_p, roll_gain_d; |
61 | int Override = 0; |
61 | int Override = 0; |
62 | int TargetGier = 0; |
62 | int TargetGier = 0; |
63 | 63 | ||
64 | extern int sollGier; |
64 | extern int sollGier; |
65 | extern int RemoteLinkLost; |
65 | extern int RemoteLinkLost; |
66 | 66 | ||
67 | volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered |
67 | volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered |
68 | volatile uint8_t CK_A, CK_B;// UBX checksum bytes |
68 | volatile uint8_t CK_A, CK_B;// UBX checksum bytes |
69 | volatile uint8_t ignorePacket;// true when previous packet was not processed |
69 | volatile uint8_t ignorePacket;// true when previous packet was not processed |
70 | volatile unsigned short msgLen; |
70 | volatile unsigned short msgLen; |
71 | volatile uint8_t msgID; |
71 | volatile uint8_t msgID; |
72 | 72 | ||
73 | void GPSscanData (void); |
73 | void GPSscanData (void); |
74 | void GPSupdate(void); |
74 | void GPSupdate(void); |
75 | void SetNewHeading(unsigned long maxDistance); |
75 | void SetNewHeading(unsigned long maxDistance); |
76 | 76 | ||
77 | /* **************************************************************************** |
77 | /* **************************************************************************** |
78 | Functionname: GPSupdate */ /*! |
78 | Functionname: GPSupdate */ /*! |
79 | Description: |
79 | Description: |
80 | 80 | ||
81 | @param[in] |
81 | @param[in] |
82 | 82 | ||
83 | @return void |
83 | @return void |
84 | @pre - |
84 | @pre - |
85 | @post - |
85 | @post - |
86 | @author Michael Walter |
86 | @author Michael Walter |
87 | **************************************************************************** */ |
87 | **************************************************************************** */ |
88 | void GPSupdate(void) |
88 | void GPSupdate(void) |
89 | { |
89 | { |
90 | float SIN_H, COS_H; |
90 | float SIN_H, COS_H; |
91 | long max_p = 0; |
91 | long max_p = 0; |
92 | long max_d = 0; |
92 | long max_d = 0; |
93 | int SwitchPos = 0; |
93 | int SwitchPos = 0; |
94 | 94 | ||
95 | /* Determine Selector Switch Position */ |
95 | /* Determine Selector Switch Position */ |
96 | if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100) |
96 | if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100) |
97 | { |
97 | { |
98 | SwitchPos = 0; |
98 | SwitchPos = 0; |
99 | } |
99 | } |
100 | else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) |
100 | else if (PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) |
101 | { |
101 | { |
102 | SwitchPos = 2;/* Target Mode */ |
102 | SwitchPos = 2;/* Target Mode */ |
103 | } |
103 | } |
104 | else |
104 | else |
105 | { |
105 | { |
106 | SwitchPos = 1;/* Position Hold */ |
106 | SwitchPos = 1;/* Position Hold */ |
107 | } |
107 | } |
108 | 108 | ||
109 | /* Overide On / Off */ |
109 | /* Overide On / Off */ |
110 | if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 || |
110 | if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 10 || |
111 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
111 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 || |
112 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10) |
112 | abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 10) |
113 | { |
113 | { |
114 | Override = 1; |
114 | Override = 1; |
115 | } |
115 | } |
116 | else |
116 | else |
117 | { |
117 | { |
118 | Override = 0; |
118 | Override = 0; |
119 | } |
119 | } |
120 | 120 | ||
121 | /* Set Home Position */ |
121 | /* Set Home Position */ |
122 | if ((actualPos.state > 2) && |
122 | if ((actualPos.state > 2) && |
123 | (homePosValid == 0)) |
123 | (homePosValid == 0)) |
124 | { |
124 | { |
125 | homePos.north = actualPos.north; |
125 | homePos.north = actualPos.north; |
126 | homePos.east = actualPos.east; |
126 | homePos.east = actualPos.east; |
127 | homePos.altitude = actualPos.altitude ; |
127 | homePos.altitude = actualPos.altitude ; |
128 | homePosValid = 1; |
128 | homePosValid = 1; |
129 | } |
129 | } |
130 | 130 | ||
131 | /* Set Target Position */ |
131 | /* Set Target Position */ |
132 | if ((actualPos.state > 2) && |
132 | if ((actualPos.state > 2) && |
133 | (SwitchPos == 0)) |
133 | (SwitchPos == 0)) |
134 | { |
134 | { |
135 | targetPos.north = actualPos.north; |
135 | targetPos.north = actualPos.north; |
136 | targetPos.east = actualPos.east; |
136 | targetPos.east = actualPos.east; |
137 | targetPos.altitude = actualPos.altitude ; |
137 | targetPos.altitude = actualPos.altitude ; |
138 | targetPosValid = 1; |
138 | targetPosValid = 1; |
139 | } |
139 | } |
140 | if ((actualPos.state < 3) && |
140 | if ((actualPos.state < 3) && |
141 | (SwitchPos == 0)) |
141 | (SwitchPos == 0)) |
142 | { |
142 | { |
143 | targetPosValid = 0; |
143 | targetPosValid = 0; |
144 | } |
144 | } |
145 | 145 | ||
146 | /* Set Hold Position */ |
146 | /* Set Hold Position */ |
147 | if ((actualPos.state > 2) && |
147 | if ((actualPos.state > 2) && |
148 | ((Override == 1) || |
148 | ((Override == 1) || |
149 | (SwitchPos == 2) )) /* Update the hold position in case we are in target mode */ |
149 | (SwitchPos == 2) )) /* Update the hold position in case we are in target mode */ |
150 | { |
150 | { |
151 | holdPos.north = actualPos.north; |
151 | holdPos.north = actualPos.north; |
152 | holdPos.east = actualPos.east; |
152 | holdPos.east = actualPos.east; |
153 | holdPos.altitude = actualPos.altitude ; |
153 | holdPos.altitude = actualPos.altitude ; |
154 | holdPosValid = 1; |
154 | holdPosValid = 1; |
155 | } |
155 | } |
156 | if ((actualPos.state < 3) && |
156 | if ((actualPos.state < 3) && |
157 | (Override == 1)) |
157 | (Override == 1)) |
158 | { |
158 | { |
159 | holdPosValid = 0; |
159 | holdPosValid = 0; |
160 | } |
160 | } |
161 | 161 | ||
162 | /* Indicate Valid GPS Position */ |
162 | /* Indicate Valid GPS Position */ |
163 | if ((actualPos.state > 2) && |
163 | if ((actualPos.state > 2) && |
164 | (SwitchPos == 0)) |
164 | (SwitchPos == 0)) |
165 | { |
165 | { |
166 | if(BeepMuster == 0xffff) |
166 | if(BeepMuster == 0xffff) |
167 | { |
167 | { |
168 | beeptime = 5000; |
168 | beeptime = 5000; |
169 | BeepMuster = 0x0100; |
169 | BeepMuster = 0x0100; |
170 | } |
170 | } |
171 | } |
171 | } |
172 | 172 | ||
173 | if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */ |
173 | if (RemoteLinkLost == 0) /* Disable Heading Update in case we lost the RC - Link */ |
174 | { |
174 | { |
175 | max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
175 | max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
176 | max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
176 | max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
177 | } |
177 | } |
178 | else |
178 | else |
179 | { |
179 | { |
180 | max_p = 8; |
180 | max_p = 8; |
181 | max_d = 4; |
181 | max_d = 4; |
182 | } |
182 | } |
183 | 183 | ||
184 | /* Those seem to be good values */ |
184 | /* Those seem to be good values */ |
185 | max_p = 8; |
185 | max_p = 8; |
186 | max_d = 4; |
186 | max_d = 4; |
187 | 187 | ||
188 | //DebugOut.Analog[11] = max_p; |
188 | //DebugOut.Analog[11] = max_p; |
189 | //DebugOut.Analog[12] = max_d; |
189 | //DebugOut.Analog[12] = max_d; |
190 | 190 | ||
191 | static int GPSTrackingCycles = 0; |
191 | static int GPSTrackingCycles = 0; |
192 | long maxGainPos = 140; |
192 | long maxGainPos = 140; |
193 | long maxGainVel = 140; |
193 | long maxGainVel = 140; |
194 | 194 | ||
195 | /* Ramp up gain */ |
195 | /* Ramp up gain */ |
196 | if (GPSTrackingCycles < 1000) |
196 | if (GPSTrackingCycles < 1000) |
197 | { |
197 | { |
198 | GPSTrackingCycles++; |
198 | GPSTrackingCycles++; |
199 | } |
199 | } |
200 | maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
200 | maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
201 | maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
201 | maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
202 | 202 | ||
203 | /* Determine Offset from nominal Position */ |
203 | /* Determine Offset from nominal Position */ |
204 | if (actualPos.state > 2 ) |
204 | if (actualPos.state > 2 ) |
205 | { |
205 | { |
206 | if ((SwitchPos == 2) && |
206 | if ((SwitchPos == 2) && |
207 | (targetPosValid == 1) && |
207 | (targetPosValid == 1) && |
208 | (RemoteLinkLost == 0) && |
208 | (RemoteLinkLost == 0) && |
209 | (Override == 0)) |
209 | (Override == 0)) |
210 | { /* determine distance from target position */ |
210 | { /* determine distance from target position */ |
211 | distanceNS = actualPos.north - targetPos.north; // in 0.1m |
211 | distanceNS = actualPos.north - targetPos.north; // in 0.1m |
212 | distanceEW = actualPos.east - targetPos.east; // in 0.1m |
212 | distanceEW = actualPos.east - targetPos.east; // in 0.1m |
213 | velocityNS = actualPos.velNorth; // in 0.1m/s |
213 | velocityNS = actualPos.velNorth; // in 0.1m/s |
214 | velocityEW = actualPos.velEast; // in 0.1m/s |
214 | velocityEW = actualPos.velEast; // in 0.1m/s |
215 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
215 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
216 | } |
216 | } |
217 | else if ((SwitchPos == 1)&& |
217 | else if ((SwitchPos == 1)&& |
218 | (holdPosValid == 1) && |
218 | (holdPosValid == 1) && |
219 | (RemoteLinkLost == 0) && |
219 | (RemoteLinkLost == 0) && |
220 | (Override == 0)) |
220 | (Override == 0)) |
221 | { /* determine distance from hold position */ |
221 | { /* determine distance from hold position */ |
222 | distanceNS = actualPos.north - holdPos.north; // in 0.1m |
222 | distanceNS = actualPos.north - holdPos.north; // in 0.1m |
223 | distanceEW = actualPos.east - holdPos.east; // in 0.1m |
223 | distanceEW = actualPos.east - holdPos.east; // in 0.1m |
224 | velocityNS = actualPos.velNorth; // in 0.1m/s |
224 | velocityNS = actualPos.velNorth; // in 0.1m/s |
225 | velocityEW = actualPos.velEast; // in 0.1m/s |
225 | velocityEW = actualPos.velEast; // in 0.1m/s |
226 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
226 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
227 | } |
227 | } |
228 | else if ((RemoteLinkLost == 1) && |
228 | else if ((RemoteLinkLost == 1) && |
229 | (homePosValid ==1)) /* determine distance from target position */ |
229 | (homePosValid ==1)) /* determine distance from target position */ |
230 | {/* Overide in case the remote link got lost */ |
230 | {/* Overide in case the remote link got lost */ |
231 | distanceNS = actualPos.north - homePos.north; // in 0.1m |
231 | distanceNS = actualPos.north - homePos.north; // in 0.1m |
232 | distanceEW = actualPos.east - homePos.east; // in 0.1m |
232 | distanceEW = actualPos.east - homePos.east; // in 0.1m |
233 | velocityNS = actualPos.velNorth; // in 0.1m/s |
233 | velocityNS = actualPos.velNorth; // in 0.1m/s |
234 | velocityEW = actualPos.velEast; // in 0.1m/s |
234 | velocityEW = actualPos.velEast; // in 0.1m/s |
235 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
235 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
236 | } |
236 | } |
237 | else |
237 | else |
238 | { |
238 | { |
239 | distanceNS = 0.0F; // in 0.1m |
239 | distanceNS = 0.0F; // in 0.1m |
240 | distanceEW = 0.0F; // in 0.1m |
240 | distanceEW = 0.0F; // in 0.1m |
241 | velocityNS = 0.0F; // in 0.1m/s |
241 | velocityNS = 0.0F; // in 0.1m/s |
242 | velocityEW = 0.0F; // in 0.1m/s |
242 | velocityEW = 0.0F; // in 0.1m/s |
243 | maxDistance = 0.0F; |
243 | maxDistance = 0.0F; |
244 | GPSTrackingCycles = 0; |
244 | GPSTrackingCycles = 0; |
245 | } |
245 | } |
246 | 246 | ||
247 | /* Limit GPS_Nick to 25 */ |
247 | /* Limit GPS_Nick to 25 */ |
248 | nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
248 | nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
249 | nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS * max_d)/50))); //m/s |
249 | nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityNS * max_d)/50))); //m/s |
250 | roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
250 | roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
251 | roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50))); //m/s |
251 | roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((velocityEW * max_d)/50))); //m/s |
252 | TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
252 | TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
253 | 253 | ||
254 | /* PD-Control */ |
254 | /* PD-Control */ |
255 | nick_gain = nick_gain_p + nick_gain_d; |
255 | nick_gain = nick_gain_p + nick_gain_d; |
256 | roll_gain = -(roll_gain_p + roll_gain_d); |
256 | roll_gain = -(roll_gain_p + roll_gain_d); |
257 | 257 | ||
258 | /* Calculate Distance to Target */ |
258 | /* Calculate Distance to Target */ |
259 | SIN_H = (float) sin(status.Psi); |
259 | SIN_H = (float) sin(status.Psi); |
260 | COS_H =(float) cos(status.Psi); |
260 | COS_H =(float) cos(status.Psi); |
261 | 261 | ||
262 | GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
262 | GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
263 | GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
263 | GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
264 | 264 | ||
265 | GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
265 | GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
266 | GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
266 | GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
267 | 267 | ||
268 | /* Set New Heading */ |
268 | /* Set New Heading */ |
269 | SetNewHeading(maxDistance); |
269 | SetNewHeading(maxDistance); |
270 | } |
270 | } |
271 | else |
271 | else |
272 | { |
272 | { |
273 | GPS_Nick = 0; |
273 | GPS_Nick = 0; |
274 | GPS_Roll = 0; |
274 | GPS_Roll = 0; |
275 | maxDistance = 0.0F; |
275 | maxDistance = 0.0F; |
276 | } |
276 | } |
277 | } |
277 | } |
278 | 278 | ||
279 | 279 | ||
280 | #if 0 |
280 | #if 0 |
281 | if (maxDistance / 10 < 12) |
281 | if (maxDistance / 10 < 12) |
282 | { |
282 | { |
283 | static int iState = 0; |
283 | static int iState = 0; |
284 | switch (iState) |
284 | switch (iState) |
285 | { |
285 | { |
286 | case 0: |
286 | case 0: |
287 | targetPos.north += 400; |
287 | targetPos.north += 400; |
288 | targetPos.east += 0; |
288 | targetPos.east += 0; |
289 | GPSTrackingCycles = 0; |
289 | GPSTrackingCycles = 0; |
290 | iState++; |
290 | iState++; |
291 | break; |
291 | break; |
292 | case 1: |
292 | case 1: |
293 | targetPos.north += 0; |
293 | targetPos.north += 0; |
294 | targetPos.east += 400; |
294 | targetPos.east += 400; |
295 | GPSTrackingCycles = 0; |
295 | GPSTrackingCycles = 0; |
296 | iState++; |
296 | iState++; |
297 | break; |
297 | break; |
298 | case 2: |
298 | case 2: |
299 | targetPos.north -= 400; |
299 | targetPos.north -= 400; |
300 | targetPos.east += 0; |
300 | targetPos.east += 0; |
301 | GPSTrackingCycles = 0; |
301 | GPSTrackingCycles = 0; |
302 | iState++; |
302 | iState++; |
303 | break; |
303 | break; |
304 | case 3: |
304 | case 3: |
305 | targetPos.north += 0; |
305 | targetPos.north += 0; |
306 | targetPos.east -= 400; |
306 | targetPos.east -= 400; |
307 | GPSTrackingCycles = 0; |
307 | GPSTrackingCycles = 0; |
308 | iState=0; |
308 | iState=0; |
309 | break; |
309 | break; |
310 | default: |
310 | default: |
311 | iState=0; |
311 | iState=0; |
312 | break; |
312 | break; |
313 | } |
313 | } |
314 | beeptime = 5000; |
314 | beeptime = 5000; |
315 | BeepMuster = 0x0c00; |
315 | BeepMuster = 0x0c00; |
316 | } |
316 | } |
317 | #endif |
317 | #endif |
318 | 318 | ||
319 | 319 | ||
320 | void SetNewHeading(unsigned long maxDistance) |
320 | void SetNewHeading(unsigned long maxDistance) |
321 | /* Set New Heading */ |
321 | /* Set New Heading */ |
322 | { |
322 | { |
323 | int OffsetGier = 0; |
323 | int OffsetGier = 0; |
324 | if (maxDistance / 10 > 8) |
324 | if (maxDistance / 10 > 8) |
325 | { |
325 | { |
326 | OffsetGier = 4; |
326 | OffsetGier = 4; |
327 | } |
327 | } |
328 | 328 | ||
329 | if (TargetGier < 0) |
329 | if (TargetGier < 0) |
330 | { |
330 | { |
331 | TargetGier += 3600; |
331 | TargetGier += 3600; |
332 | } |
332 | } |
333 | if (TargetGier > sollGier) |
333 | if (TargetGier > sollGier) |
334 | { |
334 | { |
335 | if (TargetGier - sollGier < 1800) |
335 | if (TargetGier - sollGier < 1800) |
336 | { |
336 | { |
337 | sollGier += OffsetGier; |
337 | sollGier += OffsetGier; |
338 | } |
338 | } |
339 | else |
339 | else |
340 | { |
340 | { |
341 | sollGier -= OffsetGier; |
341 | sollGier -= OffsetGier; |
342 | } |
342 | } |
343 | } |
343 | } |
344 | else |
344 | else |
345 | { |
345 | { |
346 | if (sollGier - TargetGier < 1800) |
346 | if (sollGier - TargetGier < 1800) |
347 | { |
347 | { |
348 | sollGier -= OffsetGier; |
348 | sollGier -= OffsetGier; |
349 | } |
349 | } |
350 | else |
350 | else |
351 | { |
351 | { |
352 | sollGier += OffsetGier; |
352 | sollGier += OffsetGier; |
353 | } |
353 | } |
354 | } |
354 | } |
355 | } |
355 | } |
356 | 356 | ||
357 | /* **************************************************************************** |
357 | /* **************************************************************************** |
358 | Functionname: InitGPS */ /*! |
358 | Functionname: InitGPS */ /*! |
359 | Description: |
359 | Description: |
360 | 360 | ||
361 | @return void |
361 | @return void |
362 | @pre - |
362 | @pre - |
363 | @post - |
363 | @post - |
364 | @author |
364 | @author |
365 | **************************************************************************** */ |
365 | **************************************************************************** */ |
366 | void InitGPS(void) |
366 | void InitGPS(void) |
367 | { |
367 | { |
368 | // USART1 Control and Status Register A, B, C and baud rate register |
368 | // USART1 Control and Status Register A, B, C and baud rate register |
369 | uint8_t sreg = SREG; |
369 | uint8_t sreg = SREG; |
370 | //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
370 | //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
371 | 371 | ||
372 | // disable all interrupts before reconfiguration |
372 | // disable all interrupts before reconfiguration |
373 | cli(); |
373 | cli(); |
374 | 374 | ||
375 | // disable RX-Interrupt |
375 | // disable RX-Interrupt |
376 | UCSR1B &= ~(1 << RXCIE1); |
376 | UCSR1B &= ~(1 << RXCIE1); |
377 | // disable TX-Interrupt |
377 | // disable TX-Interrupt |
378 | UCSR1B &= ~(1 << TXCIE1); |
378 | UCSR1B &= ~(1 << TXCIE1); |
379 | // disable DRE-Interrupt |
379 | // disable DRE-Interrupt |
380 | UCSR1B &= ~(1 << UDRIE1); |
380 | UCSR1B &= ~(1 << UDRIE1); |
381 | 381 | ||
382 | // set direction of RXD1 and TXD1 pins |
382 | // set direction of RXD1 and TXD1 pins |
383 | // set RXD1 (PD2) as an input pin |
383 | // set RXD1 (PD2) as an input pin |
384 | PORTD |= (1 << PORTD2); |
384 | PORTD |= (1 << PORTD2); |
385 | DDRD &= ~(1 << DDD2); |
385 | DDRD &= ~(1 << DDD2); |
386 | 386 | ||
387 | // set TXD1 (PD3) as an output pin |
387 | // set TXD1 (PD3) as an output pin |
388 | PORTD |= (1 << PORTD3); |
388 | PORTD |= (1 << PORTD3); |
389 | DDRD |= (1 << DDD3); |
389 | DDRD |= (1 << DDD3); |
390 | 390 | ||
391 | // USART0 Baud Rate Register |
391 | // USART0 Baud Rate Register |
392 | // set clock divider |
392 | // set clock divider |
393 | UBRR1H = 0; |
393 | UBRR1H = 0; |
394 | UBRR1L = BAUDRATE; |
394 | UBRR1L = BAUDRATE; |
395 | 395 | ||
396 | // enable double speed operation |
396 | // enable double speed operation |
397 | //UCSR1A |= (1 << U2X1); |
397 | //UCSR1A |= (1 << U2X1); |
398 | UCSR1A = _B0(U2X1); |
398 | UCSR1A = _B0(U2X1); |
399 | 399 | ||
400 | // enable receiver and transmitter |
400 | // enable receiver and transmitter |
401 | UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
401 | UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
402 | // set asynchronous mode |
402 | // set asynchronous mode |
403 | UCSR1C &= ~(1 << UMSEL11); |
403 | UCSR1C &= ~(1 << UMSEL11); |
404 | UCSR1C &= ~(1 << UMSEL10); |
404 | UCSR1C &= ~(1 << UMSEL10); |
405 | // no parity |
405 | // no parity |
406 | UCSR1C &= ~(1 << UPM11); |
406 | UCSR1C &= ~(1 << UPM11); |
407 | UCSR1C &= ~(1 << UPM10); |
407 | UCSR1C &= ~(1 << UPM10); |
408 | // 1 stop bit |
408 | // 1 stop bit |
409 | UCSR1C &= ~(1 << USBS1); |
409 | UCSR1C &= ~(1 << USBS1); |
410 | // 8-bit |
410 | // 8-bit |
411 | UCSR1B &= ~(1 << UCSZ12); |
411 | UCSR1B &= ~(1 << UCSZ12); |
412 | UCSR1C |= (1 << UCSZ11); |
412 | UCSR1C |= (1 << UCSZ11); |
413 | UCSR1C |= (1 << UCSZ10); |
413 | UCSR1C |= (1 << UCSZ10); |
414 | 414 | ||
415 | // flush receive buffer explicit |
415 | // flush receive buffer explicit |
416 | while ( UCSR1A & (1<<RXC1) ) UDR1; |
416 | while ( UCSR1A & (1<<RXC1) ) UDR1; |
417 | 417 | ||
418 | // enable interrupts at the end |
418 | // enable interrupts at the end |
419 | // enable RX-Interrupt |
419 | // enable RX-Interrupt |
420 | UCSR1B |= (1 << RXCIE1); |
420 | UCSR1B |= (1 << RXCIE1); |
421 | // enable TX-Interrupt |
421 | // enable TX-Interrupt |
422 | UCSR1B |= (1 << TXCIE1); |
422 | UCSR1B |= (1 << TXCIE1); |
423 | // enable DRE interrupt |
423 | // enable DRE interrupt |
424 | //UCSR1B |= (1 << UDRIE1); |
424 | //UCSR1B |= (1 << UDRIE1); |
425 | 425 | ||
426 | // restore global interrupt flags |
426 | // restore global interrupt flags |
427 | SREG = sreg; |
427 | SREG = sreg; |
428 | 428 | ||
429 | gpsState = GPS_EMPTY; |
429 | gpsState = GPS_EMPTY; |
430 | } |
430 | } |
431 | 431 | ||
432 | 432 | ||
433 | /* **************************************************************************** |
433 | /* **************************************************************************** |
434 | Functionname: InitGPS */ /*! |
434 | Functionname: InitGPS */ /*! |
435 | Description: Copy GPS paket data to actualPos |
435 | Description: Copy GPS paket data to actualPos |
436 | 436 | ||
437 | @return void |
437 | @return void |
438 | @pre - |
438 | @pre - |
439 | @post - |
439 | @post - |
440 | @author |
440 | @author |
441 | **************************************************************************** */ |
441 | **************************************************************************** */ |
442 | void GPSscanData (void) |
442 | void GPSscanData (void) |
443 | { |
443 | { |
444 | if (navStatus.packetStatus == 1)// valid packet |
444 | if (navStatus.packetStatus == 1)// valid packet |
445 | { |
445 | { |
446 | actualPos.state = navStatus.GPSfix; |
446 | actualPos.state = navStatus.GPSfix; |
447 | if ((actualPos.state > 1) && (GPSTracking == 0)) |
447 | if ((actualPos.state > 1) && (GPSTracking == 0)) |
448 | { |
448 | { |
449 | GRN_FLASH; |
449 | GRN_FLASH; |
450 | } |
450 | } |
451 | navStatus.packetStatus = 0; |
451 | navStatus.packetStatus = 0; |
452 | } |
452 | } |
453 | 453 | ||
454 | if (navPosUtm.packetStatus == 1)// valid packet |
454 | if (navPosUtm.packetStatus == 1)// valid packet |
455 | { |
455 | { |
456 | actualPos.north = navPosUtm.NORTH/10L; |
456 | actualPos.north = navPosUtm.NORTH/10L; |
457 | actualPos.east = navPosUtm.EAST/10L; |
457 | actualPos.east = navPosUtm.EAST/10L; |
458 | actualPos.altitude = navPosUtm.ALT/100; |
458 | actualPos.altitude = navPosUtm.ALT/100; |
459 | actualPos.ITOW = navPosUtm.ITOW; |
459 | actualPos.ITOW = navPosUtm.ITOW; |
460 | navPosUtm.packetStatus = 0; |
460 | navPosUtm.packetStatus = 0; |
461 | } |
461 | } |
462 | /* |
462 | /* |
463 | if (navPosLlh.packetStatus == 1) |
463 | if (navPosLlh.packetStatus == 1) |
464 | { |
464 | { |
465 | actualPos.longi = navPosLlh.LON; |
465 | actualPos.longi = navPosLlh.LON; |
466 | actualPos.lati = navPosLlh.LAT; |
466 | actualPos.lati = navPosLlh.LAT; |
467 | actualPos.height = navPosLlh.HEIGHT; |
467 | actualPos.height = navPosLlh.HEIGHT; |
468 | navPosLlh.packetStatus = 0; |
468 | navPosLlh.packetStatus = 0; |
469 | } |
469 | } |
470 | */ |
470 | */ |
471 | if (navVelNed.packetStatus == 1) |
471 | if (navVelNed.packetStatus == 1) |
472 | { |
472 | { |
473 | actualPos.velNorth = navVelNed.VEL_N; |
473 | actualPos.velNorth = navVelNed.VEL_N; |
474 | actualPos.velEast = navVelNed.VEL_E; |
474 | actualPos.velEast = navVelNed.VEL_E; |
475 | actualPos.velDown = navVelNed.VEL_D; |
475 | actualPos.velDown = navVelNed.VEL_D; |
476 | actualPos.groundSpeed = navVelNed.GSpeed; |
476 | actualPos.groundSpeed = navVelNed.GSpeed; |
477 | navVelNed.packetStatus = 0; |
477 | navVelNed.packetStatus = 0; |
478 | } |
478 | } |
479 | } |
479 | } |
480 | 480 | ||
481 | /* **************************************************************************** |
481 | /* **************************************************************************** |
482 | Functionname: SIGNAL */ /*! |
482 | Functionname: SIGNAL */ /*! |
483 | Description: |
483 | Description: |
484 | 484 | ||
485 | @return void |
485 | @return void |
486 | @pre - |
486 | @pre - |
487 | @post - |
487 | @post - |
488 | @author |
488 | @author |
489 | **************************************************************************** */ |
489 | **************************************************************************** */ |
490 | SIGNAL (SIG_USART1_RECV) |
490 | SIGNAL (SIG_USART1_RECV) |
491 | { |
491 | { |
492 | uint8_t c; |
492 | uint8_t c; |
493 | uint8_t re; |
493 | uint8_t re; |
494 | 494 | ||
495 | re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ? |
495 | re = (UCSR1A & (_B1(FE1) | _B1(DOR1)));// any error occured ? |
496 | c = UDR1; |
496 | c = UDR1; |
497 | 497 | ||
498 | if (re == 0) |
498 | if (re == 0) |
499 | { |
499 | { |
500 | switch (gpsState) |
500 | switch (gpsState) |
501 | { |
501 | { |
502 | case GPS_EMPTY: |
502 | case GPS_EMPTY: |
503 | if (c == SYNC_CHAR1) |
503 | if (c == SYNC_CHAR1) |
504 | { |
504 | { |
505 | gpsState = GPS_SYNC1; |
505 | gpsState = GPS_SYNC1; |
506 | } |
506 | } |
507 | break; |
507 | break; |
508 | case GPS_SYNC1: |
508 | case GPS_SYNC1: |
509 | if (c == SYNC_CHAR2) |
509 | if (c == SYNC_CHAR2) |
510 | { |
510 | { |
511 | gpsState = GPS_SYNC2; |
511 | gpsState = GPS_SYNC2; |
512 | } |
512 | } |
513 | else if (c != SYNC_CHAR1) |
513 | else if (c != SYNC_CHAR1) |
514 | { |
514 | { |
515 | gpsState = GPS_EMPTY; |
515 | gpsState = GPS_EMPTY; |
516 | } |
516 | } |
517 | break; |
517 | break; |
518 | case GPS_SYNC2: |
518 | case GPS_SYNC2: |
519 | if (c == CLASS_NAV) |
519 | if (c == CLASS_NAV) |
520 | { |
520 | { |
521 | gpsState = GPS_CLASS; |
521 | gpsState = GPS_CLASS; |
522 | } |
522 | } |
523 | else |
523 | else |
524 | { |
524 | { |
525 | gpsState = GPS_EMPTY; |
525 | gpsState = GPS_EMPTY; |
526 | } |
526 | } |
527 | break; |
527 | break; |
528 | case GPS_CLASS:// msg ID seen: init packed receive |
528 | case GPS_CLASS:// msg ID seen: init packed receive |
529 | msgID = c; |
529 | msgID = c; |
530 | CK_A = CLASS_NAV + c; |
530 | CK_A = CLASS_NAV + c; |
531 | CK_B = CLASS_NAV + CK_A; |
531 | CK_B = CLASS_NAV + CK_A; |
532 | gpsState = GPS_LEN1; |
532 | gpsState = GPS_LEN1; |
533 | 533 | ||
534 | switch (msgID) |
534 | switch (msgID) |
535 | { |
535 | { |
536 | case MSGID_STATUS: |
536 | case MSGID_STATUS: |
537 | ubxP = (char*)&navStatus; |
537 | ubxP = (char*)&navStatus; |
538 | ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
538 | ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t)); |
539 | ubxSp = (char*)&navStatus.packetStatus; |
539 | ubxSp = (char*)&navStatus.packetStatus; |
540 | ignorePacket = navStatus.packetStatus; |
540 | ignorePacket = navStatus.packetStatus; |
541 | break; |
541 | break; |
542 | /* |
542 | /* |
543 | case MSGID_POSLLH: |
543 | case MSGID_POSLLH: |
544 | ubxP = (char*)&navPosLlh; |
544 | ubxP = (char*)&navPosLlh; |
545 | ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
545 | ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t)); |
546 | ubxSp = (char*)&navPosLlh.packetStatus; |
546 | ubxSp = (char*)&navPosLlh.packetStatus; |
547 | ignorePacket = navPosLlh.packetStatus; |
547 | ignorePacket = navPosLlh.packetStatus; |
548 | break; |
548 | break; |
549 | */ |
549 | */ |
550 | case MSGID_POSUTM: |
550 | case MSGID_POSUTM: |
551 | ubxP = (char*)&navPosUtm; |
551 | ubxP = (char*)&navPosUtm; |
552 | ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
552 | ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t)); |
553 | ubxSp = (char*)&navPosUtm.packetStatus; |
553 | ubxSp = (char*)&navPosUtm.packetStatus; |
554 | ignorePacket = navPosUtm.packetStatus; |
554 | ignorePacket = navPosUtm.packetStatus; |
555 | break; |
555 | break; |
556 | case MSGID_VELNED: |
556 | case MSGID_VELNED: |
557 | ubxP = (char*)&navVelNed; |
557 | ubxP = (char*)&navVelNed; |
558 | ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
558 | ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t)); |
559 | ubxSp = (char*)&navVelNed.packetStatus; |
559 | ubxSp = (char*)&navVelNed.packetStatus; |
560 | ignorePacket = navVelNed.packetStatus; |
560 | ignorePacket = navVelNed.packetStatus; |
561 | break; |
561 | break; |
562 | default: |
562 | default: |
563 | ignorePacket = 1; |
563 | ignorePacket = 1; |
564 | ubxSp = (char*)0; |
564 | ubxSp = (char*)0; |
565 | } |
565 | } |
566 | break; |
566 | break; |
567 | case GPS_LEN1:// first len byte |
567 | case GPS_LEN1:// first len byte |
568 | msgLen = c; |
568 | msgLen = c; |
569 | CK_A += c; |
569 | CK_A += c; |
570 | CK_B += CK_A; |
570 | CK_B += CK_A; |
571 | gpsState = GPS_LEN2; |
571 | gpsState = GPS_LEN2; |
572 | break; |
572 | break; |
573 | case GPS_LEN2:// second len byte |
573 | case GPS_LEN2:// second len byte |
574 | msgLen = msgLen + (c * 256); |
574 | msgLen = msgLen + (c * 256); |
575 | CK_A += c; |
575 | CK_A += c; |
576 | CK_B += CK_A; |
576 | CK_B += CK_A; |
577 | gpsState = GPS_FILLING;// next data will be stored in packet struct |
577 | gpsState = GPS_FILLING;// next data will be stored in packet struct |
578 | break; |
578 | break; |
579 | case GPS_FILLING: |
579 | case GPS_FILLING: |
580 | CK_A += c; |
580 | CK_A += c; |
581 | CK_B += CK_A; |
581 | CK_B += CK_A; |
582 | 582 | ||
583 | if ( !ignorePacket && ubxP < ubxEp) |
583 | if ( !ignorePacket && ubxP < ubxEp) |
584 | { |
584 | { |
585 | *ubxP++ = c; |
585 | *ubxP++ = c; |
586 | } |
586 | } |
587 | 587 | ||
588 | if (--msgLen == 0) |
588 | if (--msgLen == 0) |
589 | { |
589 | { |
590 | gpsState = GPS_CKA; |
590 | gpsState = GPS_CKA; |
591 | } |
591 | } |
592 | break; |
592 | break; |
593 | case GPS_CKA: |
593 | case GPS_CKA: |
594 | if (c == CK_A) |
594 | if (c == CK_A) |
595 | { |
595 | { |
596 | gpsState = GPS_CKB; |
596 | gpsState = GPS_CKB; |
597 | } |
597 | } |
598 | else |
598 | else |
599 | { |
599 | { |
600 | gpsState = GPS_EMPTY; |
600 | gpsState = GPS_EMPTY; |
601 | } |
601 | } |
602 | break; |
602 | break; |
603 | case GPS_CKB: |
603 | case GPS_CKB: |
604 | if (c == CK_B && ubxSp)// No error -> packet received successfully |
604 | if (c == CK_B && ubxSp)// No error -> packet received successfully |
605 | { |
605 | { |
606 | *ubxSp = 1;// set packetStatus in struct |
606 | *ubxSp = 1;// set packetStatus in struct |
607 | GPSscanData(); |
607 | GPSscanData(); |
608 | } |
608 | } |
609 | gpsState = GPS_EMPTY;// ready for next packet |
609 | gpsState = GPS_EMPTY;// ready for next packet |
610 | break; |
610 | break; |
611 | default: |
611 | default: |
612 | gpsState = GPS_EMPTY;// ready for next packet |
612 | gpsState = GPS_EMPTY;// ready for next packet |
613 | } |
613 | } |
614 | } |
614 | } |
615 | else// discard any data if error occured |
615 | else// discard any data if error occured |
616 | { |
616 | { |
617 | gpsState = GPS_EMPTY; |
617 | gpsState = GPS_EMPTY; |
618 | } |
618 | } |
619 | } |
619 | } |
620 | 620 | ||
621 | 621 | ||
622 | 622 | ||
623 | 623 | ||
624 | 624 |