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