Rev 792 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 792 | Rev 794 | ||
|---|---|---|---|
| Line 3... | Line 3... | ||
| 3 | #include "fc.h" |
3 | #include "fc.h" |
| 4 | #include "ubx.h" |
4 | #include "ubx.h" |
| 5 | #include "mymath.h" |
5 | #include "mymath.h" |
| 6 | #include "timer0.h" |
6 | #include "timer0.h" |
| 7 | #include "uart.h" |
7 | #include "uart.h" |
| - | 8 | #include "rc.h" |
|
| - | 9 | #include "eeprom.h" |
|
| Line 8... | Line 10... | ||
| 8 | 10 | ||
| 9 | #define TSK_IDLE 0 |
11 | #define TSK_IDLE 0 |
| 10 | #define TSK_HOLD 1 |
12 | #define TSK_HOLD 1 |
| Line 24... | Line 26... | ||
| 24 | 26 | ||
| 25 | typedef struct |
27 | typedef struct |
| 26 | { |
28 | { |
| 27 | int32_t Longitude; |
29 | int32_t Longitude; |
| - | 30 | int32_t Latitude; |
|
| 28 | int32_t Latitude; |
31 | int32_t Altitude; |
| 29 | uint8_t Status; |
32 | uint8_t Status; |
| Line 30... | Line 33... | ||
| 30 | } GPS_Pos_t; |
33 | } GPS_Pos_t; |
| 31 | 34 | ||
| 32 | // GPS coordinates for hold position |
35 | // GPS coordinates for hold position |
| 33 | GPS_Pos_t HoldPosition = {0,0, INVALID}; |
36 | GPS_Pos_t HoldPosition = {0,0,0,INVALID}; |
| Line 34... | Line 37... | ||
| 34 | // GPS coordinates for home position |
37 | // GPS coordinates for home position |
| Line -... | Line 38... | ||
| - | 38 | GPS_Pos_t HomePosition = {0,0,0,INVALID}; |
|
| - | 39 | ||
| - | 40 | ||
| - | 41 | // --------------------------------------------------------------------------------- |
|
| - | 42 | ||
| - | 43 | // checks pitch and roll sticks for manual control |
|
| - | 44 | uint8_t IsManualControlled(void) |
|
| 35 | GPS_Pos_t HomePosition = {0,0, INVALID}; |
45 | { |
| 36 | 46 | if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0; |
|
| 37 | 47 | else return 1; |
|
| 38 | // --------------------------------------------------------------------------------- |
48 | } |
| 39 | 49 | ||
| 40 | // set home position to current positon |
50 | // set home position to current positon |
| 41 | void GPS_SetHomePosition(void) |
51 | void GPS_SetHomePosition(void) |
| - | 52 | { |
|
| 42 | { |
53 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
| 43 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
54 | { |
| 44 | { |
55 | HomePosition.Longitude = GPSInfo.longitude; |
| 45 | HomePosition.Longitude = GPSInfo.longitude; |
56 | HomePosition.Latitude = GPSInfo.latitude; |
| 46 | HomePosition.Latitude = GPSInfo.latitude; |
57 | HomePosition.Altitude = GPSInfo.altitude; |
| Line 58... | Line 69... | ||
| 58 | { |
69 | { |
| 59 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
70 | if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D) |
| 60 | { |
71 | { |
| 61 | HoldPosition.Longitude = GPSInfo.longitude; |
72 | HoldPosition.Longitude = GPSInfo.longitude; |
| 62 | HoldPosition.Latitude = GPSInfo.latitude; |
73 | HoldPosition.Latitude = GPSInfo.latitude; |
| - | 74 | HoldPosition.Altitude = GPSInfo.altitude; |
|
| 63 | HoldPosition.Status = VALID; |
75 | HoldPosition.Status = VALID; |
| 64 | } |
76 | } |
| 65 | else |
77 | else |
| 66 | { |
78 | { |
| 67 | HoldPosition.Status = INVALID; |
79 | HoldPosition.Status = INVALID; |
| Line 229... | Line 241... | ||
| 229 | GPS_Neutral(); |
241 | GPS_Neutral(); |
| 230 | break; // eof TSK_IDLE |
242 | break; // eof TSK_IDLE |
| 231 | case TSK_HOLD: |
243 | case TSK_HOLD: |
| 232 | if(HoldPosition.Status != INVALID) |
244 | if(HoldPosition.Status != INVALID) |
| 233 | { |
245 | { |
| 234 | // if sticks are centered (no manual control) |
246 | if( IsManualControlled() ) // MK controlled by user |
| - | 247 | { |
|
| 235 | if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
248 | // update hold point to current gps position |
| - | 249 | GPS_SetHoldPosition(); |
|
| - | 250 | // disable gps control |
|
| - | 251 | GPS_Neutral(); |
|
| - | 252 | GPS_P_Delay = 0; |
|
| - | 253 | } |
|
| - | 254 | else // GPS control active |
|
| 236 | { |
255 | { |
| 237 | if(GPS_P_Delay<7) |
256 | if(GPS_P_Delay<7) |
| 238 | { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
257 | { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| 239 | GPS_P_Delay++; |
258 | GPS_P_Delay++; |
| 240 | GPS_SetHoldPosition(); // update hold point to current gps position |
259 | GPS_SetHoldPosition(); // update hold point to current gps position |
| 241 | GPS_PDController(NULL); // activates only the D-Part |
260 | GPS_PDController(NULL); // activates only the D-Part |
| 242 | } |
261 | } |
| 243 | else GPS_PDController(&HoldPosition);// activates the P&D-Part |
262 | else GPS_PDController(&HoldPosition);// activates the P&D-Part |
| 244 | } |
263 | } |
| 245 | else // MK controlled by user |
- | |
| 246 | { |
- | |
| 247 | // update hold point to current gps position |
- | |
| 248 | GPS_SetHoldPosition(); |
- | |
| 249 | // disable gps control |
- | |
| 250 | GPS_Neutral(); |
- | |
| 251 | GPS_P_Delay = 0; |
- | |
| 252 | } |
- | |
| 253 | } |
264 | } |
| 254 | else // invalid Hold Position |
265 | else // invalid Hold Position |
| 255 | { // try to catch a valid hold position from gps data input |
266 | { // try to catch a valid hold position from gps data input |
| 256 | GPS_SetHoldPosition(); |
267 | GPS_SetHoldPosition(); |
| 257 | GPS_Neutral(); |
268 | GPS_Neutral(); |
| Line 261... | Line 272... | ||
| 261 | if(HomePosition.Status != INVALID) |
272 | if(HomePosition.Status != INVALID) |
| 262 | { |
273 | { |
| 263 | // update hold point to current gps position |
274 | // update hold point to current gps position |
| 264 | // to avoid a flight back if home comming is deactivated |
275 | // to avoid a flight back if home comming is deactivated |
| 265 | GPS_SetHoldPosition(); |
276 | GPS_SetHoldPosition(); |
| 266 | // if sticks are centered (no manual control) |
277 | if( IsManualControlled() ) // MK controlled by user |
| 267 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
- | |
| 268 | { |
278 | { |
| 269 | GPS_PDController(&HomePosition); |
279 | GPS_Neutral(); |
| 270 | } |
280 | } |
| 271 | else // manual control |
281 | else // GPS control active |
| 272 | { |
282 | { |
| 273 | GPS_Neutral(); |
283 | GPS_PDController(&HomePosition); |
| 274 | } |
284 | } |
| 275 | } |
285 | } |
| 276 | else // bad home position |
286 | else // bad home position |
| 277 | { |
287 | { |
| 278 | BeepTime = 50; // signal invalid home position |
288 | BeepTime = 50; // signal invalid home position |
| 279 | // try to hold at least the position as a fallback option |
289 | // try to hold at least the position as a fallback option |
| Line 280... | Line 290... | ||
| 280 | 290 | ||
| 281 | if (HoldPosition.Status != INVALID) |
291 | if (HoldPosition.Status != INVALID) |
| 282 | { |
292 | { |
| 283 | // if sticks are centered (no manual control) |
- | |
| 284 | if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
293 | if( IsManualControlled() ) // MK controlled by user |
| 285 | { |
294 | { |
| 286 | GPS_PDController(&HoldPosition); |
295 | GPS_Neutral(); |
| 287 | } |
296 | } |
| 288 | else // manual control or no reference position |
297 | else // GPS control active |
| 289 | { |
298 | { |
| 290 | GPS_Neutral(); |
299 | GPS_PDController(&HoldPosition); |
| 291 | } |
300 | } |
| 292 | } |
301 | } |
| 293 | else |
302 | else |
| 294 | { // try to catch a valid hold position |
303 | { // try to catch a valid hold position |