1 // attitude_indicator.cxx - a vacuum-powered attitude indicator.
2 // Written by David Megginson, started 2002.
4 // This file is in the Public Domain and comes with no warranty.
9 #include <math.h> // fabs()
11 #include "attitude_indicator.hxx"
12 #include <Main/fg_props.hxx>
13 #include <Main/util.hxx>
16 AttitudeIndicator::AttitudeIndicator ()
20 AttitudeIndicator::~AttitudeIndicator ()
25 AttitudeIndicator::init ()
27 // TODO: allow index of pump and AI
29 _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
30 _roll_in_node = fgGetNode("/orientation/roll-deg", true);
31 _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
33 fgGetNode("/instrumentation/attitude-indicator/config/tumble-flag",
36 fgGetNode("/instrumentation/attitude-indicator/caged-flag", true);
38 fgGetNode("/instrumentation/attitude-indicator/tumble-norm", true);
40 fgGetNode("/instrumentation/attitude-indicator/internal-pitch-deg",
43 fgGetNode("/instrumentation/attitude-indicator/internal-roll-deg",
46 fgGetNode("/instrumentation/attitude-indicator/indicated-pitch-deg",
49 fgGetNode("/instrumentation/attitude-indicator/indicated-roll-deg",
54 AttitudeIndicator::bind ()
56 fgTie("/instrumentation/attitude-indicator/serviceable",
57 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
58 fgTie("/instrumentation/attitude-indicator/spin",
59 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
63 AttitudeIndicator::unbind ()
65 fgUntie("/instrumentation/attitude-indicator/serviceable");
66 fgUntie("/instrumentation/attitude-indicator/spin");
70 AttitudeIndicator::update (double dt)
72 // If it's caged, it doesn't indicate
73 if (_caged_node->getBoolValue()) {
74 _roll_int_node->setDoubleValue(0.0);
75 _pitch_int_node->setDoubleValue(0.0);
79 // Get the spin from the gyro
80 _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
82 double spin = _gyro.get_spin_norm();
84 // Calculate the responsiveness
85 double responsiveness = spin * spin * spin * spin * spin * spin;
87 // Get the indicated roll and pitch
88 double roll = _roll_in_node->getDoubleValue();
89 double pitch = _pitch_in_node->getDoubleValue();
91 // Calculate the tumble for the
93 if (_tumble_flag_node->getBoolValue()) {
94 double tumble = _tumble_node->getDoubleValue();
95 if (fabs(roll) > 45.0) {
96 double target = (fabs(roll) - 45.0) / 45.0;
97 target *= target; // exponential past +-45 degrees
101 if (fabs(target) > fabs(tumble))
106 else if (tumble < -1.0)
109 // Reerect in 5 minutes
110 double step = dt/300.0;
113 else if (tumble > step)
117 _tumble_node->setDoubleValue(tumble);
120 roll = fgGetLowPass(_roll_int_node->getDoubleValue(), roll,
122 pitch = fgGetLowPass(_pitch_int_node->getDoubleValue(), pitch,
125 // Assign the new values
126 _roll_int_node->setDoubleValue(roll);
127 _pitch_int_node->setDoubleValue(pitch);
129 // add in a gyro underspin "error" if gyro is spinning too slowly
130 const double spin_thresh = 0.8;
131 const double max_roll_error = 40.0;
132 const double max_pitch_error = 12.0;
135 if ( spin <= spin_thresh ) {
136 double roll_error_factor = (spin_thresh - spin) / spin_thresh;
137 double pitch_error_factor = (spin_thresh - spin) / spin_thresh;
138 roll_error = roll_error_factor * roll_error_factor * max_roll_error;
139 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
145 _roll_out_node->setDoubleValue(roll + roll_error);
146 _pitch_out_node->setDoubleValue(pitch + pitch_error);
149 // end of attitude_indicator.cxx