Rev 938 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 938 | Rev 939 | ||
|---|---|---|---|
| Line 14... | Line 14... | ||
| 14 | GPS_FLIGHT_MODE_FREE, |
14 | GPS_FLIGHT_MODE_FREE, |
| 15 | GPS_FLIGHT_MODE_AID, |
15 | GPS_FLIGHT_MODE_AID, |
| 16 | GPS_FLIGHT_MODE_HOME, |
16 | GPS_FLIGHT_MODE_HOME, |
| 17 | } FlightMode_t; |
17 | } FlightMode_t; |
| Line -... | Line 18... | ||
| - | 18 | ||
| 18 | 19 | #define GPS_POSINTEGRAL_LIMIT 3000 |
|
| 19 | #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
20 | #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
| - | 21 | #define GPS_I_LIMIT 156 // limit for the position error integral |
|
| 20 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
22 | #define GPS_D_LIMIT 625 |
| Line 21... | Line 23... | ||
| 21 | #define GPS_P_LIMIT 250 |
23 | #define GPS_P_LIMIT 312 |
| 22 | 24 | ||
| 23 | 25 | ||
| Line 204... | Line 206... | ||
| 204 | // reset error integral |
206 | // reset error integral |
| 205 | GPSPosDevIntegral_North = 0; |
207 | GPSPosDevIntegral_North = 0; |
| 206 | GPSPosDevIntegral_East = 0; |
208 | GPSPosDevIntegral_East = 0; |
| 207 | } |
209 | } |
| Line -... | Line 210... | ||
| - | 210 | ||
| - | 211 | GPSPosDevIntegral_North += GPSPosDev_North; |
|
| - | 212 | GPSPosDevIntegral_East += GPSPosDev_East; |
|
| - | 213 | GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
|
| 208 | 214 | ||
| Line -... | Line 215... | ||
| - | 215 | //Calculate PID-components of the controller |
|
| - | 216 | ||
| - | 217 | // D-Part |
|
| - | 218 | D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/256; |
|
| - | 219 | D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/256; |
|
| 209 | //Calculate PID-components of the controller |
220 | GPS_LimitXY(&D_North, &D_East, GPS_D_LIMIT); |
| 210 | 221 | ||
| 211 | // P-Part |
222 | // P-Part |
| - | 223 | P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/1024; |
|
| Line 212... | Line 224... | ||
| 212 | P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
224 | P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/1024; |
| 213 | P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
225 | GPS_LimitXY(&P_North, &P_East, GPS_P_LIMIT); |
| 214 | 226 | ||
| 215 | // I-Part |
- | |
| 216 | I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
- | |
| 217 | I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
- | |
| 218 | - | ||
| 219 | // combine P- & I-Part |
- | |
| 220 | PID_North = P_North + I_North; |
- | |
| 221 | PID_East = P_East + I_East; |
227 | // I-Part |
| 222 | - | ||
| 223 | //limit PI-Part |
- | |
| 224 | if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
- | |
| 225 | { |
- | |
| 226 | // P-Part limit is not reached |
- | |
| 227 | // update position error integrals |
- | |
| 228 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
- | |
| 229 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 230 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 231 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
- | |
| 232 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 233 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 234 | } |
- | |
| 235 | - | ||
| 236 | // D-Part |
228 | I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| 237 | D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
229 | I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| 238 | D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
230 | GPS_LimitXY(&I_North, &I_East, GPS_I_LIMIT); |
| 239 | 231 | ||
| Line 240... | Line 232... | ||
| 240 | // combine PI- and D-Part |
232 | // combine P & I & D-Part |
| 241 | PID_North += D_North; |
233 | PID_North = P_North + I_North + D_North; |
| 242 | PID_East += D_East; |
234 | PID_East = P_East + I_East + D_East; |
| Line 404... | Line 396... | ||
| 404 | } // eof switch GPS_Task |
396 | } // eof switch GPS_Task |
| 405 | } // eof gps data quality is good |
397 | } // eof gps data quality is good |
| 406 | else // gps data quality is bad |
398 | else // gps data quality is bad |
| 407 | { // disable gps control |
399 | { // disable gps control |
| 408 | GPS_Neutral(); |
400 | GPS_Neutral(); |
| - | 401 | if(FlightMode != GPS_FLIGHT_MODE_FREE) |
|
| - | 402 | { |
|
| 409 | // beep if signal is not sufficient |
403 | // beep if signal is not sufficient |
| 410 | if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
404 | if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| 411 | else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
405 | else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| - | 406 | } |
|
| 412 | } |
407 | } |
| 413 | // set current data as processed to avoid further calculations on the same gps data |
408 | // set current data as processed to avoid further calculations on the same gps data |
| 414 | GPSInfo.status = PROCESSED; |
409 | GPSInfo.status = PROCESSED; |
| 415 | break; |
410 | break; |
| 416 | } // eof GPSInfo.status |
411 | } // eof GPSInfo.status |