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