Re-interpret gyro wander under g-forces
Signed-off-by: Vivian Meazza <vivian.meazza@lineone.net>
This commit is contained in:
Vivian Meazza
parent
b88d0988cd
commit
0840b188d2
2 changed files with 209 additions and 190 deletions
|
|
@ -20,10 +20,11 @@
|
|||
|
||||
#include "mrg.hxx"
|
||||
|
||||
const double MasterReferenceGyro::gravity = -32.1740485564;
|
||||
|
||||
MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
|
||||
_name(node->getStringValue("name", "master-reference-gyro")),
|
||||
_num(node->getIntValue("number", 0))
|
||||
_name(node->getStringValue("name", "master-reference-gyro")),
|
||||
_num(node->getIntValue("number", 0))
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -34,237 +35,252 @@ MasterReferenceGyro::~MasterReferenceGyro ()
|
|||
void
|
||||
MasterReferenceGyro::init ()
|
||||
{
|
||||
_last_hdg = 0;
|
||||
_last_roll = 0;
|
||||
_last_pitch = 0;
|
||||
_indicated_hdg = 0;
|
||||
_indicated_roll = 0;
|
||||
_indicated_pitch = 0;
|
||||
_indicated_hdg_rate = 0;
|
||||
_indicated_roll_rate = 0;
|
||||
_indicated_pitch_rate = 0;
|
||||
_erect_time = 0;
|
||||
|
||||
string branch;
|
||||
branch = "/instrumentation/" + _name;
|
||||
_last_hdg = 0;
|
||||
_last_roll = 0;
|
||||
_last_pitch = 0;
|
||||
_indicated_hdg = 0;
|
||||
_indicated_roll = 0;
|
||||
_indicated_pitch = 0;
|
||||
_indicated_hdg_rate = 0;
|
||||
_indicated_roll_rate = 0;
|
||||
_indicated_pitch_rate = 0;
|
||||
_erect_time = 180;
|
||||
_last_g = 1;
|
||||
_g_error = 10;
|
||||
|
||||
_pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
|
||||
_roll_in_node = fgGetNode("/orientation/roll-deg", true);
|
||||
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
|
||||
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-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);
|
||||
_g_in_node = fgGetNode("/accelerations/pilot-g", true);
|
||||
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
|
||||
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", 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);
|
||||
_hdg_mag_out_node = node->getChild("indicated-mag-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);
|
||||
_error_out_node = node->getChild("heading-bug-error-deg", 0, true);
|
||||
_hdg_input_source_node = node->getChild("heading-source", 0, true);
|
||||
_fast_erect_node = node->getChild("fast-erect", 0, true);
|
||||
string branch;
|
||||
branch = "/instrumentation/" + _name;
|
||||
|
||||
_electrical_node->setDoubleValue(0);
|
||||
_responsiveness_node->setDoubleValue(0.75);
|
||||
_off_node->setBoolValue(false);
|
||||
_hdg_input_source_node->setBoolValue(false);
|
||||
_fast_erect_node->setBoolValue(false);
|
||||
_pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
|
||||
_roll_in_node = fgGetNode("/orientation/roll-deg", true);
|
||||
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
|
||||
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-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);
|
||||
//_g_in_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
|
||||
_g_in_node = fgGetNode("/accelerations/pilot-g", true);
|
||||
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
|
||||
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", 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);
|
||||
_hdg_mag_out_node = node->getChild("indicated-mag-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);
|
||||
_error_out_node = node->getChild("heading-bug-error-deg", 0, true);
|
||||
_hdg_input_source_node = node->getChild("heading-source", 0, true);
|
||||
_fast_erect_node = node->getChild("fast-erect", 0, true);
|
||||
|
||||
_electrical_node->setDoubleValue(0);
|
||||
_responsiveness_node->setDoubleValue(0.75);
|
||||
_off_node->setBoolValue(false);
|
||||
_hdg_input_source_node->setBoolValue(false);
|
||||
_fast_erect_node->setBoolValue(false);
|
||||
_g_in_node->setDoubleValue(1);
|
||||
}
|
||||
|
||||
void
|
||||
MasterReferenceGyro::bind ()
|
||||
{
|
||||
std::ostringstream temp;
|
||||
string branch;
|
||||
temp << _num;
|
||||
branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
|
||||
std::ostringstream temp;
|
||||
string branch;
|
||||
temp << _num;
|
||||
branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
|
||||
|
||||
fgTie((branch + "/serviceable").c_str(),
|
||||
&_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
|
||||
fgTie((branch + "/spin").c_str(),
|
||||
&_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
|
||||
fgTie((branch + "/serviceable").c_str(),
|
||||
&_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
|
||||
fgTie((branch + "/spin").c_str(),
|
||||
&_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
|
||||
}
|
||||
|
||||
void
|
||||
MasterReferenceGyro::unbind ()
|
||||
{
|
||||
std::ostringstream temp;
|
||||
string branch;
|
||||
temp << _num;
|
||||
branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
|
||||
std::ostringstream temp;
|
||||
string branch;
|
||||
temp << _num;
|
||||
branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
|
||||
|
||||
fgUntie((branch + "/serviceable").c_str());
|
||||
fgUntie((branch + "/spin").c_str());
|
||||
fgUntie((branch + "/serviceable").c_str());
|
||||
fgUntie((branch + "/spin").c_str());
|
||||
}
|
||||
|
||||
void
|
||||
MasterReferenceGyro::update (double dt)
|
||||
{
|
||||
double indicated_roll = 0;
|
||||
double indicated_pitch = 0;
|
||||
double indicated_hdg = 0;
|
||||
double indicated_roll_rate = 0;
|
||||
double indicated_pitch_rate = 0;
|
||||
double indicated_hdg_rate = 0;
|
||||
|
||||
// Get the spin from the gyro
|
||||
_gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
|
||||
_gyro.update(dt);
|
||||
double spin = _gyro.get_spin_norm();
|
||||
|
||||
// set the "off-flag"
|
||||
if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
|
||||
_off_node->setBoolValue(false);
|
||||
} else {
|
||||
_off_node->setBoolValue(true);
|
||||
//sanity check
|
||||
if (!fgGetBool("/sim/fdm-initialized", false)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Get the input values
|
||||
double hdg = _hdg_mag_in_node->getDoubleValue();
|
||||
double indicated_roll = 0;
|
||||
double indicated_pitch = 0;
|
||||
double indicated_hdg = 0;
|
||||
double indicated_roll_rate = 0;
|
||||
double indicated_pitch_rate = 0;
|
||||
double indicated_hdg_rate = 0;
|
||||
double hdg = 0;
|
||||
double erect_time_factor = 1;
|
||||
|
||||
if(_hdg_input_source_node->getBoolValue())
|
||||
hdg = _hdg_in_node->getDoubleValue();
|
||||
const double erect_time = 180;
|
||||
const double max_g_error = 10.0;
|
||||
|
||||
double roll = _roll_in_node->getDoubleValue();
|
||||
double pitch = _pitch_in_node->getDoubleValue();
|
||||
|
||||
double roll_rate = _yaw_rate_node->getDoubleValue();
|
||||
double pitch_rate = _yaw_rate_node->getDoubleValue();
|
||||
double yaw_rate = _yaw_rate_node->getDoubleValue();
|
||||
double g = _g_in_node->getDoubleValue();
|
||||
//Get the spin from the gyro
|
||||
_gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
|
||||
_gyro.update(dt);
|
||||
double spin = _gyro.get_spin_norm();
|
||||
|
||||
//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 );
|
||||
// set the "off-flag"
|
||||
if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
|
||||
_off_node->setBoolValue(false);
|
||||
} else {
|
||||
_off_node->setBoolValue(true);
|
||||
return;
|
||||
}
|
||||
|
||||
if ((hdg - _last_hdg) > 180)
|
||||
_last_hdg += 360;
|
||||
if ((hdg - _last_hdg) < -180)
|
||||
_last_hdg -= 360;
|
||||
// Get the input values
|
||||
if(_hdg_input_source_node->getBoolValue()){
|
||||
hdg = _hdg_in_node->getDoubleValue();
|
||||
} else {
|
||||
hdg = _hdg_mag_in_node->getDoubleValue();
|
||||
}
|
||||
|
||||
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
|
||||
double roll = _roll_in_node->getDoubleValue();
|
||||
double pitch = _pitch_in_node->getDoubleValue();
|
||||
double g = _g_in_node->getDoubleValue()/* / gravity*/;
|
||||
|
||||
//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 );
|
||||
g = fgGetLowPass( _last_g , g, 0.015 );
|
||||
double roll_rate = _yaw_rate_node->getDoubleValue();
|
||||
double pitch_rate = _pitch_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 );
|
||||
|
||||
if ((hdg - _last_hdg) > 180)
|
||||
_last_hdg += 360;
|
||||
if ((hdg - _last_hdg) < -180)
|
||||
_last_hdg -= 360;
|
||||
|
||||
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 );
|
||||
g = fgGetLowPass( _last_g , g, 1.5);
|
||||
|
||||
|
||||
// store the new values
|
||||
_last_roll = roll;
|
||||
_last_pitch = pitch;
|
||||
_last_hdg = hdg;
|
||||
_last_roll_rate = roll_rate;
|
||||
_last_pitch_rate = pitch_rate;
|
||||
_last_yaw_rate = yaw_rate;
|
||||
_last_g = g;
|
||||
// store the new values
|
||||
_last_roll = roll;
|
||||
_last_pitch = pitch;
|
||||
_last_hdg = hdg;
|
||||
_last_roll_rate = roll_rate;
|
||||
_last_pitch_rate = pitch_rate;
|
||||
_last_yaw_rate = yaw_rate;
|
||||
_last_g = g;
|
||||
|
||||
if (_erect_time > 0){
|
||||
//the gyro only erects inside limits
|
||||
if ( fabs ( yaw_rate ) <= 5
|
||||
&& g <= 1.5 && g >= -0.5){
|
||||
|
||||
if ( !_fast_erect_node->getBoolValue() )
|
||||
_erect_time -= dt;
|
||||
else
|
||||
_erect_time -= 2 * dt;
|
||||
if ( !_fast_erect_node->getBoolValue() ){
|
||||
erect_time_factor = 1;
|
||||
} else {
|
||||
erect_time_factor = 2;
|
||||
}
|
||||
|
||||
_g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor;
|
||||
} else {
|
||||
_g_error += (max_g_error /(erect_time * 0.33)) * dt * 2;
|
||||
|
||||
//SG_LOG(SG_GENERAL, SG_ALERT,_num <<
|
||||
// " g input " << _g_in_node->getDoubleValue() * gravity
|
||||
// <<" _erect_time " << _erect_time
|
||||
// << " yaw " << yaw_rate
|
||||
// << " pitch " << _pitch_rate_node->getDoubleValue()
|
||||
// << " roll " << _roll_rate_node->getDoubleValue());
|
||||
|
||||
}
|
||||
|
||||
//SG_LOG(SG_GENERAL, SG_ALERT,
|
||||
// "g input " << g <<" _erect_time " << _erect_time << " spin " << spin);
|
||||
//cout << "_g_error "<< _g_error << endl;
|
||||
_g_error = SGMiscd::clip(_g_error, 0, 10);
|
||||
// cout << "_g_error clip "<< _g_error << endl;
|
||||
indicated_roll = _last_roll + _g_error;
|
||||
indicated_pitch = _last_pitch + _g_error;
|
||||
indicated_hdg = _last_hdg + _g_error;
|
||||
indicated_roll_rate = _last_roll_rate;
|
||||
indicated_pitch_rate = _last_pitch_rate;
|
||||
indicated_hdg_rate = _last_yaw_rate;
|
||||
// calculate the difference between the indicated heading
|
||||
// and the selected heading for use with an autopilot
|
||||
static SGPropertyNode *bnode
|
||||
= fgGetNode( "/autopilot/settings/heading-bug-deg", false );
|
||||
|
||||
//the gyro only erects inside limits
|
||||
if ( fabs ( yaw_rate ) <= 5
|
||||
&& ( g <= 1.5 || g >= -0.5)
|
||||
&& _erect_time <=0 ) {
|
||||
indicated_roll = _last_roll;
|
||||
indicated_pitch = _last_pitch;
|
||||
indicated_hdg = _last_hdg;
|
||||
indicated_roll_rate = _last_roll_rate;
|
||||
indicated_pitch_rate = _last_pitch_rate;
|
||||
indicated_hdg_rate = _last_yaw_rate;
|
||||
} else {
|
||||
indicated_roll_rate = 0;
|
||||
indicated_pitch_rate = 0;
|
||||
indicated_hdg_rate = 0;
|
||||
|
||||
if (_erect_time <= 0 )
|
||||
_erect_time = 34;
|
||||
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 );
|
||||
//SG_LOG(SG_GENERAL, SG_ALERT,
|
||||
//"autopilot input " << bnode->getDoubleValue()
|
||||
//<< " output " << _error_out_node->getDoubleValue()<<);
|
||||
}
|
||||
|
||||
}
|
||||
//smooth the output
|
||||
double factor = _responsiveness_node->getDoubleValue() * dt;
|
||||
|
||||
// calculate the difference between the indicated heading
|
||||
// and the selected heading for use with an autopilot
|
||||
static SGPropertyNode *bnode
|
||||
= fgGetNode( "/autopilot/settings/heading-bug-deg", false );
|
||||
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
|
||||
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
|
||||
//indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
|
||||
|
||||
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 );
|
||||
//SG_LOG(SG_GENERAL, SG_ALERT,
|
||||
//"autopilot input " << bnode->getDoubleValue()
|
||||
//<< " output " << _error_out_node->getDoubleValue()<<);
|
||||
}
|
||||
|
||||
//smooth the output
|
||||
double factor = _responsiveness_node->getDoubleValue() * dt;
|
||||
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
|
||||
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
|
||||
indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
|
||||
|
||||
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
|
||||
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
|
||||
//indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
|
||||
// store the new values
|
||||
_indicated_roll = indicated_roll;
|
||||
_indicated_pitch = indicated_pitch;
|
||||
_indicated_hdg = indicated_hdg;
|
||||
|
||||
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
|
||||
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
|
||||
indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
|
||||
_indicated_roll_rate = indicated_roll_rate;
|
||||
_indicated_pitch_rate = indicated_pitch_rate;
|
||||
_indicated_hdg_rate = indicated_hdg_rate;
|
||||
|
||||
// store the new values
|
||||
_indicated_roll = indicated_roll;
|
||||
_indicated_pitch = indicated_pitch;
|
||||
_indicated_hdg = indicated_hdg;
|
||||
// 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;
|
||||
const double max_hdg_error = 140.0;
|
||||
double roll_error;
|
||||
double pitch_error;
|
||||
double hdg_error;
|
||||
|
||||
_indicated_roll_rate = indicated_roll_rate;
|
||||
_indicated_pitch_rate = indicated_pitch_rate;
|
||||
_indicated_hdg_rate = indicated_hdg_rate;
|
||||
if ( spin <= spin_thresh ) {
|
||||
double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
|
||||
double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
|
||||
double hdg_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;
|
||||
hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
|
||||
} else {
|
||||
roll_error = 0.0;
|
||||
pitch_error = 0.0;
|
||||
hdg_error = 0.0;
|
||||
}
|
||||
|
||||
// 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;
|
||||
const double max_hdg_error = 140.0;
|
||||
double roll_error;
|
||||
double pitch_error;
|
||||
double hdg_error;
|
||||
|
||||
if ( spin <= spin_thresh ) {
|
||||
double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
|
||||
double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
|
||||
double hdg_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;
|
||||
hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
|
||||
} else {
|
||||
roll_error = 0.0;
|
||||
pitch_error = 0.0;
|
||||
hdg_error = 0.0;
|
||||
}
|
||||
|
||||
_roll_out_node->setDoubleValue( _indicated_roll + roll_error );
|
||||
_pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
|
||||
_hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
|
||||
_pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
|
||||
_roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
|
||||
_hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
|
||||
_roll_out_node->setDoubleValue( _indicated_roll + roll_error );
|
||||
_pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
|
||||
_hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
|
||||
_pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
|
||||
_roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
|
||||
_hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
|
||||
}
|
||||
|
||||
// end of mrg.cxx
|
||||
|
|
|
|||
|
|
@ -49,6 +49,8 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
static const double gravity; //conversion factor
|
||||
|
||||
string _name;
|
||||
int _num;
|
||||
|
||||
|
|
@ -66,6 +68,7 @@ private:
|
|||
double _last_yaw_rate;
|
||||
double _last_g;
|
||||
double _erect_time;
|
||||
double _g_error;
|
||||
|
||||
Gyro _gyro;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue