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