Rev 936 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 936 | Rev 937 | ||
|---|---|---|---|
| Line 16... | Line 16... | ||
| 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_STICK_LIMIT 200 // limit of gps stick control to avoid critical flight attitudes |
19 | #define GPS_STICK_LIMIT 200 // limit of gps stick control to avoid critical flight attitudes |
| 20 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
20 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
| Line 21... | Line 21... | ||
| 21 | #define GPS_P_LIMIT 3200 |
21 | #define GPS_P_LIMIT 100 |
| 22 | 22 | ||
| 23 | 23 | ||
| Line 207... | Line 207... | ||
| 207 | } |
207 | } |
| Line 208... | Line 208... | ||
| 208 | 208 | ||
| Line 209... | Line 209... | ||
| 209 | //Calculate PID-components of the controller (negative sign for compensation) |
209 | //Calculate PID-components of the controller (negative sign for compensation) |
| 210 | 210 | ||
| 211 | // P-Part |
211 | // P-Part |
| Line 212... | Line 212... | ||
| 212 | P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/16; |
212 | P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
| 213 | P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/16; |
213 | P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
| 214 | 214 | ||
| Line 215... | Line 215... | ||
| 215 | // I-Part |
215 | // I-Part |
| 216 | I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/64; |
216 | I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
| 217 | I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/64; |
217 | I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
| Line 232... | Line 232... | ||
| 232 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
232 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
| 233 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
233 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
| 234 | } |
234 | } |
| Line 235... | Line 235... | ||
| 235 | 235 | ||
| 236 | // D-Part |
236 | // D-Part |
| 237 | D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/4; |
237 | D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
| Line 238... | Line 238... | ||
| 238 | D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/4; |
238 | D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
| 239 | 239 | ||
| 240 | // combine PI- and D-Part |
240 | // combine PI- and D-Part |
| Line 241... | Line 241... | ||
| 241 | PID_North += D_North; |
241 | PID_North += D_North; |
| 242 | PID_East += D_East; |
242 | PID_East += D_East; |
| 243 | 243 | ||
| Line 244... | Line 244... | ||
| 244 | // scale combination with gain. |
244 | // scale combination with gain. |
| Line 245... | Line 245... | ||
| 245 | PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / (100 * 32); |
245 | PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / 100; |
| 246 | PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / (100 * 32); |
246 | PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / 100; |