Rev 73 | Rev 89 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 73 | Rev 74 | ||
---|---|---|---|
Line 405... | Line 405... | ||
405 | // If GPS signal condition is sufficient for a reliable position measurement |
405 | // If GPS signal condition is sufficient for a reliable position measurement |
406 | if(GPS_IsSignalOK()) |
406 | if(GPS_IsSignalOK()) |
407 | { |
407 | { |
408 | // update home deviation info |
408 | // update home deviation info |
409 | GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation); |
409 | GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation); |
410 | 410 | ||
411 | // if the MK is starting or the home position is invalid then store the home position |
411 | // if the MK is starting or the home position is invalid then store the home position |
412 | if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID)) |
412 | if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID)) |
413 | { // try to update the home position from the current position |
413 | { // try to update the home position from the current position |
414 | if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition)) |
414 | if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition)) |
415 | { |
415 | { |
Line 457... | Line 457... | ||
457 | // the GPS control is directed to a target position |
457 | // the GPS control is directed to a target position |
458 | // given by a waypoint or by the home position |
458 | // given by a waypoint or by the home position |
459 | case GPS_FLIGHT_MODE_WAYPOINT: |
459 | case GPS_FLIGHT_MODE_WAYPOINT: |
460 | NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
460 | NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
461 | NaviData.NCFlags |= NC_FLAG_CH; |
461 | NaviData.NCFlags |= NC_FLAG_CH; |
462 | 462 | ||
463 | if(GPS_IsManuallyControlled()) // the human pilot takes the action |
463 | if(GPS_IsManuallyControlled()) // the human pilot takes the action |
464 | { |
464 | { |
465 | GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
465 | GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
466 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); // update hold position |
466 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); // update hold position |
467 | GPS_pTargetPosition = NULL; // set target position invalid |
467 | GPS_pTargetPosition = NULL; // set target position invalid |
Line 484... | Line 484... | ||
484 | { |
484 | { |
485 | GPS_pWaypointOld = GPS_pWaypoint; |
485 | GPS_pWaypointOld = GPS_pWaypoint; |
486 | // reset the arrived bit to break a pending HoldTime of the old WP |
486 | // reset the arrived bit to break a pending HoldTime of the old WP |
487 | WPArrived = FALSE; |
487 | WPArrived = FALSE; |
488 | } |
488 | } |
489 | 489 | ||
490 | if(CurrentTargetDeviation.Status != INVALID) |
490 | if(CurrentTargetDeviation.Status != INVALID) |
491 | { // if the waypoint was not catched and the target area has been reached |
491 | { // if the waypoint was not catched and the target area has been reached |
492 | if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100))) |
492 | if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100))) |
493 | { |
493 | { |
494 | WPArrived = TRUE; |
494 | WPArrived = TRUE; |
Line 505... | Line 505... | ||
505 | WPArrived = FALSE; // which is not arrived |
505 | WPArrived = FALSE; // which is not arrived |
506 | } |
506 | } |
507 | } // EOF if(WPArrived) |
507 | } // EOF if(WPArrived) |
508 | } |
508 | } |
509 | } // EOF waypoint trigger logic |
509 | } // EOF waypoint trigger logic |
510 | 510 | ||
511 | if(GPS_pWaypoint != NULL) // Waypoint exist |
511 | if(GPS_pWaypoint != NULL) // Waypoint exist |
512 | { |
512 | { |
513 | // update the hold position |
513 | // update the hold position |
514 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
514 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
515 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
515 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
Line 640... | Line 640... | ||
640 | NaviData.TargetPositionDeviation.Bearing = CurrentTargetDeviation.Bearing; |
640 | NaviData.TargetPositionDeviation.Bearing = CurrentTargetDeviation.Bearing; |
641 | NaviData.HomePositionDeviation.Distance = CurrentHomeDeviation.Distance; |
641 | NaviData.HomePositionDeviation.Distance = CurrentHomeDeviation.Distance; |
642 | NaviData.HomePositionDeviation.Bearing = CurrentHomeDeviation.Bearing; |
642 | NaviData.HomePositionDeviation.Bearing = CurrentHomeDeviation.Bearing; |
643 | NaviData.UBat = FC.UBat; |
643 | NaviData.UBat = FC.UBat; |
644 | NaviData.GroundSpeed = (u16)GPSData.Speed_Ground; |
644 | NaviData.GroundSpeed = (u16)GPSData.Speed_Ground; |
645 | NaviData.Heading = (s16)GPSData.Heading; |
645 | NaviData.Heading = (s16)(GPSData.Heading/100000L); |
646 | NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg |
646 | NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg |
647 | NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg |
647 | NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg |
648 | NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg |
648 | NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg |
649 | NaviData.RC_Quality = (u8) FC.RC_Quality; |
649 | NaviData.RC_Quality = (u8) FC.RC_Quality; |
650 | NaviData.MKFlags = (u8)FC.MKFlags; |
650 | NaviData.MKFlags = (u8)FC.MKFlags; |
651 | NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
651 | NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
Line 652... | Line 652... | ||
652 | 652 | ||
653 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
653 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
654 | return; |
654 | return; |