//
// This file is in the Public Domain and comes with no warranty.
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
#include <simgear/compiler.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/math/sg_random.h>
_time_before_search_sec(0),
_last_frequency_khz(-1),
_transmitter_valid(false),
- _transmitter_elevation_ft(0),
+ _transmitter_pos(SGGeod::fromDeg(0, 0)),
+ _transmitter_cart(0, 0, 0),
_transmitter_range_nm(0),
_ident_count(0),
_last_ident_time(0),
} else if ( cname == "number" ) {
num = child->getIntValue();
} else {
- SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in adf config logic" );
+ SG_LOG( SG_INSTR, SG_WARN, "Error in adf config logic" );
if ( name.length() ) {
- SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name );
+ SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
}
}
}
: _time_before_search_sec(0),
_last_frequency_khz(-1),
_transmitter_valid(false),
- _transmitter_elevation_ft(0),
+ _transmitter_pos(SGGeod::fromDeg(0, 0)),
+ _transmitter_cart(0, 0, 0),
_transmitter_range_nm(0),
_ident_count(0),
_last_ident_time(0),
std::ostringstream temp;
temp << name << num;
adf_ident = temp.str();
-
- _serviceable_node->setBoolValue(true);
}
void
}
// Calculate the bearing to the transmitter
- Point3D location =
- sgGeodToCart(Point3D(longitude_rad, latitude_rad, altitude_m));
-
- double distance_nm = _transmitter.distance3D(location) * SG_METER_TO_NM;
- double range_nm = adjust_range(_transmitter_elevation_ft,
+ SGGeod geod = SGGeod::fromRadM(longitude_rad, latitude_rad, altitude_m);
+ SGVec3d location = SGVec3d::fromGeod(geod);
+
+ double distance_nm = dist(_transmitter_cart, location) * SG_METER_TO_NM;
+ double range_nm = adjust_range(_transmitter_pos.getElevationFt(),
altitude_m * SG_METER_TO_FEET,
_transmitter_range_nm);
if (distance_nm <= range_nm) {
double bearing, az2, s;
double heading = _heading_node->getDoubleValue();
- geo_inverse_wgs_84(altitude_m,
- latitude_deg,
- longitude_deg,
- _transmitter_lat_deg,
- _transmitter_lon_deg,
+ geo_inverse_wgs_84(geod, _transmitter_pos,
&bearing, &az2, &s);
_in_range_node->setBoolValue(true);
if ( _transmitter_valid ) {
ident = nav->get_trans_ident();
if ( ident != _last_ident ) {
- _transmitter_lon_deg = nav->get_lon();
- _transmitter_lat_deg = nav->get_lat();
- _transmitter = Point3D(nav->get_x(), nav->get_y(), nav->get_z());
- _transmitter_elevation_ft = nav->get_elev_ft();
+ _transmitter_pos = nav->get_pos();
+ _transmitter_cart = nav->get_cart();
_transmitter_range_nm = nav->get_range();
}
}
{
double old_bearing_deg = _bearing_node->getDoubleValue();
- bearing_deg += _error_node->getDoubleValue();
-
while ((bearing_deg - old_bearing_deg) >= 180)
old_bearing_deg += 360;
while ((bearing_deg - old_bearing_deg) <= -180)
old_bearing_deg -= 360;
-
+ bearing_deg += _error_node->getDoubleValue();
bearing_deg =
fgGetLowPass(old_bearing_deg, bearing_deg, dt * RESPONSIVENESS);