168,9 → 168,9 |
{ |
if(Parameter.NaviGpsModeControl < 50) |
{ |
if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY); |
if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY); |
if(CheckDelay(SwitchDelay)) |
{ |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
NCFlags |= NC_FLAG_FREE; |
475,7 → 475,7 |
GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
if(GPS_pWaypoint != NULL) // if new WP exist |
{ // update WP hold time stamp immediately! |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
} |
508,7 → 508,7 |
} |
else |
{ |
/* |
/* |
#define PH_MOVE_THRESHOLD 8 |
if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
{ // indirect control by moving the hold position with rc sticks |
519,7 → 519,7 |
GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
}*/ |
// set target position |
// set target position |
GPS_pTargetPosition = &GPS_HoldPosition; |
GPS_TargetRadius = 100; // 1 meter |
} |
546,7 → 546,7 |
GPS_pWaypoint = WPList_Next(); // goto to next WP |
if(GPS_pWaypoint != NULL) // if new WP exist |
{ // update WP hold time stamp immediately! |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
BeepTime = 255; |
563,7 → 563,7 |
if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist |
if(GPS_pWaypoint != NULL) // if new WP exist |
{ // update WP hold time stamp immediately! |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
} |
573,7 → 573,7 |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
} |
} |
} |
} |
else // pointer to waypoint does not exist |
{ |
// try to catch the first waypoint from the list |
580,17 → 580,25 |
GPS_pWaypoint = WPList_Begin(); |
if(GPS_pWaypoint != NULL) // if new WP exist |
{ // update WP hold time stamp immediately! |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
} |
} |
// EOF waypoint trigger logic |
|
if(GPS_pWaypoint != NULL) // Waypoint exist |
{ |
// possible new data have been put into wp-list |
if(GPS_pWaypoint->Position.Status == NEWDATA) |
{ |
WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
// update the hold position |
GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
// set target to the waypoint |
GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
// update target radius |
GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
|
} |