Rev 942 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 942 | Rev 950 | ||
---|---|---|---|
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 |