_pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
_roll_in_node = fgGetNode("/orientation/roll-deg", true);
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
_pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
_roll_in_node = fgGetNode("/orientation/roll-deg", true);
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
_pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
_roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
_yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
_pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
_roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
_yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_off_node = node->getChild("off-flag", 0, true);
_pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
_roll_out_node = node->getChild("indicated-roll-deg", 0, true);
_hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_off_node = node->getChild("off-flag", 0, true);
_pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
_roll_out_node = node->getChild("indicated-roll-deg", 0, true);
_hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
_pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
_roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
_hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
_responsiveness_node = node->getChild("responsiveness", 0, true);
_pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
_roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
_hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
_responsiveness_node = node->getChild("responsiveness", 0, true);
- _error_out_node = node->getChild("heading-error-deg", 0, true);
+ _error_out_node = node->getChild("heading-bug-error-deg", 0, true);
+ _hdg_input_source_node = node->getChild("heading-source", 0, true);
_electrical_node->setDoubleValue(0);
_responsiveness_node->setDoubleValue(0.75);
_off_node->setBoolValue(false);
_electrical_node->setDoubleValue(0);
_responsiveness_node->setDoubleValue(0.75);
_off_node->setBoolValue(false);
double roll_rate = _yaw_rate_node->getDoubleValue();
double pitch_rate = _yaw_rate_node->getDoubleValue();
double yaw_rate = _yaw_rate_node->getDoubleValue();
double roll_rate = _yaw_rate_node->getDoubleValue();
double pitch_rate = _yaw_rate_node->getDoubleValue();
double yaw_rate = _yaw_rate_node->getDoubleValue();
//modulate the input by the spin rate
double responsiveness = spin * spin * spin * spin * spin * spin;
roll = fgGetLowPass( _last_roll, roll, responsiveness );
pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
//modulate the input by the spin rate
double responsiveness = spin * spin * spin * spin * spin * spin;
roll = fgGetLowPass( _last_roll, roll, responsiveness );
pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
//but we need to filter the hdg and yaw_rate as well - yuk!
responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
//but we need to filter the hdg and yaw_rate as well - yuk!
responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
_last_roll_rate = roll_rate;
_last_pitch_rate = pitch_rate;
_last_yaw_rate = yaw_rate;
_last_roll_rate = roll_rate;
_last_pitch_rate = pitch_rate;
_last_yaw_rate = yaw_rate;
// calculate the difference between the indicated heading
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
// calculate the difference between the indicated heading
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
if ( bnode ) {
double diff = bnode->getDoubleValue() - indicated_hdg;
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
_error_out_node->setDoubleValue( diff );
if ( bnode ) {
double diff = bnode->getDoubleValue() - indicated_hdg;
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
_error_out_node->setDoubleValue( diff );
//smooth the output
double factor = _responsiveness_node->getDoubleValue() * dt;
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
//smooth the output
double factor = _responsiveness_node->getDoubleValue() * dt;
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );