- offset -= dt * (0.25 / 60.0); // 360deg/day
- offset = SGMiscd::normalizePeriodic(-360.0,360.0,offset);
+
+ // calculate scaling factor
+ double factor = POW6(spin);
+
+ // calculate time-based precession (scaled by spin factor, since
+ // there is no precession when the gyro is stuck).
+ offset -= dt * (0.25 / 60.0) * factor; // 360deg/day
+
+ // indication should get more and more stuck at low gyro spins
+ if (spin < 0.9)
+ {
+ // when gyro spin is low, then any heading change results in
+ // increasing the offset
+ double diff = SGMiscd::normalizePeriodic(-180.0, 180.0, _last_heading_deg - heading);
+ // scaled by 1-factor, so indication is fully stuck at spin==0 (offset compensates
+ // any heading change)
+ offset += diff * (1.0-factor);
+ }
+ _last_heading_deg = heading;
+
+ // normalize offset
+ offset = SGMiscd::normalizePeriodic(-180.0,180.0,offset);