+
+ roll = fgGetLowPass(_roll_int_node->getDoubleValue(), roll,
+ responsiveness);
+ pitch = fgGetLowPass(_pitch_int_node->getDoubleValue(), pitch,
+ responsiveness);
+
+ // Assign the new values
+ _roll_int_node->setDoubleValue(roll);
+ _pitch_int_node->setDoubleValue(pitch);
+
+ // add in a gyro underspin "error" if gyro is spinning too slowly
+ const double spin_thresh = 0.8;
+ const double max_roll_error = 40.0;
+ const double max_pitch_error = 12.0;
+ double roll_error;
+ double pitch_error;
+ if ( spin <= spin_thresh ) {
+ double roll_error_factor = (spin_thresh - spin) / spin_thresh;
+ double pitch_error_factor = (spin_thresh - spin) / spin_thresh;
+ roll_error = roll_error_factor * roll_error_factor * max_roll_error;
+ pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
+ } else {
+ roll_error = 0.0;
+ pitch_error = 0.0;
+ }
+
+ _roll_out_node->setDoubleValue(roll + roll_error);
+ _pitch_out_node->setDoubleValue(pitch + pitch_error);