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