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.
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);
}
--- /dev/null
+// 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
--- /dev/null
+// 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
projects / flightgear.git / commitdiff