Rev 818 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 818 | Rev 828 | ||
|---|---|---|---|
| Line 94... | Line 94... | ||
| 94 | // calculates the GPS control stick values from the deviation to target position |
94 | // calculates the GPS control stick values from the deviation to target position |
| 95 | // if the pointer to the target positin is NULL or is the target position invalid |
95 | // if the pointer to the target positin is NULL or is the target position invalid |
| 96 | // then the P part of the controller is deactivated. |
96 | // then the P part of the controller is deactivated. |
| 97 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
97 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
| 98 | { |
98 | { |
| - | 99 | int32_t PID_Stick, PID_Pitch, PID_Roll; |
|
| 99 | int32_t coscompass, sincompass; |
100 | int32_t coscompass, sincompass; |
| 100 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
101 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 101 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
102 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
| 102 | int32_t PID_North = 0, PID_East = 0; |
103 | int32_t PID_North = 0, PID_East = 0; |
| 103 | uint8_t GPS_Stick_Limited = 0; |
- | |
| 104 | static int32_t cos_target_latitude = 1; |
104 | static int32_t cos_target_latitude = 1; |
| 105 | static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
105 | static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
| 106 | static GPS_Pos_t *pLastTargetPos = 0; |
106 | static GPS_Pos_t *pLastTargetPos = 0; |
| Line 107... | Line 107... | ||
| 107 | 107 | ||
| Line 183... | Line 183... | ||
| 183 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
183 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
| 184 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
184 | // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
| Line 185... | Line 185... | ||
| 185 | 185 | ||
| 186 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
186 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
| 187 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
187 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
| 188 | GPS_Roll = (int16_t)((coscompass * PID_East - sincompass * PID_North) / 8192); |
188 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
| Line 189... | Line 189... | ||
| 189 | GPS_Pitch = -1*(int16_t)((sincompass * PID_East + coscompass * PID_North) / 8192); |
189 | PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
| 190 | 190 | ||
| 191 | // limit GPS controls |
- | |
| 192 | if (GPS_Pitch > GPS_STICK_LIMIT) |
- | |
| 193 | { |
- | |
| 194 | GPS_Pitch = GPS_STICK_LIMIT; |
- | |
| 195 | GPS_Stick_Limited = 1; |
191 | // limit GPS control vector |
| 196 | } |
192 | PID_Stick = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
| 197 | else if (GPS_Pitch < -GPS_STICK_LIMIT) |
193 | if (PID_Stick > GPS_STICK_LIMIT) |
| 198 | { |
- | |
| 199 | GPS_Pitch = -GPS_STICK_LIMIT; |
- | |
| 200 | GPS_Stick_Limited = 1; |
194 | { |
| 201 | } |
- | |
| 202 | if (GPS_Roll > GPS_STICK_LIMIT) |
195 | // normalize control vector components to the limit |
| 203 | { |
- | |
| 204 | GPS_Roll = GPS_STICK_LIMIT; |
196 | PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/PID_Stick; |
| 205 | GPS_Stick_Limited = 1; |
- | |
| 206 | } |
- | |
| 207 | else if (GPS_Roll < -GPS_STICK_LIMIT) |
- | |
| 208 | { |
- | |
| 209 | GPS_Roll = -GPS_STICK_LIMIT; |
- | |
| 210 | GPS_Stick_Limited = 1; |
- | |
| 211 | } |
197 | PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/PID_Stick; |
| 212 | 198 | } |
|
| 213 | if(!GPS_Stick_Limited) // prevent further growing of error integrals if control limit is reached |
199 | else // prevent further growing of error integrals if control limit is reached |
| 214 | { |
200 | { |
| 215 | // calculate position error integrals |
201 | // calculate position error integrals |
| 216 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
202 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
| 217 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
203 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
| 218 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
204 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
| 219 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
205 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
| 220 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
206 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
| Line -... | Line 207... | ||
| - | 207 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
|
| - | 208 | } |
|
| - | 209 | ||
| 221 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
210 | GPS_Roll = (int16_t)PID_Roll; |
| 222 | } |
211 | GPS_Pitch = (int16_t)PID_Pitch; |
| 223 | 212 | ||
| 224 | } |
213 | } |
| 225 | else // invalid GPS data or bad compass reading |
214 | else // invalid GPS data or bad compass reading |