| 16,11 → 16,9 |
|---|
| 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_I_LIMIT 156 // limit for the position error integral |
| #define GPS_D_LIMIT 625 |
| #define GPS_P_LIMIT 312 |
| #define GPS_POSINTEGRAL_LIMIT 32000 |
| #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_P_LIMIT 25 |
| |
| |
| typedef struct |
| 208,31 → 206,36 |
|---|
| 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); |
| D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/512; |
| D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/512; |
| |
| // P-Part |
| 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); |
| P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/2048; |
| P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/2048; |
| |
| // I-Part |
| 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 & D-Part |
| PID_North = P_North + I_North + D_North; |
| PID_East = P_East + I_East + D_East; |
| |
| // combine P & I |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_North, GPS_POSINTEGRAL_LIMIT); |
| } |
| |
| // 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; |
| 253,8 → 256,8 |
|---|
| |
| coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| PID_Nick = (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2; |
| PID_Roll = (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2; |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| |
| |
| // limit resulting GPS control vector |