| 161,8 → 161,8 |
|---|
| //Calculate PID-components of the controller (negative sign for compensation) |
| P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/2048; |
| I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/2048; |
| I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
| I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
| D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| // PD-controller |
| 213,16 → 213,16 |
|---|
| if(!GPS_Stick_Limited) // prevent further growing of error integrals if control limit is reached |
| { |
| // calculate position error integrals |
| GPSPosDevIntegral_North += GPSPosDev_North/4; |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
| else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
| GPSPosDevIntegral_East += GPSPosDev_East/4; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
| else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
| } |
| |
| } |
| else // invalid GPS data |
| else // invalid GPS data or bad compass reading |
| { |
| GPS_Neutral(); // do nothing |
| // reset error integral |