fly/flightgear
1
0
Fork 0
Code Activity

Vivian MEAZZA: Directional Gyro (DG) compass & electrically powered master

Browse source 
reference gyro
This commit is contained in:
mfranz 2006-10-23 21:58:28 +00:00
parent 43ce93303c
commit eb6b28f507
5 changed files with 564 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/Instrumentation/Makefile.am
View file

@ -17,10 +17,12 @@ libInstrumentation_a_SOURCES = \
gyro.cxx gyro.hxx \
heading_indicator.cxx heading_indicator.hxx \
heading_indicator_fg.cxx heading_indicator_fg.hxx \
heading_indicator_dg.cxx heading_indicator_dg.hxx \
kr_87.hxx kr_87.cxx \
kt_70.cxx kt_70.hxx \
mag_compass.cxx mag_compass.hxx \
marker_beacon.cxx marker_beacon.hxx \
mrg.cxx mrg.hxx\
navradio.cxx navradio.hxx \
slip_skid_ball.cxx slip_skid_ball.hxx \
transponder.cxx transponder.hxx \

155
src/Instrumentation/heading_indicator_dg.cxx Normal file
View file

@ -0,0 +1,155 @@
// heading_indicator_dg.cxx - a Directional Gyro (DG) compass.
// Based on the vacuum driven Heading Indicator Written by David Megginson,
// started 2002.
//
// Written by Vivian Meazza, started 2005.
//
// This file is in the Public Domain and comes with no warranty.
#include <simgear/compiler.h>
#include STL_IOSTREAM
#include STL_STRING
#include <sstream>
#include <Main/fg_props.hxx>
#include <Main/util.hxx>
#include "heading_indicator_dg.hxx"
HeadingIndicatorDG::HeadingIndicatorDG ( SGPropertyNode *node ) :
name("heading-indicator-dg"),
num(0)
{
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "name" ) {
name = cval;
} else if ( cname == "number" ) {
num = child->getIntValue();
} else {
SG_LOG( SG_INSTR, SG_WARN, "Error in DG heading-indicator config logic" );
if ( name.length() ) {
SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
}
}
}
}
HeadingIndicatorDG::HeadingIndicatorDG ()
{
}
HeadingIndicatorDG::~HeadingIndicatorDG ()
{
}
void
HeadingIndicatorDG::init ()
{
string branch;
branch = "/instrumentation/" + name;
_heading_in_node = fgGetNode("/orientation/heading-deg", true);
SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
_offset_node = node->getChild("offset-deg", 0, true);
_serviceable_node = node->getChild("serviceable", 0, true);
_error_node = node->getChild("heading-bug-error-deg", 0, true);
_nav1_error_node = node->getChild("nav1-course-error-deg", 0, true);
_heading_out_node = node->getChild("indicated-heading-deg", 0, true);
_last_heading_deg = (_heading_in_node->getDoubleValue() +
_offset_node->getDoubleValue());
_electrical_node = fgGetNode("/systems/electrical/outputs/DG", true);
}
void
HeadingIndicatorDG::bind ()
{
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);
}
void
HeadingIndicatorDG::unbind ()
{
std::ostringstream temp;
string branch;
temp << num;
branch = "/instrumentation/" + name + "[" + temp.str() + "]";
fgUntie((branch + "/serviceable").c_str());
fgUntie((branch + "/spin").c_str());
}
void
HeadingIndicatorDG::update (double dt)
{
// Get the spin from the gyro
_gyro.set_power_norm(_electrical_node->getDoubleValue());
_gyro.update(dt);
double spin = _gyro.get_spin_norm();
// No time-based precession for a flux gate compass
// No magvar
double offset = 0;
// TODO: movement-induced error
// Next, calculate the indicated heading,
// introducing errors.
double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
double heading = _heading_in_node->getDoubleValue();
// Now, we have to get the current
// heading and the last heading into
// the same range.
while ((heading - _last_heading_deg) > 180)
_last_heading_deg += 360;
while ((heading - _last_heading_deg) < -180)
_last_heading_deg -= 360;
heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
_last_heading_deg = heading;
heading += offset;
while (heading < 0)
heading += 360;
while (heading > 360)
heading -= 360;
_heading_out_node->setDoubleValue(heading);
// calculate the difference between the indicacted heading
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
= fgGetNode( "/autopilot/settings/heading-bug-deg", false );
if ( bnode ) {
double diff = bnode->getDoubleValue() - heading;
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
_error_node->setDoubleValue( diff );
}
// calculate the difference between the indicated heading
// and the selected nav1 radial for use with an autopilot
SGPropertyNode *nnode
= fgGetNode( "/instrumentation/nav/radials/selected-deg", false );
if ( nnode ) {
double diff = nnode->getDoubleValue() - heading;
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
_nav1_error_node->setDoubleValue( diff );
}
}
// end of heading_indicator_fg.cxx

64
src/Instrumentation/heading_indicator_dg.hxx Normal file
View file

@ -0,0 +1,64 @@
// heading_indicator.hxx - a vacuum-powered heading indicator.
// Written by David Megginson, started 2002.
//
// This file is in the Public Domain and comes with no warranty.
#ifndef __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX
#define __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX 1
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
#include "gyro.hxx"
/**
* Model an electrically-powered heading indicator.
*
* Input properties:
*
* /instrumentation/"name"/serviceable
* /instrumentation/"name"/spin
* /instrumentation/"name"/offset-deg
* /orientation/heading-deg
* /systems/electrical/outputs/DG
*
* Output properties:
*
* /instrumentation/"name"/indicated-heading-deg
*/
class HeadingIndicatorDG : public SGSubsystem
{
public:
HeadingIndicatorDG ( SGPropertyNode *node );
HeadingIndicatorDG ();
virtual ~HeadingIndicatorDG ();
virtual void init ();
virtual void bind ();
virtual void unbind ();
virtual void update (double dt);
private:
Gyro _gyro;
double _last_heading_deg;
string name;
int num;
//string vacuum_system;
SGPropertyNode_ptr _offset_node;
SGPropertyNode_ptr _heading_in_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _heading_out_node;
SGPropertyNode_ptr _electrical_node;
SGPropertyNode_ptr _error_node;
SGPropertyNode_ptr _nav1_error_node;
};
#endif // __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX

247
src/Instrumentation/mrg.cxx Normal file
View file

@ -0,0 +1,247 @@
// MRG.cxx - an electrcally powered master reference gyro.
// Written by Vivian Meazza based on wrok by David Megginson, started 2006.
//
// This file is in the Public Domain and comes with no warranty.
// TODO:
// - better spin-up
#include <simgear/compiler.h>
#include STL_IOSTREAM
#include STL_STRING
#include <sstream>
#include <math.h> // fabs()
#include <Main/fg_props.hxx>
#include <Main/util.hxx>
#include "mrg.hxx"
MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
name("master-reference-gyro"),
num(0)
{
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "name" ) {
name = cval;
} else if ( cname == "number" ) {
num = (int) child->getDoubleValue();
} else {
SG_LOG( SG_INSTR, SG_WARN, "Error in mrg config logic" );
if ( name.length() ) {
SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
}
}
}
}
MasterReferenceGyro::MasterReferenceGyro ()
{
}
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;
string branch;
branch = "/instrumentation/" + name;
_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);
_g_in_node = fgGetNode("/accelerations/pilot-g-damped", true);
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", 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);
_error_out_node = node->getChild("heading-error-deg", 0, true);
_electrical_node->setDoubleValue(0);
_responsiveness_node->setDoubleValue(0.75);
_off_node->setBoolValue(false);
}
void
MasterReferenceGyro::bind ()
{
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);
}
void
MasterReferenceGyro::unbind ()
{
std::ostringstream temp;
string branch;
temp << num;
branch = "/instrumentation/" + name + "[" + temp.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 ) {
_off_node->setBoolValue(true);
} else if ( spin == 1 ) {
_off_node->setBoolValue(false);
} else {
_off_node->setBoolValue(true);
}
// Get the input values
double roll = _roll_in_node->getDoubleValue();
double pitch = _pitch_in_node->getDoubleValue();
double hdg = _hdg_in_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 );
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 );
// 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;
//the gyro only erects inside limits
if ( abs ( yaw_rate ) <= 5
&& (_g_in_node->getDoubleValue() <= 1.5
|| _g_in_node->getDoubleValue() >= -0.5) ) {
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;
}
// calculate the difference between the indicated heading
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
= fgGetNode( "autopilot/settings/target-heading-deg", false );
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 );
}
//cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
//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_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
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 );
// store the new values
_indicated_roll = indicated_roll;
_indicated_pitch = indicated_pitch;
_indicated_hdg = indicated_hdg;
_indicated_roll_rate = indicated_roll_rate;
_indicated_pitch_rate = indicated_pitch_rate;
_indicated_hdg_rate = indicated_hdg_rate;
// 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 );
}
// end of mrg.cxx

96
src/Instrumentation/mrg.hxx Normal file
View file

@ -0,0 +1,96 @@
// attitude_indicator.hxx - a vacuum-powered attitude indicator.
// Written by David Megginson, started 2002.
//
// This file is in the Public Domain and comes with no warranty.
#ifndef __INSTRUMENTS_MRG_HXX
#define __INSTRUMENTS_MRG_HXX 1
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
#include "gyro.hxx"
/**
* Model a vacuum-powered attitude indicator.
*
* Input properties:
*
* /instrumentation/"name"/config/tumble-flag
* /instrumentation/"name"/serviceable
* /instrumentation/"name"/caged-flag
* /instrumentation/"name"/tumble-norm
* /orientation/pitch-deg
* /orientation/roll-deg
* "vacuum-system"/suction-inhg
*
* Output properties:
*
* /instrumentation/"name"/indicated-pitch-deg
* /instrumentation/"name"/indicated-roll-deg
* /instrumentation/"name"/tumble-norm
*/
class MasterReferenceGyro : public SGSubsystem
{
public:
MasterReferenceGyro ( SGPropertyNode *node );
MasterReferenceGyro ();
virtual ~MasterReferenceGyro ();
virtual void init ();
virtual void bind ();
virtual void unbind ();
virtual void update (double dt);
private:
string name;
int num;
double _last_roll;
double _last_pitch;
double _last_hdg;
double _indicated_roll;
double _indicated_pitch;
double _indicated_hdg;
double _indicated_pitch_rate;
double _indicated_roll_rate;
double _indicated_hdg_rate;
double _last_roll_rate;
double _last_pitch_rate;
double _last_yaw_rate;
Gyro _gyro;
SGPropertyNode_ptr _tumble_flag_node;
SGPropertyNode_ptr _caged_node;
SGPropertyNode_ptr _off_node;
SGPropertyNode_ptr _tumble_node;
SGPropertyNode_ptr _pitch_in_node;
SGPropertyNode_ptr _roll_in_node;
SGPropertyNode_ptr _hdg_in_node;
SGPropertyNode_ptr _g_in_node;
SGPropertyNode_ptr _electrical_node;
SGPropertyNode_ptr _pitch_int_node;
SGPropertyNode_ptr _roll_int_node;
SGPropertyNode_ptr _hdg_int_node;
SGPropertyNode_ptr _pitch_out_node;
SGPropertyNode_ptr _roll_out_node;
SGPropertyNode_ptr _hdg_out_node;
SGPropertyNode_ptr _pitch_rate_out_node;
SGPropertyNode_ptr _roll_rate_out_node;
SGPropertyNode_ptr _hdg_rate_out_node;
SGPropertyNode_ptr _error_out_node;
SGPropertyNode_ptr _yaw_rate_node;
SGPropertyNode_ptr _roll_rate_node;
SGPropertyNode_ptr _pitch_rate_node;
SGPropertyNode_ptr _responsiveness_node;
};
#endif // __INSTRUMENTS_MRG_HXX