| 15,7 → 15,7 |
|---|
| #define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
| #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
| #define MAX_VELOCITY 250 // max ground speed in cm/s during position control |
| #define MAX_VELOCITY 300 // max ground speed in cm/s during position control |
| |
| |
| int16_t GPS_Pitch = 0, GPS_Roll = 0; |
| 165,21 → 165,26 |
|---|
| P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
| |
| //limit P-part to limit the max velocity |
| temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the P-Part limit |
| temp = (int32_t)c_sqrt(P_North*P_North + P_East*P_East); // the current P-Part |
| // I-Part |
| I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
| I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
| |
| // combine P- & I-Part |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| |
| //limit PI-Part to limit the max velocity |
| temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit |
| temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part |
| if(temp > temp1) // P-Part limit is reached |
| { |
| // normalize P-part components to the P-Part limit |
| P_North = (P_North * temp1)/temp; |
| P_East = (P_East * temp1)/temp; |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| PID_North = (PID_North * temp1)/temp; |
| PID_East = (PID_East * temp1)/temp; |
| } |
| else // P-Part under its limit |
| else // PI-Part under its limit |
| { |
| // calculate position error integrals |
| // 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; |
| 188,18 → 193,14 |
|---|
| else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
| } |
| |
| // I-Part |
| I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
| I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
| |
| // D-Part |
| // D-Part |
| D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| |
| |
| // combine P- I- D-Part |
| PID_North = P_North + I_North + D_North; |
| PID_East = P_East + I_East + D_East; |
| // combine PI- and D-Part |
| PID_North += D_North; |
| PID_East += D_East; |
| |
| // GPS to pitch and roll settings |
| |