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