fly/flightgear
1
0
Fork 0
Code Activity

mag-compass: add deviation table and pitch property

Browse source
This commit is contained in:
Torsten Dreyer 2013-11-05 22:26:50 +01:00
parent 79e93918e0
commit 6a09f01ca9
2 changed files with 29 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

27
src/Instrumentation/mag_compass.cxx
View file

@ -18,13 +18,22 @@
#include "mag_compass.hxx"
MagCompass::MagCompass ( SGPropertyNode *node )
: _error_deg(0.0),
_rate_degps(0.0),
: _rate_degps(0.0),
_name(node->getStringValue("name", "magnetic-compass")),
_num(node->getIntValue("number", 0))
{
SGPropertyNode_ptr n = node->getNode( "deviation", false );
if( n ) {
SGPropertyNode_ptr deviation_table_node = n->getNode( "table", false );
if( NULL != deviation_table_node ) {
_deviation_table = new SGInterpTable( deviation_table_node );
} else {
std::string deviation_node_name = n->getStringValue();
if( false == deviation_node_name.empty() )
_deviation_node = fgGetNode( deviation_node_name, true );
}
}
}
MagCompass::~MagCompass ()
@ -39,6 +48,7 @@ MagCompass::init ()
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_serviceable_node = node->getChild("serviceable", 0, true);
_pitch_offset_node = node->getChild("pitch-offset-deg", 0, true);
_roll_node = fgGetNode("/orientation/roll-deg", true);
_pitch_node = fgGetNode("/orientation/pitch-deg", true);
_heading_node = fgGetNode("/orientation/heading-magnetic-deg", true);
@ -55,7 +65,6 @@ MagCompass::init ()
void
MagCompass::reinit ()
{
_error_deg = 0.0;
_rate_degps = 0.0;
}
@ -110,7 +119,8 @@ MagCompass::update (double delta_time_sec)
double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
// pitch angle (radians)
double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS
+ _pitch_offset_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
// magnetic heading (radians)
double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
@ -160,6 +170,13 @@ MagCompass::update (double delta_time_sec)
// This is the value that the compass
// is *trying* to display.
double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
if( _deviation_node ) {
target_deg -= _deviation_node->getDoubleValue();
} else if( _deviation_table ) {
target_deg -= _deviation_table->interpolate( SGMiscd::normalizePeriodic( 0.0, 360.0, target_deg ) );
}
double old_deg = _out_node->getDoubleValue();
while ((target_deg - old_deg) > 180.0)

8
src/Instrumentation/mag_compass.hxx
View file

@ -13,6 +13,7 @@
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/math/interpolater.hxx>
/**
@ -21,6 +22,9 @@
* Input properties:
*
* /instrumentation/"name"/serviceable
* /instrumentation/"name"/pitch-offset-deg
* /instrumentation/"name"/max-pitch-deg
* /instrumentation/"name"/max-roll-deg
* /orientation/roll-deg
* /orientation/pitch-deg
* /orientation/heading-magnetic-deg
@ -49,13 +53,15 @@ public:
private:
double _error_deg;
double _rate_degps;
std::string _name;
int _num;
SGSharedPtr<SGInterpTable> _deviation_table;
SGPropertyNode_ptr _deviation_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _pitch_offset_node;
SGPropertyNode_ptr _roll_node;
SGPropertyNode_ptr _pitch_node;
SGPropertyNode_ptr _heading_node;