Rev 1805 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1805 | Rev 1821 | ||
|---|---|---|---|
| Line 200... | Line 200... | ||
| 200 | if (pTargetPos != NULL) // if there is a target position |
200 | if (pTargetPos != NULL) // if there is a target position |
| 201 | { |
201 | { |
| 202 | if (pTargetPos->Status != INVALID) // and the position data are valid |
202 | if (pTargetPos->Status != INVALID) // and the position data are valid |
| 203 | { |
203 | { |
| 204 | // if the target data are updated or the target pointer has changed |
204 | // if the target data are updated or the target pointer has changed |
| 205 | if ((pTargetPos->Status != PROCESSED) || (pTargetPos |
205 | if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos)) { |
| 206 | != pLastTargetPos)) { |
- | |
| 207 | // reset error integral |
206 | // reset error integral |
| 208 | GPSPosDevIntegral_North = 0; |
207 | GPSPosDevIntegral_North = 0; |
| 209 | GPSPosDevIntegral_East = 0; |
208 | GPSPosDevIntegral_East = 0; |
| 210 | // recalculate latitude projection |
209 | // recalculate latitude projection |
| 211 | cos_target_latitude = (int32_t) c_cos_8192( |
210 | cos_target_latitude = (int32_t) c_cos_8192( |
| Line 249... | Line 248... | ||
| 249 | P_East = ((int32_t) dynamicParams.NaviGpsP * GPSPosDev_East) / 2048; |
248 | P_East = ((int32_t) dynamicParams.NaviGpsP * GPSPosDev_East) / 2048; |
| Line 250... | Line 249... | ||
| 250 | 249 | ||
| 251 | // I-Part |
250 | // I-Part |
| 252 | I_North = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_North) |
251 | I_North = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_North) |
| 253 | / 8192; |
252 | / 8192; |
| 254 | I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) |
- | |
| Line 255... | Line 253... | ||
| 255 | / 8192; |
253 | I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) / 8192; |
| 256 | 254 | ||
| 257 | // combine P & I |
255 | // combine P & I |
| 258 | PID_North = P_North + I_North; |
256 | PID_North = P_North + I_North; |
| Line 410... | Line 408... | ||
| 410 | GPS_Neutral(); |
408 | GPS_Neutral(); |
| 411 | if (FlightMode != GPS_FLIGHT_MODE_FREE) { |
409 | if (FlightMode != GPS_FLIGHT_MODE_FREE) { |
| 412 | // beep if signal is not sufficient |
410 | // beep if signal is not sufficient |
| 413 | if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
411 | if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
| 414 | BeepTime = 100; |
412 | BeepTime = 100; |
| 415 | else if (GPSInfo.satnum < staticParams.NaviGpsMinSat |
413 | else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm |
| 416 | && !(beep_rythm % 5)) |
414 | % 5)) |
| 417 | BeepTime = 10; |
415 | BeepTime = 10; |
| 418 | } |
416 | } |
| 419 | } |
417 | } |
| 420 | // set current data as processed to avoid further calculations on the same gps data |
418 | // set current data as processed to avoid further calculations on the same gps data |
| 421 | GPSInfo.status = PROCESSED; |
419 | GPSInfo.status = PROCESSED; |