134,7 → 134,10 |
// Update GPSParamter |
void GPS_UpdateParameter(void) |
{ |
#define SWITCH_DELAY 500 |
static u32 SwitchDelay = 0; |
static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF; |
|
// in case of bad receiving conditions |
if(FC.RC_Quality < 100) |
{ // set fixed parameter |
165,21 → 168,33 |
{ |
if(Parameter.NaviGpsModeControl < 50) |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
NCFlags |= NC_FLAG_FREE; |
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; |
} |
} |
else if(Parameter.NaviGpsModeControl < 180) |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID; |
NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH); |
NCFlags |= NC_FLAG_PH; |
if(FlightMode_Old == GPS_FLIGHT_MODE_AID) SetDelay(SWITCH_DELAY); |
if(CheckDelay(SwitchDelay)) |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID; |
NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH); |
NCFlags |= NC_FLAG_PH; |
} |
} |
else |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
NCFlags |= NC_FLAG_CH; |
if(FlightMode_Old == GPS_FLIGHT_MODE_WAYPOINT) SetDelay(SWITCH_DELAY); |
if(CheckDelay(SwitchDelay)) |
{ |
GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
NCFlags |= NC_FLAG_CH; |
} |
} |
} |
GPS_Parameter.Gain = (float)Parameter.NaviGpsGain; |
202,7 → 217,7 |
// FlightMode changed? |
if(GPS_Parameter.FlightMode != FlightMode_Old) |
{ |
BeepTime = 100; // beep to indicate that mode has been switched |
BeepTime = 200; // beep to indicate that mode has been switched |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
// if the mode has changed to free |
if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free |
489,6 → 504,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 |
498,7 → 514,8 |
// of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
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 |
GPS_pTargetPosition = &GPS_HoldPosition; |
GPS_TargetRadius = 100; // 1 meter |
} |