Rev 92 | Rev 95 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 92 | Rev 93 | ||
|---|---|---|---|
| Line 487... | Line 487... | ||
| 487 | { |
487 | { |
| 488 | // check if the pointer to the waypoint has been changed or the data have been updated |
488 | // check if the pointer to the waypoint has been changed or the data have been updated |
| 489 | if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA)) |
489 | if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA)) |
| 490 | { |
490 | { |
| 491 | GPS_pWaypointOld = GPS_pWaypoint; |
491 | GPS_pWaypointOld = GPS_pWaypoint; |
| 492 | } |
492 | } |
| 493 | // if WP has been reached once, wait hold time before trigger to next one |
493 | // if WP has been reached once, wait hold time before trigger to next one |
| 494 | if(NCFlags & NC_FLAG_TARGET_REACHED) |
494 | if(NCFlags & NC_FLAG_TARGET_REACHED) |
| 495 | { |
495 | { |
| 496 | /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */ |
496 | /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */ |
| 497 | if(CheckDelay(WPTime)) |
497 | if(CheckDelay(WPTime)) |
| Line 509... | Line 509... | ||
| 509 | if(GPS_pWaypoint != NULL) // Waypoint exist |
509 | if(GPS_pWaypoint != NULL) // Waypoint exist |
| 510 | { |
510 | { |
| 511 | // update the hold position |
511 | // update the hold position |
| 512 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
512 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
| 513 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
513 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
| 514 | GPS_TargetRadius = (u32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
514 | GPS_TargetRadius = (u32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
| Line 515... | Line 515... | ||
| 515 | 515 | ||
| 516 | } |
516 | } |
| 517 | else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached |
517 | else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached |
| 518 | { |
518 | { |
| Line 602... | Line 602... | ||
| 602 | /* Calculate position deviation from ranged target */ |
602 | /* Calculate position deviation from ranged target */ |
| Line 603... | Line 603... | ||
| 603 | 603 | ||
| 604 | // calculate deviation of current position to ranged target position in cm |
604 | // calculate deviation of current position to ranged target position in cm |
| 605 | if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition, &CurrentTargetDeviation)) |
605 | if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition, &CurrentTargetDeviation)) |
| 606 | { // set target reached flag of we once reached the target point |
606 | { // set target reached flag of we once reached the target point |
| 607 | if(!(NCFlags |= NC_FLAG_TARGET_REACHED) && (CurrentTargetDeviation.Distance < GPS_TargetRadius)) |
607 | if(!(NCFlags & NC_FLAG_TARGET_REACHED) && (CurrentTargetDeviation.Distance < GPS_TargetRadius)) |
| 608 | { |
608 | { |
| 609 | NCFlags |= NC_FLAG_TARGET_REACHED; // set target reached flag |
609 | NCFlags |= NC_FLAG_TARGET_REACHED; // set target reached flag |
| 610 | } |
610 | } |
| 611 | // implement your control code here based |
611 | // implement your control code here based |
| 612 | // in the info available in the CurrentTargetDeviation, GPSData and FromFlightCtrl.GyroHeading |
612 | // in the info available in the CurrentTargetDeviation, GPSData and FromFlightCtrl.GyroHeading |
| 613 | GPS_Stick.Nick = 0; |
613 | GPS_Stick.Nick = 0; |