Rev 779 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 779 | Rev 781 | ||
---|---|---|---|
Line 17... | Line 17... | ||
17 | 17 | ||
18 | int16_t GPS_Pitch = 0; |
18 | int16_t GPS_Pitch = 0; |
Line 19... | Line 19... | ||
19 | int16_t GPS_Roll = 0; |
19 | int16_t GPS_Roll = 0; |
20 | - | ||
- | 20 | ||
Line 21... | Line 21... | ||
21 | int32_t GPS_P_Factor = 0, GPS_D_Factor = 0; |
21 | int32_t GPS_P_Factor = 0, GPS_D_Factor = 0; |
22 | int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
22 | |
23 | 23 | ||
24 | 24 | ||
25 | typedef struct |
25 | typedef struct |
26 | { |
- | |
27 | int32_t Northing; |
26 | { |
Line 28... | Line 27... | ||
28 | int32_t Easting; |
27 | int32_t Longitude; |
29 | uint8_t Status; |
28 | int32_t Latitude; |
30 | 29 | uint8_t Status; |
|
31 | } GPS_Pos_t; |
30 | } GPS_Pos_t; |
Line 32... | Line 31... | ||
32 | 31 | ||
Line 33... | Line 32... | ||
33 | // GPS coordinates for hold position |
32 | // GPS coordinates for hold position |
34 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
33 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
35 | // GPS coordinates for home position |
34 | // GPS coordinates for home position |
- | 35 | GPS_Pos_t HomePosition = {0,0, INVALID}; |
|
- | 36 | ||
36 | GPS_Pos_t HomePosition = {0,0, INVALID}; |
37 | |
37 | 38 | // --------------------------------------------------------------------------------- |
|
38 | 39 | ||
39 | // --------------------------------------------------------------------------------- |
40 | // set home position to current positon |
- | 41 | void GPS_SetHomePosition(void) |
|
- | 42 | { |
|
- | 43 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
|
- | 44 | { |
|
- | 45 | HomePosition.Longitude = GPSInfo.longitude; |
|
- | 46 | HomePosition.Latitude = GPSInfo.latitude; |
|
- | 47 | HomePosition.Status = VALID; |
|
- | 48 | BeepTime = 1000; // signal if new home position was set |
|
- | 49 | } |
|
- | 50 | else |
|
- | 51 | { |
|
- | 52 | HomePosition.Status = INVALID; |
|
- | 53 | } |
|
- | 54 | } |
|
- | 55 | ||
- | 56 | // set hold position to current positon |
|
- | 57 | void GPS_SetHoldPosition(void) |
|
- | 58 | { |
|
- | 59 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
|
- | 60 | { |
|
40 | 61 | HoldPosition.Longitude = GPSInfo.longitude; |
|
Line 41... | Line 62... | ||
41 | // set home position to current hold positon |
62 | HoldPosition.Latitude = GPSInfo.latitude; |
42 | void GPS_SetHomePosition(void) |
63 | HoldPosition.Status = VALID; |
43 | { |
64 | } |
Line 62... | Line 83... | ||
62 | 83 | ||
63 | // calculates the GPS control sticks values from the deviation to target position |
84 | // calculates the GPS control sticks values from the deviation to target position |
64 | void GPS_PDController(GPS_Pos_t *TargetPos) |
85 | void GPS_PDController(GPS_Pos_t *TargetPos) |
65 | { |
86 | { |
- | 87 | int32_t coscompass, sincompass; |
|
66 | int32_t coscompass, sincompass; |
88 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
67 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
89 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
- | 90 | int32_t PD_North = 0, PD_East = 0; |
|
- | 91 | static int32_t cos_target_latitude = 1; |
|
Line 68... | Line 92... | ||
68 | int32_t PD_North = 0, PD_East = 0; |
92 | |
69 | 93 | ||
- | 94 | if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
|
70 | if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
95 | { |
- | 96 | // calculate position deviation from latitude and longitude differences |
|
- | 97 | GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally |
|
- | 98 | GPSPosDev_East = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally |
|
- | 99 | // recalculate the target latitude projection only if the target data are updated |
|
- | 100 | // to save time |
|
- | 101 | if (TargetPos->Status != PROCESSED) |
|
- | 102 | { |
|
- | 103 | cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L)); |
|
- | 104 | TargetPos->Status = PROCESSED; |
|
71 | { |
105 | } |
- | 106 | // calculate latitude projection |
|
Line 72... | Line 107... | ||
72 | GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing; |
107 | GPSPosDev_East *= cos_target_latitude; |
73 | GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting; |
108 | GPSPosDev_East /= 8192; |
Line 74... | Line 109... | ||
74 | 109 | ||
Line 156... | Line 191... | ||
156 | if(GPS_Task != TSK_IDLE) |
191 | if(GPS_Task != TSK_IDLE) |
157 | { |
192 | { |
158 | BeepTime = 100; // beep if signal is neccesary |
193 | BeepTime = 100; // beep if signal is neccesary |
159 | } |
194 | } |
160 | break; |
195 | break; |
161 | case PROCESSED: // if gps data are already processed |
196 | case PROCESSED: // if gps data are already processed do nothing |
162 | // downcount timeout |
197 | // downcount timeout |
163 | if(GPSTimeout) GPSTimeout--; |
198 | if(GPSTimeout) GPSTimeout--; |
164 | // if no new data arrived within timeout set current data invalid |
199 | // if no new data arrived within timeout set current data invalid |
165 | // and therefore disable GPS |
200 | // and therefore disable GPS |
166 | else |
201 | else |
Line 168... | Line 203... | ||
168 | GPS_Neutral(); |
203 | GPS_Neutral(); |
169 | GPSInfo.status = INVALID; |
204 | GPSInfo.status = INVALID; |
170 | } |
205 | } |
171 | break; |
206 | break; |
172 | case VALID: // new valid data from gps device |
207 | case VALID: // new valid data from gps device |
173 | // if the gps data quality is sufficient |
208 | // if the gps data quality is good |
174 | if (GPSInfo.satfix == SATFIX_3D) |
209 | if (GPSInfo.satfix == SATFIX_3D) |
175 | { |
210 | { |
176 | switch(GPS_Task) // check what's to do |
211 | switch(GPS_Task) // check what's to do |
177 | { |
212 | { |
178 | case TSK_IDLE: |
213 | case TSK_IDLE: |
179 | // update hold point to current gps position |
214 | // update hold position to current gps position |
180 | HoldPosition.Northing = GPSInfo.utmnorth; |
- | |
181 | HoldPosition.Easting = GPSInfo.utmeast; |
215 | GPS_SetHoldPosition(); // can get invalid if gps signal is bad |
182 | HoldPosition.Status = VALID; |
- | |
183 | // disable gps control |
216 | // disable gps control |
184 | GPS_Neutral(); |
217 | GPS_Neutral(); |
185 | break; // eof TSK_IDLE |
218 | break; // eof TSK_IDLE |
186 | case TSK_HOLD: |
219 | case TSK_HOLD: |
187 | if(HoldPosition.Status == VALID) |
220 | if(HoldPosition.Status != INVALID) |
188 | { |
221 | { |
189 | // if sticks are centered (no manual control) |
222 | // if sticks are centered (no manual control) |
190 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
223 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
191 | { |
224 | { |
192 | GPS_PDController(&HoldPosition); |
225 | GPS_PDController(&HoldPosition); |
193 | } |
226 | } |
194 | else // MK controlled by user |
227 | else // MK controlled by user |
195 | { |
228 | { |
196 | // update hold point to current gps position |
229 | // update hold point to current gps position |
197 | HoldPosition.Northing = GPSInfo.utmnorth; |
- | |
198 | HoldPosition.Easting = GPSInfo.utmeast; |
- | |
199 | HoldPosition.Status = GPSInfo.status; |
230 | GPS_SetHoldPosition(); |
200 | // disable gps control |
231 | // disable gps control |
201 | GPS_Neutral(); |
232 | GPS_Neutral(); |
202 | } |
233 | } |
203 | } |
234 | } |
- | 235 | else // invalid Hold Position |
|
- | 236 | { // try to catch a valid hold position from gps data input |
|
- | 237 | GPS_SetHoldPosition(); |
|
- | 238 | GPS_Neutral(); |
|
- | 239 | } |
|
204 | break; // eof TSK_HOLD |
240 | break; // eof TSK_HOLD |
205 | case TSK_HOME: |
241 | case TSK_HOME: |
206 | if(HomePosition.Status == VALID) |
242 | if(HomePosition.Status != INVALID) |
207 | { |
243 | { |
208 | // update hold point to current gps position |
244 | // update hold point to current gps position |
209 | // to avoid a flight back if home comming is deactivated |
245 | // to avoid a flight back if home comming is deactivated |
210 | HoldPosition.Northing = GPSInfo.utmnorth; |
- | |
211 | HoldPosition.Easting = GPSInfo.utmeast; |
- | |
212 | HoldPosition.Status = GPSInfo.status; |
246 | GPS_SetHoldPosition(); |
213 | // if sticks are centered (no manual control) |
247 | // if sticks are centered (no manual control) |
214 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
248 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
215 | { |
249 | { |
216 | GPS_PDController(&HomePosition); |
250 | GPS_PDController(&HomePosition); |
217 | } |
251 | } |
Line 222... | Line 256... | ||
222 | } |
256 | } |
223 | else // bad home position |
257 | else // bad home position |
224 | { |
258 | { |
225 | BeepTime = 50; // signal invalid home position |
259 | BeepTime = 50; // signal invalid home position |
226 | // try to hold at least the position as a fallback option |
260 | // try to hold at least the position as a fallback option |
- | 261 | ||
227 | // if sticks are centered (no manual control) |
262 | if (HoldPosition.Status != INVALID) |
228 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID)) |
- | |
229 | { |
263 | { |
- | 264 | // if sticks are centered (no manual control) |
|
- | 265 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
|
- | 266 | { |
|
230 | GPS_PDController(&HoldPosition); |
267 | GPS_PDController(&HoldPosition); |
- | 268 | } |
|
- | 269 | else // manual control or no reference position |
|
- | 270 | { |
|
- | 271 | GPS_Neutral(); |
|
- | 272 | } |
|
231 | } |
273 | } |
- | 274 | else |
|
232 | else // manual control or no rteference position |
275 | { // try to catch a valid hold position |
233 | { |
276 | GPS_SetHoldPosition(); |
234 | GPS_Neutral(); |
277 | GPS_Neutral(); |
235 | } |
278 | } |
236 | } |
279 | } |
237 | break; // eof TSK_HOME |
280 | break; // eof TSK_HOME |
238 | default: // unhandled task |
281 | default: // unhandled task |
239 | GPS_Neutral(); |
282 | GPS_Neutral(); |
240 | break; // eof default |
283 | break; // eof default |
241 | } // eof switch GPS_Task |
284 | } // eof switch GPS_Task |
242 | } // eof 3D-FIX |
285 | } // eof 3D-FIX |
243 | - | ||
244 | else // no 3D-SATFIX |
286 | else // no 3D-SATFIX |
245 | { // disable gps control |
287 | { // disable gps control |
246 | GPS_Neutral(); |
288 | GPS_Neutral(); |
247 | if(GPS_Task != TSK_IDLE) |
289 | if(GPS_Task != TSK_IDLE) |
248 | { |
290 | { |