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/indicated-pitch-deg",
43 fgGetNode("/instrumentation/attitude-indicator/indicated-roll-deg",
48 AttitudeIndicator::bind ()
50 fgTie("/instrumentation/attitude-indicator/serviceable",
51 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
52 fgTie("/instrumentation/attitude-indicator/spin",
53 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
57 AttitudeIndicator::unbind ()
59 fgUntie("/instrumentation/attitude-indicator/serviceable");
60 fgUntie("/instrumentation/attitude-indicator/spin");
64 AttitudeIndicator::update (double dt)
66 // If it's caged, it doesn't indicate
67 if (_caged_node->getBoolValue()) {
68 _roll_out_node->setDoubleValue(0.0);
69 _pitch_out_node->setDoubleValue(0.0);
73 // Get the spin from the gyro
74 _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
76 double spin = _gyro.get_spin_norm();
78 // Calculate the responsiveness
79 double responsiveness = spin * spin * spin * spin * spin * spin;
81 // Get the indicated roll and pitch
82 double roll = _roll_in_node->getDoubleValue();
83 double pitch = _pitch_in_node->getDoubleValue();
85 // Calculate the tumble for the
87 if (_tumble_flag_node->getBoolValue()) {
88 double tumble = _tumble_node->getDoubleValue();
89 if (fabs(roll) > 45.0) {
90 double target = (fabs(roll) - 45.0) / 45.0;
91 target *= target; // exponential past +-45 degrees
95 if (fabs(target) > fabs(tumble))
100 else if (tumble < -1.0)
103 // Reerect in 5 minutes
104 double step = dt/300.0;
107 else if (tumble > step)
111 _tumble_node->setDoubleValue(tumble);
114 roll = fgGetLowPass(_roll_out_node->getDoubleValue(), roll,
116 pitch = fgGetLowPass(_pitch_out_node->getDoubleValue(), pitch,
119 // Assign the new values
120 _roll_out_node->setDoubleValue(roll);
121 _pitch_out_node->setDoubleValue(pitch);
124 // end of attitude_indicator.cxx