| 18,7 → 18,7 |
|---|
| |
| #define GPS_STICK_LIMIT 200 // 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 3200 |
| #define GPS_P_LIMIT 100 |
| |
| |
| typedef struct |
| 209,12 → 209,12 |
|---|
| //Calculate PID-components of the controller (negative sign for compensation) |
| |
| // P-Part |
| P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/16; |
| P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/16; |
| P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
| P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
| |
| // I-Part |
| I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/64; |
| I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/64; |
| I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
| I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
| |
| // combine P- & I-Part |
| PID_North = P_North + I_North; |
| 234,8 → 234,8 |
|---|
| } |
| |
| // D-Part |
| D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/4; |
| D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/4; |
| 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; |
| 242,8 → 242,8 |
|---|
| PID_East += D_East; |
| |
| // scale combination with gain. |
| PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / (100 * 32); |
| PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / (100 * 32); |
| PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / 100; |
| PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / 100; |
| |
| // GPS to nick and roll settings |
| |