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 |