16,9 → 16,11 |
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_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
#define GPS_P_LIMIT 250 |
#define GPS_I_LIMIT 156 // limit for the position error integral |
#define GPS_D_LIMIT 625 |
#define GPS_P_LIMIT 312 |
|
|
typedef struct |
206,41 → 208,31 |
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); |
|
// P-Part |
P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
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); |
|
// I-Part |
I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
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-Part |
PID_North = P_North + I_North; |
PID_East = P_East + I_East; |
// combine P & I & D-Part |
PID_North = P_North + I_North + D_North; |
PID_East = P_East + I_East + D_East; |
|
//limit PI-Part |
if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
{ |
// P-Part limit is not reached |
// 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; |
GPSPosDevIntegral_East += GPSPosDev_East/16; |
if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
} |
|
// D-Part |
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; |
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; |
406,9 → 398,12 |
else // gps data quality is bad |
{ // disable gps control |
GPS_Neutral(); |
// beep if signal is not sufficient |
if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
if(FlightMode != GPS_FLIGHT_MODE_FREE) |
{ |
// beep if signal is not sufficient |
if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
} |
} |
// set current data as processed to avoid further calculations on the same gps data |
GPSInfo.status = PROCESSED; |