1 // MRG.cxx - an electrcally powered master reference gyro.
2 // Written by Vivian Meazza based on wrok by David Megginson, started 2006.
4 // This file is in the Public Domain and comes with no warranty.
9 #include <simgear/compiler.h>
14 #include <math.h> // fabs()
16 #include <Main/fg_props.hxx>
17 #include <Main/util.hxx>
22 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
23 name("master-reference-gyro"),
27 for ( i = 0; i < node->nChildren(); ++i ) {
28 SGPropertyNode *child = node->getChild(i);
29 string cname = child->getName();
30 string cval = child->getStringValue();
31 if ( cname == "name" ) {
33 } else if ( cname == "number" ) {
34 num = (int) child->getDoubleValue();
36 SG_LOG( SG_INSTR, SG_WARN, "Error in mrg config logic" );
37 if ( name.length() ) {
38 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
44 MasterReferenceGyro::MasterReferenceGyro ()
48 MasterReferenceGyro::~MasterReferenceGyro ()
53 MasterReferenceGyro::init ()
61 _indicated_hdg_rate = 0;
62 _indicated_roll_rate = 0;
63 _indicated_pitch_rate = 0;
66 branch = "/instrumentation/" + name;
68 _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
69 _roll_in_node = fgGetNode("/orientation/roll-deg", true);
70 _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
71 _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
72 _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
73 _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
74 _g_in_node = fgGetNode("/accelerations/pilot-g-damped", true);
75 _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
77 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
78 _off_node = node->getChild("off-flag", 0, true);
79 _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
80 _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
81 _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
82 _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
83 _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
84 _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
85 _responsiveness_node = node->getChild("responsiveness", 0, true);
86 _error_out_node = node->getChild("heading-error-deg", 0, true);
88 _electrical_node->setDoubleValue(0);
89 _responsiveness_node->setDoubleValue(0.75);
90 _off_node->setBoolValue(false);
94 MasterReferenceGyro::bind ()
96 std::ostringstream temp;
99 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
101 fgTie((branch + "/serviceable").c_str(),
102 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
103 fgTie((branch + "/spin").c_str(),
104 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
108 MasterReferenceGyro::unbind ()
110 std::ostringstream temp;
113 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
115 fgUntie((branch + "/serviceable").c_str());
116 fgUntie((branch + "/spin").c_str());
120 MasterReferenceGyro::update (double dt)
122 double indicated_roll = 0;
123 double indicated_pitch = 0;
124 double indicated_hdg = 0;
125 double indicated_roll_rate = 0;
126 double indicated_pitch_rate = 0;
127 double indicated_hdg_rate = 0;
129 // Get the spin from the gyro
130 _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
132 double spin = _gyro.get_spin_norm();
134 // set the "off-flag"
135 if ( _electrical_node->getDoubleValue() == 0 ) {
136 _off_node->setBoolValue(true);
137 } else if ( spin == 1 ) {
138 _off_node->setBoolValue(false);
140 _off_node->setBoolValue(true);
143 // Get the input values
144 double roll = _roll_in_node->getDoubleValue();
145 double pitch = _pitch_in_node->getDoubleValue();
146 double hdg = _hdg_in_node->getDoubleValue();
147 double roll_rate = _yaw_rate_node->getDoubleValue();
148 double pitch_rate = _yaw_rate_node->getDoubleValue();
149 double yaw_rate = _yaw_rate_node->getDoubleValue();
151 //modulate the input by the spin rate
152 double responsiveness = spin * spin * spin * spin * spin * spin;
153 roll = fgGetLowPass( _last_roll, roll, responsiveness );
154 pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
155 hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
157 //but we need to filter the hdg and yaw_rate as well - yuk!
158 responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
159 yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
161 // store the new values
165 _last_roll_rate = roll_rate;
166 _last_pitch_rate = pitch_rate;
167 _last_yaw_rate = yaw_rate;
169 //the gyro only erects inside limits
170 if ( abs ( yaw_rate ) <= 5
171 && (_g_in_node->getDoubleValue() <= 1.5
172 || _g_in_node->getDoubleValue() >= -0.5) ) {
173 indicated_roll = _last_roll;
174 indicated_pitch = _last_pitch;
175 indicated_hdg = _last_hdg;
176 indicated_roll_rate = _last_roll_rate;
177 indicated_pitch_rate = _last_pitch_rate;
178 indicated_hdg_rate = _last_yaw_rate;
180 indicated_roll_rate = 0;
181 indicated_pitch_rate = 0;
182 indicated_hdg_rate = 0;
185 // calculate the difference between the indicated heading
186 // and the selected heading for use with an autopilot
187 static SGPropertyNode *bnode
188 = fgGetNode( "autopilot/settings/target-heading-deg", false );
191 double diff = bnode->getDoubleValue() - indicated_hdg;
192 if ( diff < -180.0 ) { diff += 360.0; }
193 if ( diff > 180.0 ) { diff -= 360.0; }
194 _error_out_node->setDoubleValue( diff );
196 //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
198 double factor = _responsiveness_node->getDoubleValue() * dt;
200 indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
201 indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
202 indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
204 indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
205 indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
206 indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
208 // store the new values
209 _indicated_roll = indicated_roll;
210 _indicated_pitch = indicated_pitch;
211 _indicated_hdg = indicated_hdg;
213 _indicated_roll_rate = indicated_roll_rate;
214 _indicated_pitch_rate = indicated_pitch_rate;
215 _indicated_hdg_rate = indicated_hdg_rate;
217 // add in a gyro underspin "error" if gyro is spinning too slowly
218 const double spin_thresh = 0.8;
219 const double max_roll_error = 40.0;
220 const double max_pitch_error = 12.0;
221 const double max_hdg_error = 140.0;
226 if ( spin <= spin_thresh ) {
227 double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
228 double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
229 double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
230 roll_error = roll_error_factor * roll_error_factor * max_roll_error;
231 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
232 hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
239 _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
240 _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
241 _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
242 _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
243 _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
244 _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );