Rev 813 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 813 | Rev 818 | ||
|---|---|---|---|
| Line 159... | Line 159... | ||
| 159 | } |
159 | } |
| Line 160... | Line 160... | ||
| 160 | 160 | ||
| 161 | //Calculate PID-components of the controller (negative sign for compensation) |
161 | //Calculate PID-components of the controller (negative sign for compensation) |
| 162 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
162 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| 163 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
163 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| 164 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/2048; |
164 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
| 165 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/2048; |
165 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
| 166 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
166 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| 167 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
167 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| 168 | // PD-controller |
168 | // PD-controller |
| 169 | PID_North = P_North + I_North + D_North; |
169 | PID_North = P_North + I_North + D_North; |
| Line 211... | Line 211... | ||
| 211 | } |
211 | } |
| Line 212... | Line 212... | ||
| 212 | 212 | ||
| 213 | if(!GPS_Stick_Limited) // prevent further growing of error integrals if control limit is reached |
213 | if(!GPS_Stick_Limited) // prevent further growing of error integrals if control limit is reached |
| 214 | { |
214 | { |
| 215 | // calculate position error integrals |
215 | // calculate position error integrals |
| 216 | GPSPosDevIntegral_North += GPSPosDev_North/4; |
216 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
| 217 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
217 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
| 218 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
218 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
| 219 | GPSPosDevIntegral_East += GPSPosDev_East/4; |
219 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
| 220 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
220 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
| 221 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
221 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
| Line 222... | Line 222... | ||
| 222 | } |
222 | } |
| 223 | 223 | ||
| 224 | } |
224 | } |
| 225 | else // invalid GPS data |
225 | else // invalid GPS data or bad compass reading |
| 226 | { |
226 | { |
| 227 | GPS_Neutral(); // do nothing |
227 | GPS_Neutral(); // do nothing |
| 228 | // reset error integral |
228 | // reset error integral |