Rev 151 | Rev 159 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 151 | Rev 152 | ||
|---|---|---|---|
| Line 302... | Line 302... | ||
| 302 | GPS_Neutral(); |
302 | GPS_Neutral(); |
| 303 | GPS_ClearPosition(&GPS_HoldPosition); |
303 | GPS_ClearPosition(&GPS_HoldPosition); |
| 304 | GPS_ClearPosition(&GPS_HomePosition); |
304 | GPS_ClearPosition(&GPS_HomePosition); |
| 305 | GPS_pTargetPosition = NULL; |
305 | GPS_pTargetPosition = NULL; |
| 306 | WPList_Init(); |
306 | WPList_Init(); |
| 307 | GPS_pWaypoint = WPList_Begin(); |
307 | GPS_pWaypoint = NULL; |
| 308 | GPS_UpdateParameter(); |
308 | GPS_UpdateParameter(); |
| 309 | UART1_PutString("ok"); |
309 | UART1_PutString("ok"); |
| 310 | } |
310 | } |
| Line 311... | Line 311... | ||
| 311 | 311 | ||
| Line 411... | Line 411... | ||
| 411 | static u32 GPSDataTimeout = 0; |
411 | static u32 GPSDataTimeout = 0; |
| 412 | float compassheading, sin_h, cos_h; |
412 | float compassheading, sin_h, cos_h; |
| Line 413... | Line 413... | ||
| 413 | 413 | ||
| 414 | // pointer to current target position |
414 | // pointer to current target position |
| 415 | static GPS_Pos_t * pTargetPositionOld = NULL; |
- | |
| Line 416... | Line 415... | ||
| 416 | static Waypoint_t* GPS_pWaypointOld = NULL; |
415 | static GPS_Pos_t * pTargetPositionOld = NULL; |
| 417 | 416 | ||
| 418 | static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID}; // the limited target position, this is derived from the target position with repect to the operating radius |
417 | static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID}; // the limited target position, this is derived from the target position with repect to the operating radius |
| Line 472... | Line 471... | ||
| 472 | { |
471 | { |
| 473 | BeepTime = 700; // beep on success |
472 | BeepTime = 700; // beep on success |
| 474 | GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition)); |
473 | GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition)); |
| 475 | } |
474 | } |
| 476 | GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
475 | GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
| - | 476 | if(GPS_pWaypoint != NULL) // if new WP exist |
|
| - | 477 | { // update WP hold time stamp immediately! |
|
| - | 478 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
|
| - | 479 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
| - | 480 | } |
|
| 477 | } |
481 | } |
| Line 478... | Line 482... | ||
| 478 | 482 | ||
| Line 479... | Line 483... | ||
| 479 | /* The selected flight mode influences the target position pointer and therefore the behavior */ |
483 | /* The selected flight mode influences the target position pointer and therefore the behavior */ |
| Line 538... | Line 542... | ||
| 538 | if(GPS_pWaypoint != NULL) // pointer to waypoint exist |
542 | if(GPS_pWaypoint != NULL) // pointer to waypoint exist |
| 539 | { |
543 | { |
| 540 | if(GPS_pWaypoint->Position.Status == INVALID) // should never happen |
544 | if(GPS_pWaypoint->Position.Status == INVALID) // should never happen |
| 541 | { |
545 | { |
| 542 | GPS_pWaypoint = WPList_Next(); // goto to next WP |
546 | GPS_pWaypoint = WPList_Next(); // goto to next WP |
| - | 547 | if(GPS_pWaypoint != NULL) // if new WP exist |
|
| - | 548 | { // update WP hold time stamp immediately! |
|
| - | 549 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
|
| - | 550 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
| - | 551 | } |
|
| 543 | BeepTime = 255; |
552 | BeepTime = 255; |
| 544 | } |
553 | } |
| 545 | else // waypoint position is valid |
554 | else // waypoint position is valid |
| 546 | { |
555 | { |
| 547 | // check if the pointer to the waypoint has been changed or the data have been updated |
- | |
| 548 | if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA)) |
- | |
| 549 | { |
- | |
| 550 | GPS_pWaypointOld = GPS_pWaypoint; |
- | |
| 551 | } |
- | |
| 552 | // if WP has been reached once, wait hold time before trigger to next one |
556 | // if WP has been reached once, wait hold time before trigger to next one |
| 553 | if(NCFlags & NC_FLAG_TARGET_REACHED) |
557 | if(NCFlags & NC_FLAG_TARGET_REACHED) |
| 554 | { |
558 | { |
| 555 | /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */ |
559 | /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */ |
| 556 | if(CheckDelay(WPTime)) |
560 | if(CheckDelay(WPTime)) |
| Line 558... | Line 562... | ||
| 558 | GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached |
562 | GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached |
| 559 | if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist |
563 | if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist |
| 560 | if(GPS_pWaypoint != NULL) // if new WP exist |
564 | if(GPS_pWaypoint != NULL) // if new WP exist |
| 561 | { // update WP hold time stamp immediately! |
565 | { // update WP hold time stamp immediately! |
| 562 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
566 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
| - | 567 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
| 563 | } |
568 | } |
| 564 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
- | |
| 565 | } |
569 | } |
| 566 | } // EOF if(WPArrived) |
570 | } // EOF target reached |
| 567 | else |
571 | else |
| 568 | { |
572 | { |
| 569 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
573 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
| 570 | } |
574 | } |
| 571 | } |
575 | } |
| 572 | } |
576 | } |
| 573 | else // pointer to waypoint does not exist |
577 | else // pointer to waypoint does not exist |
| 574 | { |
578 | { |
| 575 | // try to catch the first waypoint from the list |
579 | // try to catch the first waypoint from the list |
| 576 | GPS_pWaypoint = WPList_Begin(); |
580 | GPS_pWaypoint = WPList_Begin(); |
| - | 581 | if(GPS_pWaypoint != NULL) // if new WP exist |
|
| - | 582 | { // update WP hold time stamp immediately! |
|
| - | 583 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
|
| - | 584 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
| - | 585 | } |
|
| 577 | } |
586 | } |
| 578 | // EOF waypoint trigger logic |
587 | // EOF waypoint trigger logic |
| Line 579... | Line 588... | ||
| 579 | 588 | ||
| 580 | if(GPS_pWaypoint != NULL) // Waypoint exist |
589 | if(GPS_pWaypoint != NULL) // Waypoint exist |