Rev 2076 | Rev 2087 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2076 | Rev 2086 | ||
|---|---|---|---|
| Line 162... | Line 162... | ||
| 162 | // if GPS data and Compass are ok |
162 | // if GPS data and Compass are ok |
| 163 | if (navi_isGPSSignalOK() && (magneticHeading >= 0)) { |
163 | if (navi_isGPSSignalOK() && (magneticHeading >= 0)) { |
| 164 | if (pTargetPos != NULL) { // if there is a target position |
164 | if (pTargetPos != NULL) { // if there is a target position |
| 165 | if (pTargetPos->status != INVALID) { // and the position data are valid |
165 | if (pTargetPos->status != INVALID) { // and the position data are valid |
| 166 | // if the target data are updated or the target pointer has changed |
166 | // if the target data are updated or the target pointer has changed |
| 167 | if ((pTargetPos->status != PROCESSED) |
- | |
| 168 | || (pTargetPos != pLastTargetPos)) { |
167 | if ((pTargetPos->status != PROCESSED) || (pTargetPos != pLastTargetPos)) { |
| 169 | // reset error integral |
168 | // reset error integral |
| 170 | GPSPosDevIntegral_North = 0; |
169 | GPSPosDevIntegral_North = 0; |
| 171 | GPSPosDevIntegral_East = 0; |
170 | GPSPosDevIntegral_East = 0; |
| 172 | // recalculate latitude projection |
171 | // recalculate latitude projection |
| 173 | cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L); |
172 | cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L); |
| Line 266... | Line 265... | ||
| 266 | 265 | ||
| 267 | void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
266 | void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
| 268 | static uint8_t GPS_P_Delay = 0; |
267 | static uint8_t GPS_P_Delay = 0; |
| Line -... | Line 268... | ||
| - | 268 | static uint16_t beep_rythm = 0; |
|
| - | 269 | ||
| - | 270 | static uint16_t navi_testOscPrescaler = 0; |
|
| - | 271 | static uint8_t navi_testOscTimer = 0; |
|
| - | 272 | ||
| - | 273 | navi_testOscPrescaler++; |
|
| - | 274 | if (navi_testOscPrescaler == 488) { |
|
| - | 275 | navi_testOscPrescaler = 0; |
|
| - | 276 | navi_testOscTimer++; |
|
| - | 277 | if (navi_testOscTimer == staticParams.naviTestOscPeriod) { |
|
| - | 278 | navi_testOscTimer = 0; |
|
| - | 279 | if (staticParams.naviTestOscAmplitude) { |
|
| - | 280 | holdPosition->status = NEWDATA; |
|
| - | 281 | holdPosition->latitude += staticParams.naviTestOscAmplitude * 90L; |
|
| - | 282 | } |
|
| - | 283 | } else if (navitestOscTimer == staticParams.naviTestOscPeriod/2) { |
|
| - | 284 | if (staticParams.naviTestOscAmplitude) { |
|
| - | 285 | holdPosition->status = NEWDATA; |
|
| - | 286 | holdPosition->latitude -= staticParams.naviTestOscAmplitude * 90L; |
|
| - | 287 | } |
|
| - | 288 | } |
|
| 269 | static uint16_t beep_rythm = 0; |
289 | } |
| Line 270... | Line 290... | ||
| 270 | 290 | ||
| 271 | navi_updateFlightMode(); |
291 | navi_updateFlightMode(); |
| 272 | 292 | ||