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 |
|