Rev 938 | Rev 942 | Go to most recent revision | Show entire file | Regard 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; |
412 | } |
406 | } |
- | 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 |
417 | } |
412 | } |