Rev 829 | Rev 844 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 829 | Rev 830 | ||
|---|---|---|---|
| Line 13... | Line 13... | ||
| 13 | #define TSK_HOME 2 |
13 | #define TSK_HOME 2 |
| Line 14... | Line 14... | ||
| 14 | 14 | ||
| 15 | #define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
15 | #define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
| 16 | #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
16 | #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| 17 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
17 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
| Line 18... | Line 18... | ||
| 18 | #define MAX_VELOCITY 250 // max ground speed in cm/s during position control |
18 | #define MAX_VELOCITY 300 // max ground speed in cm/s during position control |
| 19 | 19 | ||
| Line 163... | Line 163... | ||
| 163 | 163 | ||
| 164 | // P-Part |
164 | // P-Part |
| 165 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
165 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| Line -... | Line 166... | ||
| - | 166 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
|
| - | 167 | ||
| - | 168 | // I-Part |
|
| - | 169 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
|
| - | 170 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
|
| - | 171 | ||
| - | 172 | // combine P- & I-Part |
|
| - | 173 | PID_North = P_North + I_North; |
|
| 166 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
174 | PID_East = P_East + I_East; |
| 167 | 175 | ||
| 168 | //limit P-part to limit the max velocity |
176 | //limit PI-Part to limit the max velocity |
| 169 | temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the P-Part limit |
177 | temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit |
| 170 | temp = (int32_t)c_sqrt(P_North*P_North + P_East*P_East); // the current P-Part |
178 | temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part |
| 171 | if(temp > temp1) // P-Part limit is reached |
179 | if(temp > temp1) // P-Part limit is reached |
| 172 | { |
180 | { |
| 173 | // normalize P-part components to the P-Part limit |
181 | // normalize P-part components to the P-Part limit |
| 174 | P_North = (P_North * temp1)/temp; |
- | |
| 175 | P_East = (P_East * temp1)/temp; |
- | |
| 176 | // reset error integral |
- | |
| 177 | GPSPosDevIntegral_North = 0; |
182 | PID_North = (PID_North * temp1)/temp; |
| 178 | GPSPosDevIntegral_East = 0; |
183 | PID_East = (PID_East * temp1)/temp; |
| 179 | } |
184 | } |
| 180 | else // P-Part under its limit |
185 | else // PI-Part under its limit |
| 181 | { |
186 | { |
| 182 | // calculate position error integrals |
187 | // update position error integrals |
| 183 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
188 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
| 184 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
189 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
| 185 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
190 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
| 186 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
191 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
| 187 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
192 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
| Line 188... | Line -... | ||
| 188 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 189 | } |
- | |
| 190 | - | ||
| 191 | // I-Part |
- | |
| 192 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
193 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
| 193 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
194 | } |
| 194 | 195 | ||
| Line 195... | Line 196... | ||
| 195 | // D-Part |
196 | // D-Part |
| 196 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
197 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| 197 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
198 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
| Line 198... | Line 199... | ||
| 198 | 199 | ||
| Line 199... | Line 200... | ||
| 199 | 200 | ||
| 200 | // combine P- I- D-Part |
201 | // combine PI- and D-Part |