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 |