-
- if (_damp_sync < interval) {
- if (_damp_roll > 0.0)
- roll_deg = _damped_roll_deg;
- if (_damp_pitch > 0.0)
- pitch_deg = _damped_pitch_deg;
- if (_damp_heading > 0.0)
- heading_deg = _damped_heading_deg;
- return;
- }
-
- while (_damp_sync >= interval) {
- _damp_sync -= interval;
-
- double d;
- if (_damp_roll > 0.0) {
- d = _damped_roll_deg - roll_deg;
- if (d >= 180.0)
- _damped_roll_deg -= 360.0;
- else if (d < -180.0)
- _damped_roll_deg += 360.0;
- roll_deg = _damped_roll_deg = roll_deg * _damp_roll + _damped_roll_deg * (1 - _damp_roll);
- }
-
- if (_damp_pitch > 0.0) {
- d = _damped_pitch_deg - pitch_deg;
- if (d >= 180.0)
- _damped_pitch_deg -= 360.0;
- else if (d < -180.0)
- _damped_pitch_deg += 360.0;
- pitch_deg = _damped_pitch_deg = pitch_deg * _damp_pitch + _damped_pitch_deg * (1 - _damp_pitch);
- }
-
- if (_damp_heading > 0.0) {
- d = _damped_heading_deg - heading_deg;
- if (d >= 180.0)
- _damped_heading_deg -= 360.0;
- else if (d < -180.0)
- _damped_heading_deg += 360.0;
- heading_deg = _damped_heading_deg = heading_deg * _damp_heading + _damped_heading_deg * (1 - _damp_heading);
- }
- }
+
+ const double interval = 0.01;
+ while (dt > interval) {
+
+ for (unsigned int i=0; i<3; ++i) {
+ if (_dampFactor[i] <= 0.0) {
+ // axis is un-damped, set output to target directly
+ _dampOutput[i] = _dampTarget[i];
+ continue;
+ }
+
+ double d = _dampOutput[i] - _dampTarget[i];
+ if (d > 180.0) {
+ _dampOutput[i] -= 360.0;
+ } else if (d < -180.0) {
+ _dampOutput[i] += 360.0;
+ }
+
+ _dampOutput[i] = (_dampTarget[i] * _dampFactor[i]) +
+ (_dampOutput[i] * (1.0 - _dampFactor[i]));
+ } // of axis iteration
+
+ dt -= interval;
+ } // of dt subdivision by interval