Renamed /velocities/side-slip-rad to /orientation/side-slip-rad.
Renamed /velocities/side-slip-deg to /orientation/side-slip-deg. Renamed /velocities/alpha-deg to /orientation/alpha-deg. Added /accelerations/ned/north-accel-fps_sec. Added /accelerations/ned/east-accel-fps_sec. Added /accelerations/ned/down-accel-fps_sec. Renamed /accelerations/pilot/x-accel-ft_sec2 to /accelerations/pilot/x-accel-fps_sec. Renamed /accelerations/pilot/y-accel-ft_sec2 to /accelerations/pilot/y-accel-fps_sec. Renamed /accelerations/pilot/z-accel-ft_sec2 to /accelerations/pilot/z-accel-fps_sec.
This commit is contained in:
david
parent
fe45f5b7a8
commit
acbb9f5aff
3 changed files with 202 additions and 6 deletions
|
|
@ -391,21 +391,29 @@ FGInterface::bind ()
|
||||||
fgTie("/velocities/glideslope", this,
|
fgTie("/velocities/glideslope", this,
|
||||||
&FGInterface::get_Gamma_vert_rad,
|
&FGInterface::get_Gamma_vert_rad,
|
||||||
&FGInterface::set_Gamma_vert_rad );
|
&FGInterface::set_Gamma_vert_rad );
|
||||||
fgTie("/velocities/side-slip-rad", this,
|
fgTie("/orientation/side-slip-rad", this,
|
||||||
&FGInterface::get_Beta); // read-only
|
&FGInterface::get_Beta); // read-only
|
||||||
fgTie("/velocities/side-slip-deg", this,
|
fgTie("/orientation/side-slip-deg", this,
|
||||||
&FGInterface::get_Beta_deg); // read-only
|
&FGInterface::get_Beta_deg); // read-only
|
||||||
fgTie("/velocities/alpha-deg", this,
|
fgTie("/orientation/alpha-deg", this,
|
||||||
&FGInterface::get_Alpha_deg); // read-only
|
&FGInterface::get_Alpha_deg); // read-only
|
||||||
fgTie("/accelerations/nlf", this,
|
fgTie("/accelerations/nlf", this,
|
||||||
&FGInterface::get_Nlf); // read-only
|
&FGInterface::get_Nlf); // read-only
|
||||||
|
|
||||||
|
// NED accelerations
|
||||||
|
fgTie("/accelerations/ned/north-accel-fps_sec",
|
||||||
|
this, &FGInterface::get_V_dot_north);
|
||||||
|
fgTie("/accelerations/ned/east-accel-fps_sec",
|
||||||
|
this, &FGInterface::get_V_dot_east);
|
||||||
|
fgTie("/accelerations/ned/down-accel-fps_sec",
|
||||||
|
this, &FGInterface::get_V_dot_down);
|
||||||
|
|
||||||
// Pilot accelerations
|
// Pilot accelerations
|
||||||
fgTie("/accelerations/pilot/x-accel-ft_sec2",
|
fgTie("/accelerations/pilot/x-accel-fps_sec",
|
||||||
this, &FGInterface::get_A_X_pilot);
|
this, &FGInterface::get_A_X_pilot);
|
||||||
fgTie("/accelerations/pilot/y-accel-ft_sec2",
|
fgTie("/accelerations/pilot/y-accel-fps_sec",
|
||||||
this, &FGInterface::get_A_Y_pilot);
|
this, &FGInterface::get_A_Y_pilot);
|
||||||
fgTie("/accelerations/pilot/z-accel-ft_sec2",
|
fgTie("/accelerations/pilot/z-accel-fps_sec",
|
||||||
this, &FGInterface::get_A_Z_pilot);
|
this, &FGInterface::get_A_Z_pilot);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
122
src/Instrumentation/mag_compass.cxx
Normal file
122
src/Instrumentation/mag_compass.cxx
Normal file
|
|
@ -0,0 +1,122 @@
|
||||||
|
// mag_compass.cxx - a magnetic compass.
|
||||||
|
// Written by David Megginson, started 2003.
|
||||||
|
//
|
||||||
|
// This file is in the Public Domain and comes with no warranty.
|
||||||
|
|
||||||
|
// This implementation is derived from an earlier one by Alex Perry,
|
||||||
|
// which appeared in src/Cockpit/steam.cxx
|
||||||
|
|
||||||
|
#ifdef HAVE_CONFIG_H
|
||||||
|
# include <config.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <plib/sg.h>
|
||||||
|
|
||||||
|
#include "mag_compass.hxx"
|
||||||
|
#include <Main/fg_props.hxx>
|
||||||
|
#include <Main/util.hxx>
|
||||||
|
|
||||||
|
|
||||||
|
MagCompass::MagCompass ()
|
||||||
|
: _error_deg(0.0),
|
||||||
|
_rate_degps(0.0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
MagCompass::~MagCompass ()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
MagCompass::init ()
|
||||||
|
{
|
||||||
|
_serviceable_node =
|
||||||
|
fgGetNode("/instrumentation/magnetic-compass/serviceable", true);
|
||||||
|
_heading_node =
|
||||||
|
fgGetNode("/orientation/heading-deg", true);
|
||||||
|
_beta_node =
|
||||||
|
fgGetNode("/orientation/side-slip-deg", true);
|
||||||
|
_variation_node =
|
||||||
|
fgGetNode("/environment/magnetic-variation-deg", true);
|
||||||
|
_dip_node =
|
||||||
|
fgGetNode("/environment/magnetic-dip-deg", true);
|
||||||
|
_north_accel_node =
|
||||||
|
fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
|
||||||
|
_east_accel_node =
|
||||||
|
fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
|
||||||
|
_down_accel_node =
|
||||||
|
fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
|
||||||
|
_out_node =
|
||||||
|
fgGetNode("/instrumentation/magnetic-compass/indicated-heading-deg",
|
||||||
|
true);
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
MagCompass::update (double delta_time_sec)
|
||||||
|
{
|
||||||
|
// algorithm from Alex Perry
|
||||||
|
// possibly broken by David Megginson
|
||||||
|
|
||||||
|
// don't update if it's broken
|
||||||
|
if (!_serviceable_node->getBoolValue())
|
||||||
|
return;
|
||||||
|
|
||||||
|
// jam on a sideslip of 12 degrees or more
|
||||||
|
if (fabs(_beta_node->getDoubleValue()) > 12.0) {
|
||||||
|
_rate_degps = 0.0;
|
||||||
|
_error_deg = _heading_node->getDoubleValue() -
|
||||||
|
_out_node->getDoubleValue();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
double accelN = _north_accel_node->getDoubleValue();
|
||||||
|
double accelE = _east_accel_node->getDoubleValue();
|
||||||
|
double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
|
||||||
|
|
||||||
|
// force vector towards magnetic north pole
|
||||||
|
double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
|
||||||
|
double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
|
||||||
|
double cosdip = cos(dip);
|
||||||
|
double forceN = cosdip * cos(var);
|
||||||
|
double forceE = cosdip * sin(var);
|
||||||
|
double forceU = sin(dip);
|
||||||
|
|
||||||
|
// rotation is around acceleration axis
|
||||||
|
// (magnitude doesn't matter)
|
||||||
|
double accel = accelN * accelN + accelE * accelE + accelU * accelU;
|
||||||
|
if (accel > 1.0)
|
||||||
|
accel = sqrt(accel);
|
||||||
|
else
|
||||||
|
accel = 1.0;
|
||||||
|
|
||||||
|
// North marking on compass card
|
||||||
|
double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
|
||||||
|
double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
|
||||||
|
double edgeU = 0.0;
|
||||||
|
|
||||||
|
// apply the force to that edge to get torques
|
||||||
|
double torqueN = edgeE * forceU - edgeU * forceE;
|
||||||
|
double torqueE = edgeU * forceN - edgeN * forceU;
|
||||||
|
double torqueU = edgeN * forceE - edgeE * forceN;
|
||||||
|
|
||||||
|
// get the component parallel to the axis
|
||||||
|
double torque = (torqueN * accelN +
|
||||||
|
torqueE * accelE +
|
||||||
|
torqueU * accelU) * 5.0 / accel;
|
||||||
|
|
||||||
|
// the compass has angular momentum,
|
||||||
|
// so apply a torque and wait
|
||||||
|
if (delta_time_sec < 1.0) {
|
||||||
|
_rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
|
||||||
|
_error_deg += delta_time_sec * _rate_degps;
|
||||||
|
}
|
||||||
|
if (_error_deg > 180.0)
|
||||||
|
_error_deg -= 360.0;
|
||||||
|
else if (_error_deg < -180.0)
|
||||||
|
_error_deg += 360.0;
|
||||||
|
|
||||||
|
// Set the indicated heading
|
||||||
|
_out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
|
||||||
|
}
|
||||||
|
|
||||||
|
// end of altimeter.cxx
|
||||||
66
src/Instrumentation/mag_compass.hxx
Normal file
66
src/Instrumentation/mag_compass.hxx
Normal file
|
|
@ -0,0 +1,66 @@
|
||||||
|
// mag_compass.hxx - an altimeter tied to the static port.
|
||||||
|
// Written by David Megginson, started 2002.
|
||||||
|
//
|
||||||
|
// This file is in the Public Domain and comes with no warranty.
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef __INSTRUMENTS_MAG_COMPASS_HXX
|
||||||
|
#define __INSTRUMENTS_MAG_COMPASS_HXX 1
|
||||||
|
|
||||||
|
#ifndef __cplusplus
|
||||||
|
# error This library requires C++
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <simgear/misc/props.hxx>
|
||||||
|
#include <Main/fgfs.hxx>
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Model a magnetic compass.
|
||||||
|
*
|
||||||
|
* Input properties:
|
||||||
|
*
|
||||||
|
* /instrumentation/magnetic-compass/serviceable
|
||||||
|
* /orientation/heading-deg
|
||||||
|
* /orientation/beta-deg
|
||||||
|
* /environment/magnetic-variation-deg
|
||||||
|
* /environment/magnetic-dip-deg
|
||||||
|
* /accelerations/ned/north-accel-fps_sec
|
||||||
|
* /accelerations/ned/east-accel-fps_sec
|
||||||
|
* /accelerations/ned/down-accel-fps_sec
|
||||||
|
*
|
||||||
|
* Output properties:
|
||||||
|
*
|
||||||
|
* /instrumentation/magnetic-compass/indicated-heading-deg
|
||||||
|
*/
|
||||||
|
class MagCompass : public FGSubsystem
|
||||||
|
{
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
MagCompass ();
|
||||||
|
virtual ~MagCompass ();
|
||||||
|
|
||||||
|
virtual void init ();
|
||||||
|
virtual void update (double dt);
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
double _error_deg;
|
||||||
|
double _rate_degps;
|
||||||
|
|
||||||
|
SGPropertyNode_ptr _serviceable_node;
|
||||||
|
SGPropertyNode_ptr _heading_node;
|
||||||
|
SGPropertyNode_ptr _beta_node;
|
||||||
|
SGPropertyNode_ptr _variation_node;
|
||||||
|
SGPropertyNode_ptr _dip_node;
|
||||||
|
SGPropertyNode_ptr _y_accel_node;
|
||||||
|
SGPropertyNode_ptr _z_accel_node;
|
||||||
|
SGPropertyNode_ptr _north_accel_node;
|
||||||
|
SGPropertyNode_ptr _east_accel_node;
|
||||||
|
SGPropertyNode_ptr _down_accel_node;
|
||||||
|
SGPropertyNode_ptr _out_node;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // __INSTRUMENTS_MAG_COMPASS_HXX
|
||||||
Loading…
Reference in a new issue