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,
|
||||
&FGInterface::get_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
|
||||
fgTie("/velocities/side-slip-deg", this,
|
||||
fgTie("/orientation/side-slip-deg", this,
|
||||
&FGInterface::get_Beta_deg); // read-only
|
||||
fgTie("/velocities/alpha-deg", this,
|
||||
fgTie("/orientation/alpha-deg", this,
|
||||
&FGInterface::get_Alpha_deg); // read-only
|
||||
fgTie("/accelerations/nlf", this,
|
||||
&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
|
||||
fgTie("/accelerations/pilot/x-accel-ft_sec2",
|
||||
fgTie("/accelerations/pilot/x-accel-fps_sec",
|
||||
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);
|
||||
fgTie("/accelerations/pilot/z-accel-ft_sec2",
|
||||
fgTie("/accelerations/pilot/z-accel-fps_sec",
|
||||
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