Rev 939 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 939 | Rev 949 | ||
|---|---|---|---|
| Line 14... | Line 14... | ||
| 14 | GPS_FLIGHT_MODE_FREE, |
14 | GPS_FLIGHT_MODE_FREE, |
| 15 | GPS_FLIGHT_MODE_AID, |
15 | GPS_FLIGHT_MODE_AID, |
| 16 | GPS_FLIGHT_MODE_HOME, |
16 | GPS_FLIGHT_MODE_HOME, |
| 17 | } FlightMode_t; |
17 | } FlightMode_t; |
| Line 18... | Line 18... | ||
| 18 | 18 | ||
| 19 | #define GPS_POSINTEGRAL_LIMIT 3000 |
19 | #define GPS_POSINTEGRAL_LIMIT 32000 |
| 20 | #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
- | |
| 21 | #define GPS_I_LIMIT 156 // limit for the position error integral |
20 | #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| 22 | #define GPS_D_LIMIT 625 |
- | |
| Line 23... | Line 21... | ||
| 23 | #define GPS_P_LIMIT 312 |
21 | #define GPS_P_LIMIT 25 |
| 24 | 22 | ||
| 25 | 23 | ||
| Line 206... | Line 204... | ||
| 206 | // reset error integral |
204 | // reset error integral |
| 207 | GPSPosDevIntegral_North = 0; |
205 | GPSPosDevIntegral_North = 0; |
| 208 | GPSPosDevIntegral_East = 0; |
206 | GPSPosDevIntegral_East = 0; |
| 209 | } |
207 | } |
| Line 210... | Line -... | ||
| 210 | - | ||
| 211 | GPSPosDevIntegral_North += GPSPosDev_North; |
- | |
| 212 | GPSPosDevIntegral_East += GPSPosDev_East; |
- | |
| 213 | GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
- | |
| 214 | 208 | ||
| Line 215... | Line 209... | ||
| 215 | //Calculate PID-components of the controller |
209 | //Calculate PID-components of the controller |
| 216 | 210 | ||
| 217 | // D-Part |
211 | // D-Part |
| 218 | D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/256; |
- | |
| Line 219... | Line 212... | ||
| 219 | D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/256; |
212 | D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/512; |
| 220 | GPS_LimitXY(&D_North, &D_East, GPS_D_LIMIT); |
213 | D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/512; |
| 221 | 214 | ||
| 222 | // P-Part |
- | |
| Line 223... | Line 215... | ||
| 223 | P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/1024; |
215 | // P-Part |
| 224 | P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/1024; |
216 | P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/2048; |
| 225 | GPS_LimitXY(&P_North, &P_East, GPS_P_LIMIT); |
217 | P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/2048; |
| 226 | - | ||
| Line -... | Line 218... | ||
| - | 218 | ||
| 227 | // I-Part |
219 | // I-Part |
| 228 | I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
220 | I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| 229 | I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192; |
221 | I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| - | 222 | ||
| - | 223 | ||
| - | 224 | // combine P & I |
|
| - | 225 | PID_North = P_North + I_North; |
|
| - | 226 | PID_East = P_East + I_East; |
|
| - | 227 | if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
|
| - | 228 | { |
|
| - | 229 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
|
| - | 230 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
|
| - | 231 | GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_North, GPS_POSINTEGRAL_LIMIT); |
|
| - | 232 | } |
|
| Line 230... | Line 233... | ||
| 230 | GPS_LimitXY(&I_North, &I_East, GPS_I_LIMIT); |
233 | |
| 231 | 234 | // combine PI- and D-Part |
|
| 232 | // combine P & I & D-Part |
235 | PID_North += D_North; |
| Line 251... | Line 254... | ||
| 251 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
254 | // in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
| 252 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
255 | // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll. |
| Line 253... | Line 256... | ||
| 253 | 256 | ||
| 254 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
257 | coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| 255 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
258 | sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| 256 | PID_Nick = (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2; |
259 | PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192; |
| Line 257... | Line 260... | ||
| 257 | PID_Roll = (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2; |
260 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192; |
| 258 | 261 | ||