202,8 → 202,7 |
if (pTargetPos->Status != INVALID) // and the position data are valid |
{ |
// if the target data are updated or the target pointer has changed |
if ((pTargetPos->Status != PROCESSED) || (pTargetPos |
!= pLastTargetPos)) { |
if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos)) { |
// reset error integral |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
251,8 → 250,7 |
// I-Part |
I_North = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_North) |
/ 8192; |
I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) |
/ 8192; |
I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) / 8192; |
|
// combine P & I |
PID_North = P_North + I_North; |
412,8 → 410,8 |
// beep if signal is not sufficient |
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
BeepTime = 100; |
else if (GPSInfo.satnum < staticParams.NaviGpsMinSat |
&& !(beep_rythm % 5)) |
else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm |
% 5)) |
BeepTime = 10; |
} |
} |