From acbb9f5aff4a169a5626ab9d2bff91416f1afc88 Mon Sep 17 00:00:00 2001 From: david Date: Sun, 26 Jan 2003 15:53:35 +0000 Subject: [PATCH] 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. --- src/FDM/flight.cxx | 20 +++-- src/Instrumentation/mag_compass.cxx | 122 ++++++++++++++++++++++++++++ src/Instrumentation/mag_compass.hxx | 66 +++++++++++++++ 3 files changed, 202 insertions(+), 6 deletions(-) create mode 100644 src/Instrumentation/mag_compass.cxx create mode 100644 src/Instrumentation/mag_compass.hxx diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index a90f0791f..a36163470 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -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); } diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx new file mode 100644 index 000000000..3ef4be2ee --- /dev/null +++ b/src/Instrumentation/mag_compass.cxx @@ -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 +#endif + +#include + +#include "mag_compass.hxx" +#include
+#include
+ + +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 diff --git a/src/Instrumentation/mag_compass.hxx b/src/Instrumentation/mag_compass.hxx new file mode 100644 index 000000000..61fb00640 --- /dev/null +++ b/src/Instrumentation/mag_compass.hxx @@ -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 +#include
+ + +/** + * 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 -- 2.39.5