Rev 828 | Rev 830 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 828 | Rev 829 | ||
|---|---|---|---|
| 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 |
|
| Line 17... | Line 18... | ||
| 17 | #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
18 | #define MAX_VELOCITY 250 // max ground speed in cm/s during position control |
| 18 | 19 | ||
| Line 94... | Line 95... | ||
| 94 | // calculates the GPS control stick values from the deviation to target position |
95 | // calculates the GPS control stick values from the deviation to target position |
| 95 | // if the pointer to the target positin is NULL or is the target position invalid |
96 | // if the pointer to the target positin is NULL or is the target position invalid |
| 96 | // then the P part of the controller is deactivated. |
97 | // then the P part of the controller is deactivated. |
| 97 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
98 | void GPS_PIDController(GPS_Pos_t *pTargetPos) |
| 98 | { |
99 | { |
| 99 | int32_t PID_Stick, PID_Pitch, PID_Roll; |
100 | int32_t temp, temp1, PID_Pitch, PID_Roll; |
| 100 | int32_t coscompass, sincompass; |
101 | int32_t coscompass, sincompass; |
| 101 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
102 | int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 102 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
103 | int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
| 103 | int32_t PID_North = 0, PID_East = 0; |
104 | int32_t PID_North = 0, PID_East = 0; |
| 104 | static int32_t cos_target_latitude = 1; |
105 | static int32_t cos_target_latitude = 1; |
| Line 157... | Line 158... | ||
| 157 | GPSPosDevIntegral_North = 0; |
158 | GPSPosDevIntegral_North = 0; |
| 158 | GPSPosDevIntegral_East = 0; |
159 | GPSPosDevIntegral_East = 0; |
| 159 | } |
160 | } |
| Line 160... | Line 161... | ||
| 160 | 161 | ||
| - | 162 | //Calculate PID-components of the controller (negative sign for compensation) |
|
| - | 163 | ||
| 161 | //Calculate PID-components of the controller (negative sign for compensation) |
164 | // P-Part |
| 162 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
165 | P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
| - | 166 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
|
| - | 167 | ||
| - | 168 | //limit P-part to limit the max velocity |
|
| - | 169 | temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the P-Part limit |
|
| - | 170 | temp = (int32_t)c_sqrt(P_North*P_North + P_East*P_East); // the current P-Part |
|
| - | 171 | if(temp > temp1) // P-Part limit is reached |
|
| - | 172 | { |
|
| - | 173 | // 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; |
|
| - | 178 | GPSPosDevIntegral_East = 0; |
|
| - | 179 | } |
|
| - | 180 | else // P-Part under its limit |
|
| - | 181 | { |
|
| - | 182 | // calculate position error integrals |
|
| - | 183 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
|
| - | 184 | 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; |
|
| - | 186 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
|
| - | 187 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
|
| - | 188 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
|
| - | 189 | } |
|
| - | 190 | ||
| 163 | P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
191 | // I-Part |
| 164 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
192 | I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
| - | 193 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
|
| - | 194 | ||
| 165 | I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
195 | // D-Part |
| 166 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
196 | D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
| - | 197 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
|
| - | 198 | ||
| 167 | D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
199 | |
| 168 | // PD-controller |
200 | // combine P- I- D-Part |
| 169 | PID_North = P_North + I_North + D_North; |
201 | PID_North = P_North + I_North + D_North; |
| Line 170... | Line 202... | ||
| 170 | PID_East = P_East + I_East + D_East; |
202 | PID_East = P_East + I_East + D_East; |
| Line 186... | Line 218... | ||
| 186 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
218 | coscompass = (int32_t)c_cos_8192(CompassHeading); |
| 187 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
219 | sincompass = (int32_t)c_sin_8192(CompassHeading); |
| 188 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
220 | PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
| 189 | PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
221 | PID_Pitch = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
| Line 190... | Line 222... | ||
| 190 | 222 | ||
| 191 | // limit GPS control vector |
223 | // limit resulting GPS control vector |
| 192 | PID_Stick = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
224 | temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Pitch*PID_Pitch); |
| 193 | if (PID_Stick > GPS_STICK_LIMIT) |
225 | if (temp > GPS_STICK_LIMIT) |
| 194 | { |
226 | { |
| 195 | // normalize control vector components to the limit |
227 | // normalize control vector components to the limit |
| 196 | PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/PID_Stick; |
228 | PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp; |
| 197 | PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/PID_Stick; |
- | |
| 198 | } |
- | |
| 199 | else // prevent further growing of error integrals if control limit is reached |
- | |
| 200 | { |
- | |
| 201 | // calculate position error integrals |
- | |
| 202 | GPSPosDevIntegral_North += GPSPosDev_North/16; |
- | |
| 203 | if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 204 | else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 205 | GPSPosDevIntegral_East += GPSPosDev_East/16; |
- | |
| 206 | if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT; |
- | |
| 207 | else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT; |
229 | PID_Pitch = (PID_Pitch * GPS_STICK_LIMIT)/temp; |
| Line 208... | Line 230... | ||
| 208 | } |
230 | } |
| 209 | 231 | ||