| 16,9 → 16,11 |
|---|
| GPS_FLIGHT_MODE_HOME, |
| } FlightMode_t; |
| |
| #define GPS_POSINTEGRAL_LIMIT 3000 |
| #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
| #define GPS_P_LIMIT 250 |
| #define GPS_I_LIMIT 156 // limit for the position error integral |
| #define GPS_D_LIMIT 625 |
| #define GPS_P_LIMIT 312 |
| |
| |
| typedef struct |
| 206,41 → 208,31 |
|---|
| GPSPosDevIntegral_East = 0; |
| } |
| |
| GPSPosDevIntegral_North += GPSPosDev_North; |
| GPSPosDevIntegral_East += GPSPosDev_East; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
| |
| //Calculate PID-components of the controller |
| |
| // D-Part |
| D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/256; |
| D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/256; |
| GPS_LimitXY(&D_North, &D_East, GPS_D_LIMIT); |
| |
| // P-Part |
| P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
| P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
| P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/1024; |
| P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/1024; |
| GPS_LimitXY(&P_North, &P_East, GPS_P_LIMIT); |
| |
| // I-Part |
| I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
| I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
| I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| GPS_LimitXY(&I_North, &I_East, GPS_I_LIMIT); |
| |
| // combine P- & I-Part |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| // combine P & I & D-Part |
| PID_North = P_North + I_North + D_North; |
| PID_East = P_East + I_East + D_East; |
| |
| //limit PI-Part |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| // P-Part limit is not reached |
| // update position error integrals |
| 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/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; |
| } |
| |
| // D-Part |
| D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
| D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
| |
| // combine PI- and D-Part |
| PID_North += D_North; |
| PID_East += D_East; |
| |
| // scale combination with gain. |
| PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / 100; |
| PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / 100; |
| 406,9 → 398,12 |
|---|
| else // gps data quality is bad |
| { // disable gps control |
| GPS_Neutral(); |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| } |
| } |
| // set current data as processed to avoid further calculations on the same gps data |
| GPSInfo.status = PROCESSED; |