# include "config.h"
#endif
+#include "rad_alt.hxx"
+
+#include <simgear/scene/material/mat.hxx>
+
+
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
-#include "rad_alt.hxx"
+#include <Scenery/scenery.hxx>
-radAlt::radAlt(SGPropertyNode *node) : agRadar(node)
+RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
+ _time(0.0),
+ _interval(node->getDoubleValue("update-interval-sec", 1.0))
{
-
_name = node->getStringValue("name", "radar-altimeter");
_num = node->getIntValue("number", 0);
-
}
-radAlt::~radAlt()
+RadarAltimeter::~RadarAltimeter()
{
}
void
-radAlt::init ()
+RadarAltimeter::init ()
{
- agRadar::init();
+ _serviceable_node = _Instrument->getNode("serviceable", true);
+ _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
+
+ std::string branch = "/instrumentation/" + _name;
+ _Instrument = fgGetNode(branch.c_str(), _num, true);
+
_user_alt_agl_node = fgGetNode("/position/altitude-agl-ft", true);
_rad_alt_warning_node = fgGetNode("/sim/alarms/rad-alt-warning", true);
- //// those properties are used by a radar instrument of a MFD
- //// input switch = OFF | TST | STBY | ON
- //// input mode = WX | WXA | MAP | TW
- //// output status = STBY | TEST | WX | WXA | MAP | blank
- //// input lightning = true | false
- //// input TRK = +/- n degrees
- //// input TILT = +/- n degree
- //// input autotilt = true | false
- //// input range = n nm (20/40/80)
- //// input display-mode = arc | rose | map | plan
-
_Instrument->setFloatValue("tilt",-85);
_Instrument->setStringValue("status","RA");
- //_Instrument->setIntValue("mode-control", 10);
-
_Instrument->getDoubleValue("elev-limit", true);
_Instrument->getDoubleValue("elev-step-deg", true);
_Instrument->getDoubleValue("az-limit-deg", true);
_Instrument->getDoubleValue("set-height-ft", true);
_Instrument->getDoubleValue("set-excursion-percent", true);
- _Instrument->setDoubleValue("hit/brg-deg", 0);
- _Instrument->setDoubleValue("hit/range-m", 0);
- _Instrument->setStringValue("hit/material", "");
- _Instrument->setDoubleValue("hit/bumpiness", 0);
-
-
-
- _Instrument->removeChild("terrain-warning");
- _Instrument->removeChild("mode-control");
- _Instrument->removeChild("limit-deg");
- _Instrument->removeChild("reference-range-nm");
- _Instrument->removeChild("heading-marker");
- _Instrument->removeChild("display-controls");
- _Instrument->removeChild("font");
-
- //cout << " radar alt init done" << endl;
+ _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
+ _Instrument->getDoubleValue("antenna/y-offset-m"),
+ _Instrument->getDoubleValue("antenna/z-offset-m"));
}
void
-radAlt::update (double delta_time_sec)
+RadarAltimeter::update (double delta_time_sec)
{
if (!_sceneryLoaded->getBoolValue())
return;
- if ( !_odg || ! _serviceable_node->getBoolValue() ) {
+ if ( ! _serviceable_node->getBoolValue() ) {
_Instrument->setStringValue("status","");
return;
}
if (_time < _interval)
return;
- _time = 0.0;
+ _time -= _interval;
update_altitude();
+ updateSetHeight();
}
double
-radAlt::getDistanceAntennaToHit(SGVec3d nearestHit) const
+RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
{
- //calculate the distance antenna to hit
-
- SGVec3d cartantennapos = getCartAntennaPos();;
+ return norm(nearestHit - getCartAntennaPos());
+}
- SGVec3d diff = nearestHit - cartantennapos;
- double distance = norm(diff);
- return distance ;
+void
+RadarAltimeter::updateSetHeight()
+{
+ double set_ht_ft = _Instrument->getDoubleValue("set-height-ft", 9999);
+ double set_excur = _Instrument->getDoubleValue("set-excursion-percent", 0);
+ if (set_ht_ft == 9999) {
+ _rad_alt_warning_node->setIntValue(9999);
+ return;
+ }
+
+ double radarAltFt = _min_radalt * SG_METER_TO_FEET;
+ if (radarAltFt < set_ht_ft * (100 - set_excur)/100)
+ _rad_alt_warning_node->setIntValue(-1);
+ else if (radarAltFt > set_ht_ft * (100 + set_excur)/100)
+ _rad_alt_warning_node->setIntValue(1);
+ else
+ _rad_alt_warning_node->setIntValue(0);
}
void
-radAlt::update_altitude()
+RadarAltimeter::update_altitude()
{
-// int mode = _radar_mode_control_node->getIntValue();
-// double tilt = _Instrument->getDoubleValue("tilt", -85);
double el_limit = _Instrument->getDoubleValue("elev-limit", 15);
double el_step = _Instrument->getDoubleValue("elev-step-deg", 15);
double az_limit = _Instrument->getDoubleValue("az-limit-deg", 15);
double az_step = _Instrument->getDoubleValue("az-step-deg", 15);
double max_range = _Instrument->getDoubleValue("max-range-m", 1500);
double min_range = _Instrument->getDoubleValue("min-range-m", 0.001);
- double set_ht_ft = _Instrument->getDoubleValue("set-height-ft", 9999);
- double set_excur = _Instrument->getDoubleValue("set-excursion-percent", 0);
- _min_radalt = max_range;
- setUserPos();
- setAntennaPos();
+ _min_radalt = max_range;
+ bool haveHit = false;
SGVec3d cartantennapos = getCartAntennaPos();
for(double brg = -az_limit; brg <= az_limit; brg += az_step){
for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
- setUserVec(brg, elev);
+ SGVec3d userVec = rayVector(brg, elev);
SGVec3d nearestHit;
- globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
- SGGeodesy::SGCartToGeod(nearestHit, hitpos);
-
- double radalt = getDistanceAntennaToHit(nearestHit);
- double course1, course2, distance;
-
- SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
- _Instrument->setDoubleValue("hit/altitude-agl-ft",
- _user_alt_agl_node->getDoubleValue());
-
- if (radalt >= min_range && radalt <= max_range) {
- getMaterial();
-
- if (radalt < _min_radalt)
- _min_radalt = radalt;
-
- _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
- _Instrument->setDoubleValue("hit/radar-altitude-ft", radalt * SG_METER_TO_FEET);
- _Instrument->setDoubleValue("hit/brg-deg", course2);
- _Instrument->setDoubleValue("hit/range-m", distance);
- _Instrument->setStringValue("hit/material", _mat_name.c_str());
- _Instrument->setDoubleValue("hit/bumpiness", _bumpinessFactor);
-
- if (set_ht_ft!= 9999){
-
- if (_min_radalt * SG_METER_TO_FEET < set_ht_ft * (100 - set_excur)/100)
- _rad_alt_warning_node->setIntValue(-1);
- else if (_min_radalt * SG_METER_TO_FEET > set_ht_ft * (100 + set_excur)/100)
- _rad_alt_warning_node->setIntValue(1);
- else
- _rad_alt_warning_node->setIntValue(0);
-
- } else
- _rad_alt_warning_node->setIntValue(9999);
-
- } else {
- _rad_alt_warning_node->setIntValue(9999);
- _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
- _Instrument->setDoubleValue("hit/radar-altitude-ft",0);
- _Instrument->setDoubleValue("hit/brg-deg", 0);
- _Instrument->setDoubleValue("hit/range-m", 0);
- _Instrument->setStringValue("hit/material", "");
- _Instrument->setDoubleValue("hit/bumpiness", 0);
- }
-
- //cout << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
- // << " ant brg " << course2
- // << " elev " << _Instrument->getDoubleValue("tilt")
- // << " gnd rng nm " << distance * SG_METER_TO_NM
- // << " ht " << hitpos.getElevationFt()
- // << " mat " << _mat_name
- // << " solid " << _solid
- // << " bumpiness " << _bumpinessFactor
- // << endl;
-
- }
+ globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
+ double measuredDistance = dist(cartantennapos, nearestHit);
+
+ if (measuredDistance >= min_range && measuredDistance <= max_range) {
+ if (measuredDistance < _min_radalt) {
+ _min_radalt = measuredDistance;
+ haveHit = true;
+ }
+ } // of hit within permissible range
+ } // of elevation step
+ } // of azimuth step
+
+ _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
+ if (!haveHit) {
+ _rad_alt_warning_node->setIntValue(9999);
}
}
+SGVec3d
+RadarAltimeter::getCartAntennaPos() const
+{
+ double yaw, pitch, roll;
+ globals->get_aircraft_orientation(yaw, pitch, roll);
+
+ // Transform to the right coordinate frame, configuration is done in
+ // the x-forward, y-right, z-up coordinates (feet), computation
+ // in the simulation usual body x-forward, y-right, z-down coordinates
+ // (meters) )
+
+ // Transform the user position to the horizontal local coordinate system.
+ SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
+
+ // and postrotate the orientation of the user model wrt the horizontal
+ // local frame
+ hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
+
+ // The offset converted to the usual body fixed coordinate system
+ // rotated to the earth-fixed coordinates axis
+ SGVec3d ecfOffset = hlTrans.backTransform(_antennaOffset);
+
+ // Add the position offset of the user model to get the geocentered position
+ return globals->get_aircraft_position_cart() + ecfOffset;
+}
+
+SGVec3d RadarAltimeter::rayVector(double az, double el) const
+{
+ double yaw, pitch, roll;
+ globals->get_aircraft_orientation(yaw, pitch, roll);
+
+ double tilt = _Instrument->getDoubleValue("tilt");
+ bool roll_stab = false,
+ pitch_stab = false;
+
+ SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);
+
+ // Transform the antenna position to the horizontal local coordinate system.
+ SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
+
+ // and postrotate the orientation of the radar wrt the horizontal
+ // local frame
+ hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
+ pitch_stab ? 0 :pitch,
+ roll_stab ? 0 : roll);
+ hlTrans *= offset;
+
+ // now rotate the rotation vector back into the
+ // earth centered frames coordinates
+ SGVec3d angleaxis(1,0,0);
+ return hlTrans.backTransform(angleaxis);
+
+}
projects / flightgear.git / commitdiff