131 lines
3.7 KiB
C++
131 lines
3.7 KiB
C++
// encoder.cxx -- class to impliment an altitude encoder
|
|
//
|
|
// Written by Roy Vegard Ovesen, started September 2004.
|
|
//
|
|
// Copyright (C) 2004 Roy Vegard Ovesen - rvovesen@tiscali.no
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful, but
|
|
// WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
# include <config.h>
|
|
#endif
|
|
|
|
#include "encoder.hxx"
|
|
|
|
|
|
// Altitude based on pressure difference from sea level.
|
|
// pressure difference inHG, altitude ft
|
|
static double altitude_data[][2] = {
|
|
{ -8.41, -8858.27 },
|
|
{ 0.00, 0.00 },
|
|
{ 3.05, 2952.76 },
|
|
{ 5.86, 5905.51 },
|
|
{ 8.41, 8858.27 },
|
|
{ 10.74, 11811.02 },
|
|
{ 12.87, 14763.78 },
|
|
{ 14.78, 17716.54 },
|
|
{ 16.55, 20669.29 },
|
|
{ 18.13, 23622.05 },
|
|
{ 19.62, 26574.80 },
|
|
{ 20.82, 29527.56 },
|
|
{ 21.96, 32480.31 },
|
|
{ 23.01, 35433.07 },
|
|
{ 23.91, 38385.83 },
|
|
{ 24.71, 41338.58 },
|
|
{ 25.40, 44291.34 },
|
|
{ 26.00, 47244.09 },
|
|
{ 26.51, 50196.85 },
|
|
{ 26.96, 53149.61 },
|
|
{ 27.35, 56102.36 },
|
|
{ 27.68, 59055.12 },
|
|
{ 27.98, 62007.87 },
|
|
{ 29.62, 100000.00 }, // just to fill it in
|
|
{ -1, -1 }
|
|
};
|
|
|
|
int round (double value, int nearest=1)
|
|
{
|
|
return ((int) (value/nearest + 0.5)) * nearest;
|
|
}
|
|
|
|
Encoder::Encoder(SGPropertyNode *node)
|
|
:
|
|
name("encoder"),
|
|
num(0),
|
|
staticPort("/systems/static"),
|
|
altitudeTable(new SGInterpTable)
|
|
{
|
|
int i;
|
|
for ( i = 0; altitude_data[i][0] != -1; i++ )
|
|
altitudeTable->addEntry(altitude_data[i][0], altitude_data[i][1]);
|
|
|
|
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 if ( cname == "static-port" ) {
|
|
staticPort = cval;
|
|
} else {
|
|
SG_LOG( SG_INSTR, SG_WARN,
|
|
"Error in encoder config logic" );
|
|
if ( name.length() ) {
|
|
SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
Encoder::~Encoder()
|
|
{
|
|
delete altitudeTable;
|
|
}
|
|
|
|
|
|
void Encoder::init()
|
|
{
|
|
string branch;
|
|
branch = "/instrumentation/" + name;
|
|
staticPort += "/pressure-inhg";
|
|
|
|
SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
|
|
// Inputs
|
|
staticPressureNode = fgGetNode(staticPort.c_str(), true);
|
|
busPowerNode = fgGetNode("/systems/electrical/outputs/encoder", true);
|
|
serviceableNode = node->getChild("serviceable", 0, true);
|
|
// Outputs
|
|
pressureAltitudeNode = node->getChild("pressure-alt-ft", 0, true);
|
|
modeCAltitudeNode = node->getChild("mode-c-alt-ft", 0, true);
|
|
}
|
|
|
|
|
|
void Encoder::update(double dt)
|
|
{
|
|
if (serviceableNode->getBoolValue())
|
|
{
|
|
double staticPressure = staticPressureNode->getDoubleValue();
|
|
double pressureAltitude =
|
|
altitudeTable->interpolate(29.92 - staticPressure);
|
|
int pressureAlt = round(pressureAltitude, 10);
|
|
int modeCAltitude = round(pressureAltitude, 100);
|
|
|
|
pressureAltitudeNode->setIntValue(pressureAlt);
|
|
modeCAltitudeNode->setIntValue(modeCAltitude);
|
|
}
|
|
}
|