96,11 → 96,11 |
// then the P part of the controller is deactivated. |
void GPS_PIDController(GPS_Pos_t *pTargetPos) |
{ |
int32_t PID_Stick, PID_Pitch, PID_Roll; |
int32_t coscompass, sincompass; |
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
int32_t PID_North = 0, PID_East = 0; |
uint8_t GPS_Stick_Limited = 0; |
static int32_t cos_target_latitude = 1; |
static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
static GPS_Pos_t *pLastTargetPos = 0; |
185,33 → 185,19 |
|
coscompass = (int32_t)c_cos_8192(CompassHeading); |
sincompass = (int32_t)c_sin_8192(CompassHeading); |
GPS_Roll = (int16_t)((coscompass * PID_East - sincompass * PID_North) / 8192); |
GPS_Pitch = -1*(int16_t)((sincompass * PID_East + coscompass * PID_North) / 8192); |
PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
|
// limit GPS controls |
if (GPS_Pitch > GPS_STICK_LIMIT) |
// limit GPS control vector |
PID_Stick = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
if (PID_Stick > GPS_STICK_LIMIT) |
{ |
GPS_Pitch = GPS_STICK_LIMIT; |
GPS_Stick_Limited = 1; |
// normalize control vector components to the limit |
PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/PID_Stick; |
PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/PID_Stick; |
} |
else if (GPS_Pitch < -GPS_STICK_LIMIT) |
else // prevent further growing of error integrals if control limit is reached |
{ |
GPS_Pitch = -GPS_STICK_LIMIT; |
GPS_Stick_Limited = 1; |
} |
if (GPS_Roll > GPS_STICK_LIMIT) |
{ |
GPS_Roll = GPS_STICK_LIMIT; |
GPS_Stick_Limited = 1; |
} |
else if (GPS_Roll < -GPS_STICK_LIMIT) |
{ |
GPS_Roll = -GPS_STICK_LIMIT; |
GPS_Stick_Limited = 1; |
} |
|
if(!GPS_Stick_Limited) // prevent further growing of error integrals if control limit is reached |
{ |
// calculate position error integrals |
GPSPosDevIntegral_North += GPSPosDev_North/16; |
if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
221,6 → 207,9 |
else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
} |
|
GPS_Roll = (int16_t)PID_Roll; |
GPS_Pitch = (int16_t)PID_Pitch; |
|
} |
else // invalid GPS data or bad compass reading |
{ |