Rev 151 | Rev 159 | Go to most recent revision | Show entire file | Regard 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 |
563 | } |
- | |
564 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
567 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
565 | } |
568 | } |
- | 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 |