| 407,7 → 407,7 |
|---|
| { |
| // update home deviation info |
| GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation); |
| |
| |
| // if the MK is starting or the home position is invalid then store the home position |
| if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID)) |
| { // try to update the home position from the current position |
| 459,7 → 459,7 |
|---|
| case GPS_FLIGHT_MODE_WAYPOINT: |
| NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
| NaviData.NCFlags |= NC_FLAG_CH; |
| |
| |
| if(GPS_IsManuallyControlled()) // the human pilot takes the action |
| { |
| GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
| 486,7 → 486,7 |
|---|
| // reset the arrived bit to break a pending HoldTime of the old WP |
| WPArrived = FALSE; |
| } |
| |
| |
| if(CurrentTargetDeviation.Status != INVALID) |
| { // if the waypoint was not catched and the target area has been reached |
| if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100))) |
| 507,7 → 507,7 |
|---|
| } // EOF if(WPArrived) |
| } |
| } // EOF waypoint trigger logic |
| |
| |
| if(GPS_pWaypoint != NULL) // Waypoint exist |
| { |
| // update the hold position |
| 642,13 → 642,13 |
|---|
| NaviData.HomePositionDeviation.Bearing = CurrentHomeDeviation.Bearing; |
| NaviData.UBat = FC.UBat; |
| NaviData.GroundSpeed = (u16)GPSData.Speed_Ground; |
| NaviData.Heading = (s16)GPSData.Heading; |
| NaviData.Heading = (s16)(GPSData.Heading/100000L); |
| NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg |
| NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg |
| NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg |
| NaviData.RC_Quality = (u8) FC.RC_Quality; |
| NaviData.MKFlags = (u8)FC.MKFlags; |
| NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
| NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
| |
| //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
| return; |