Roy Vegard Ovesen:
I've added a vertical navigation capability to the GPS module. One can input two waypoints, wp[0] and wp[1], with altitude. If the altitudes differ, then the altitude deviation from a "straigth" line from wp[0] to wp[1] is calculated. The true course and course deviation from wp[0] to wp[1] is also calculated. All this can be found in the wp subdir where one also finds the wp[0] and wp[1] subdirs. All this has to be done through the property browser. Maybe I should make a gui window for the GPS!
This commit is contained in:
ehofman
parent
0bd25eb082
commit
c7d6fa2164
2 changed files with 547 additions and 177 deletions
|
|
@ -8,11 +8,12 @@
|
|||
#endif
|
||||
|
||||
#include <simgear/compiler.h>
|
||||
|
||||
#include <Aircraft/aircraft.hxx>
|
||||
#include <simgear/route/route.hxx>
|
||||
|
||||
#include <simgear/route/route.hxx>
|
||||
#include <simgear/math/sg_random.h>
|
||||
#include <Airports/simple.hxx>
|
||||
|
||||
#include <Main/fg_init.hxx>
|
||||
#include <Main/globals.hxx>
|
||||
#include <Main/fg_props.hxx>
|
||||
|
|
@ -47,20 +48,36 @@ GPS::init ()
|
|||
_magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
|
||||
_serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
|
||||
_electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
|
||||
_wp_longitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp-longitude-deg", true);
|
||||
_wp_latitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp-latitude-deg", true);
|
||||
_wp_ID_node =
|
||||
fgGetNode("/instrumentation/gps/wp-ID", true);
|
||||
_wp_name_node =
|
||||
fgGetNode("/instrumentation/gps/wp-name", true);
|
||||
_wp_course_node =
|
||||
fgGetNode("/instrumentation/gps/desired-course-deg", true);
|
||||
_wp0_longitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/longitude-deg", true);
|
||||
_wp0_latitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/latitude-deg", true);
|
||||
_wp0_altitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/altitude-ft", true);
|
||||
_wp0_ID_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/ID", true);
|
||||
_wp0_name_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/name", true);
|
||||
_wp0_course_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/desired-course-deg", true);
|
||||
_wp0_waypoint_type_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/waypoint-type", true);
|
||||
_wp1_longitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/longitude-deg", true);
|
||||
_wp1_latitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/latitude-deg", true);
|
||||
_wp1_altitude_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/altitude-ft", true);
|
||||
_wp1_ID_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/ID", true);
|
||||
_wp1_name_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/name", true);
|
||||
_wp1_course_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/desired-course-deg", true);
|
||||
_wp1_waypoint_type_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/waypoint-type", true);
|
||||
_get_nearest_airport_node =
|
||||
fgGetNode("/instrumentation/gps/get-nearest-airport", true);
|
||||
_waypoint_type_node =
|
||||
fgGetNode("/instrumentation/gps/waypoint-type", true);
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/get-nearest-airport", true);
|
||||
_tracking_bug_node =
|
||||
fgGetNode("/instrumentation/gps/tracking-bug", true);
|
||||
|
||||
|
|
@ -71,26 +88,42 @@ GPS::init ()
|
|||
fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
|
||||
_indicated_altitude_node =
|
||||
fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
|
||||
_indicated_vertical_speed_node =
|
||||
fgGetNode("/instrumentation/gps/indicated-vertical-speed", true);
|
||||
_true_track_node =
|
||||
fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
|
||||
_magnetic_track_node =
|
||||
fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
|
||||
_speed_node =
|
||||
fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
|
||||
_wp_distance_node =
|
||||
fgGetNode("/instrumentation/gps/wp-distance-nm", true);
|
||||
_wp_ttw_node =
|
||||
fgGetNode("/instrumentation/gps/TTW",true);
|
||||
_wp_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp-bearing-deg", true);
|
||||
_wp_mag_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp-bearing-mag-deg", true);
|
||||
_wp_course_deviation_node =
|
||||
fgGetNode("/instrumentation/gps/course-deviation-deg", true);
|
||||
_wp_course_error_nm_node =
|
||||
fgGetNode("/instrumentation/gps/course-error-nm", true);
|
||||
_wp_to_flag_node =
|
||||
fgGetNode("/instrumentation/gps/to-flag", true);
|
||||
_wp0_distance_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/distance-nm", true);
|
||||
_wp0_ttw_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/TTW",true);
|
||||
_wp0_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-true-deg", true);
|
||||
_wp0_mag_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-mag-deg", true);
|
||||
_wp0_course_deviation_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/course-deviation-deg", true);
|
||||
_wp0_course_error_nm_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/course-error-nm", true);
|
||||
_wp0_to_flag_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/to-flag", true);
|
||||
_wp1_distance_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/distance-nm", true);
|
||||
_wp1_ttw_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/TTW",true);
|
||||
_wp1_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-true-deg", true);
|
||||
_wp1_mag_bearing_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-mag-deg", true);
|
||||
_wp1_course_deviation_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/course-deviation-deg", true);
|
||||
_wp1_course_error_nm_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
|
||||
_wp1_to_flag_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/to-flag", true);
|
||||
_odometer_node =
|
||||
fgGetNode("/instrumentation/gps/odometer", true);
|
||||
_trip_odometer_node =
|
||||
|
|
@ -99,7 +132,30 @@ GPS::init ()
|
|||
fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
|
||||
_magnetic_bug_error_node =
|
||||
fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
|
||||
|
||||
_true_wp0_bearing_error_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/true-bearing-error-deg", true);
|
||||
_magnetic_wp0_bearing_error_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[0]/magnetic-bearing-error-deg", true);
|
||||
_true_wp1_bearing_error_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/true-bearing-error-deg", true);
|
||||
_magnetic_wp1_bearing_error_node =
|
||||
fgGetNode("/instrumentation/gps/wp/wp[1]/magnetic-bearing-error-deg", true);
|
||||
_leg_distance_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-distance-nm", true);
|
||||
_leg_course_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-true-course-deg", true);
|
||||
_leg_magnetic_course_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-mag-course-deg", true);
|
||||
_alt_dist_ratio_node =
|
||||
fgGetNode("/instrumentation/gps/wp/alt-dist-ratio", true);
|
||||
_leg_course_deviation_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-course-deviation-deg", true);
|
||||
_leg_course_error_nm_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-course-error-nm", true);
|
||||
_leg_to_flag_node =
|
||||
fgGetNode("/instrumentation/gps/wp/leg-to-flag", true);
|
||||
_alt_deviation_node =
|
||||
fgGetNode("/instrumentation/gps/wp/alt-deviation-ft", true);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -117,19 +173,22 @@ GPS::update (double delta_time_sec)
|
|||
_indicated_longitude_node->setDoubleValue(0);
|
||||
_indicated_latitude_node->setDoubleValue(0);
|
||||
_indicated_altitude_node->setDoubleValue(0);
|
||||
_indicated_vertical_speed_node->setDoubleValue(0);
|
||||
_true_track_node->setDoubleValue(0);
|
||||
_magnetic_track_node->setDoubleValue(0);
|
||||
_speed_node->setDoubleValue(0);
|
||||
_wp_distance_node->setDoubleValue(0);
|
||||
_wp_bearing_node->setDoubleValue(0);
|
||||
_wp_longitude_node->setDoubleValue(0);
|
||||
_wp_latitude_node->setDoubleValue(0);
|
||||
_wp_course_node->setDoubleValue(0);
|
||||
_wp1_distance_node->setDoubleValue(0);
|
||||
_wp1_bearing_node->setDoubleValue(0);
|
||||
_wp1_longitude_node->setDoubleValue(0);
|
||||
_wp1_latitude_node->setDoubleValue(0);
|
||||
_wp1_course_node->setDoubleValue(0);
|
||||
_odometer_node->setDoubleValue(0);
|
||||
_trip_odometer_node->setDoubleValue(0);
|
||||
_tracking_bug_node->setDoubleValue(0);
|
||||
_true_bug_error_node->setDoubleValue(0);
|
||||
_magnetic_bug_error_node->setDoubleValue(0);
|
||||
_true_wp1_bearing_error_node->setDoubleValue(0);
|
||||
_magnetic_wp1_bearing_error_node->setDoubleValue(0);
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -140,7 +199,51 @@ GPS::update (double delta_time_sec)
|
|||
double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
|
||||
double magvar_deg = _magvar_node->getDoubleValue();
|
||||
|
||||
double speed_kt;
|
||||
/*
|
||||
|
||||
// Bias and random error
|
||||
double random_factor = sg_random();
|
||||
double random_error = 1.4;
|
||||
double error_radius = 5.1;
|
||||
double bias_max_radius = 5.1;
|
||||
double random_max_radius = 1.4;
|
||||
|
||||
bias_length += (random_factor-0.5) * 1.0e-3;
|
||||
if (bias_length <= 0.0) bias_length = 0.0;
|
||||
else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
|
||||
bias_angle += (random_factor-0.5) * 1.0e-3;
|
||||
if (bias_angle <= 0.0) bias_angle = 0.0;
|
||||
else if (bias_angle >= 360.0) bias_angle = 360.0;
|
||||
|
||||
double random_length = random_factor * random_max_radius;
|
||||
double random_angle = random_factor * 360.0;
|
||||
|
||||
double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
|
||||
double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
|
||||
double random_x = random_length * cos(random_angle * SG_PI / 180.0);
|
||||
double random_y = random_length * sin(random_angle * SG_PI / 180.0);
|
||||
double error_x = bias_x + random_x;
|
||||
double error_y = bias_y + random_y;
|
||||
double error_length = sqrt(error_x*error_x + error_y*error_y);
|
||||
double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
|
||||
|
||||
double lat2;
|
||||
double lon2;
|
||||
double az2;
|
||||
geo_direct_wgs_84 ( altitude_m, latitude_deg,
|
||||
longitude_deg, error_angle,
|
||||
error_length, &lat2, &lon2,
|
||||
&az2 );
|
||||
//cout << lat2 << " " << lon2 << endl;
|
||||
printf("%f %f \n", bias_length, bias_angle);
|
||||
printf("%3.7f %3.7f \n", lat2, lon2);
|
||||
printf("%f %f \n", error_length, error_angle);
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
double speed_kt, vertical_speed_mpm;
|
||||
|
||||
_raim_node->setBoolValue(true);
|
||||
_indicated_longitude_node->setDoubleValue(longitude_deg);
|
||||
|
|
@ -155,8 +258,12 @@ GPS::update (double delta_time_sec)
|
|||
&track1_deg, &track2_deg, &distance_m);
|
||||
speed_kt = ((distance_m * SG_METER_TO_NM) *
|
||||
((1 / delta_time_sec) * 3600.0));
|
||||
vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
|
||||
delta_time_sec);
|
||||
_indicated_vertical_speed_node->setDoubleValue
|
||||
(vertical_speed_mpm * SG_METER_TO_FEET);
|
||||
_true_track_node->setDoubleValue(track1_deg);
|
||||
mag_track_bearing = deg360(track1_deg - magvar_deg);
|
||||
mag_track_bearing = degrange360(track1_deg - magvar_deg);
|
||||
_magnetic_track_node->setDoubleValue(mag_track_bearing);
|
||||
speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
|
||||
_last_speed_kts = speed_kt;
|
||||
|
|
@ -167,156 +274,354 @@ GPS::update (double delta_time_sec)
|
|||
odometer = _trip_odometer_node->getDoubleValue();
|
||||
_trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
|
||||
|
||||
// Get waypoint position
|
||||
double wp_longitude_deg = _wp_longitude_node->getDoubleValue();
|
||||
double wp_latitude_deg = _wp_latitude_node->getDoubleValue();
|
||||
double wp_course_deg =
|
||||
_wp_course_node->getDoubleValue();
|
||||
double wp_distance, wp_bearing_deg, wp_course_deviation_deg,
|
||||
wp_course_error_m, wp_TTW;
|
||||
string wp_ID = _wp_ID_node->getStringValue();
|
||||
// Get waypoint 0 position
|
||||
double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
|
||||
double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
|
||||
double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
|
||||
* SG_FEET_TO_METER;
|
||||
double wp0_course_deg = _wp0_course_node->getDoubleValue();
|
||||
double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
|
||||
wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
|
||||
string wp0_ID = _wp0_ID_node->getStringValue();
|
||||
|
||||
// If the get-nearest-airport-node is true.
|
||||
// Get the nearest airport, and set it as waypoint.
|
||||
if (_get_nearest_airport_node->getBoolValue()) {
|
||||
FGAirport a;
|
||||
cout << "Airport found" << endl;
|
||||
a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
|
||||
_wp_ID_node->setStringValue(a.id.c_str());
|
||||
wp_longitude_deg = a.longitude;
|
||||
wp_latitude_deg = a.latitude;
|
||||
_wp_name_node->setStringValue(a.name.c_str());
|
||||
_get_nearest_airport_node->setBoolValue(false);
|
||||
_last_wp_ID = wp_ID = a.id.c_str();
|
||||
}
|
||||
|
||||
// If the waypoint ID has changed, try to find the new ID
|
||||
// in the airport-, fix-, nav-database.
|
||||
if ( !(_last_wp_ID == wp_ID) ) {
|
||||
string waypont_type =
|
||||
_waypoint_type_node->getStringValue();
|
||||
if (waypont_type == "airport") {
|
||||
FGAirport a;
|
||||
a = globals->get_airports()->search( wp_ID );
|
||||
if ( a.id == wp_ID ) {
|
||||
cout << "Airport found" << endl;
|
||||
wp_longitude_deg = a.longitude;
|
||||
wp_latitude_deg = a.latitude;
|
||||
_wp_name_node->setStringValue(a.name.c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "nav") {
|
||||
FGNav * n;
|
||||
if ( (n = current_navlist->findByIdent(wp_ID.c_str(),
|
||||
longitude_deg,
|
||||
latitude_deg)) != NULL) {
|
||||
cout << "Nav found" << endl;
|
||||
wp_longitude_deg = n->get_lon();
|
||||
wp_latitude_deg = n->get_lat();
|
||||
_wp_name_node->setStringValue(n->get_name().c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "fix") {
|
||||
FGFix f;
|
||||
if ( current_fixlist->query(wp_ID, &f) ) {
|
||||
cout << "Fix found" << endl;
|
||||
wp_longitude_deg = f.get_lon();
|
||||
wp_latitude_deg = f.get_lat();
|
||||
_wp_name_node->setStringValue(wp_ID.c_str());
|
||||
}
|
||||
}
|
||||
_last_wp_ID = wp_ID;
|
||||
}
|
||||
|
||||
// Find the bearing and distance to the waypoint.
|
||||
SGWayPoint wp(wp_longitude_deg, wp_latitude_deg, altitude_m);
|
||||
wp.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
|
||||
&wp_bearing_deg, &wp_distance);
|
||||
_wp_longitude_node->setDoubleValue(wp_longitude_deg);
|
||||
_wp_latitude_node->setDoubleValue(wp_latitude_deg);
|
||||
_wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM);
|
||||
_wp_bearing_node->setDoubleValue(wp_bearing_deg);
|
||||
_wp_mag_bearing_node->setDoubleValue(deg360(wp_bearing_deg - magvar_deg));
|
||||
// Get waypoint 1 position
|
||||
double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
|
||||
double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
|
||||
double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
|
||||
* SG_FEET_TO_METER;
|
||||
double wp1_course_deg = _wp1_course_node->getDoubleValue();
|
||||
double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
|
||||
wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
|
||||
string wp1_ID = _wp1_ID_node->getStringValue();
|
||||
|
||||
// Estimate time to waypoint.
|
||||
// If the get-nearest-airport-node is true.
|
||||
// Get the nearest airport, and set it as waypoint 1.
|
||||
if (_get_nearest_airport_node->getBoolValue()) {
|
||||
FGAirport a;
|
||||
//cout << "Airport found" << endl;
|
||||
a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
|
||||
_wp1_ID_node->setStringValue(a.id.c_str());
|
||||
wp1_longitude_deg = a.longitude;
|
||||
wp1_latitude_deg = a.latitude;
|
||||
_wp1_name_node->setStringValue(a.name.c_str());
|
||||
_get_nearest_airport_node->setBoolValue(false);
|
||||
_last_wp1_ID = wp1_ID = a.id.c_str();
|
||||
}
|
||||
|
||||
// If the waypoint 0 ID has changed, try to find the new ID
|
||||
// in the airport-, fix-, nav-database.
|
||||
if ( !(_last_wp0_ID == wp0_ID) ) {
|
||||
string waypont_type =
|
||||
_wp0_waypoint_type_node->getStringValue();
|
||||
if (waypont_type == "airport") {
|
||||
FGAirport a;
|
||||
a = globals->get_airports()->search( wp0_ID );
|
||||
if ( a.id == wp0_ID ) {
|
||||
//cout << "Airport found" << endl;
|
||||
wp0_longitude_deg = a.longitude;
|
||||
wp0_latitude_deg = a.latitude;
|
||||
_wp0_name_node->setStringValue(a.name.c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "nav") {
|
||||
FGNav * n;
|
||||
if ( (n = current_navlist->findByIdent(wp0_ID.c_str(),
|
||||
longitude_deg,
|
||||
latitude_deg)) != NULL) {
|
||||
//cout << "Nav found" << endl;
|
||||
wp0_longitude_deg = n->get_lon();
|
||||
wp0_latitude_deg = n->get_lat();
|
||||
_wp0_name_node->setStringValue(n->get_name().c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "fix") {
|
||||
FGFix f;
|
||||
if ( current_fixlist->query(wp0_ID, &f) ) {
|
||||
//cout << "Fix found" << endl;
|
||||
wp0_longitude_deg = f.get_lon();
|
||||
wp0_latitude_deg = f.get_lat();
|
||||
_wp0_name_node->setStringValue(wp0_ID.c_str());
|
||||
}
|
||||
}
|
||||
_last_wp0_ID = wp0_ID;
|
||||
}
|
||||
|
||||
// If the waypoint 1 ID has changed, try to find the new ID
|
||||
// in the airport-, fix-, nav-database.
|
||||
if ( !(_last_wp1_ID == wp1_ID) ) {
|
||||
string waypont_type =
|
||||
_wp1_waypoint_type_node->getStringValue();
|
||||
if (waypont_type == "airport") {
|
||||
FGAirport a;
|
||||
a = globals->get_airports()->search( wp1_ID );
|
||||
if ( a.id == wp1_ID ) {
|
||||
//cout << "Airport found" << endl;
|
||||
wp1_longitude_deg = a.longitude;
|
||||
wp1_latitude_deg = a.latitude;
|
||||
_wp1_name_node->setStringValue(a.name.c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "nav") {
|
||||
FGNav * n;
|
||||
if ( (n = current_navlist->findByIdent(wp1_ID.c_str(),
|
||||
longitude_deg,
|
||||
latitude_deg)) != NULL) {
|
||||
//cout << "Nav found" << endl;
|
||||
wp1_longitude_deg = n->get_lon();
|
||||
wp1_latitude_deg = n->get_lat();
|
||||
_wp1_name_node->setStringValue(n->get_name().c_str());
|
||||
}
|
||||
}
|
||||
else if (waypont_type == "fix") {
|
||||
FGFix f;
|
||||
if ( current_fixlist->query(wp1_ID, &f) ) {
|
||||
//cout << "Fix found" << endl;
|
||||
wp1_longitude_deg = f.get_lon();
|
||||
wp1_latitude_deg = f.get_lat();
|
||||
_wp1_name_node->setStringValue(wp1_ID.c_str());
|
||||
}
|
||||
}
|
||||
_last_wp1_ID = wp1_ID;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// If any of the two waypoints have changed
|
||||
// we need to calculate a new course between them,
|
||||
// and values for vertical navigation.
|
||||
if ( wp0_longitude_deg != _wp0_longitude_deg ||
|
||||
wp0_latitude_deg != _wp0_latitude_deg ||
|
||||
wp0_altitude_m != _wp0_altitude_m ||
|
||||
wp1_longitude_deg != _wp1_longitude_deg ||
|
||||
wp1_latitude_deg != _wp1_latitude_deg ||
|
||||
wp1_altitude_m != _wp1_altitude_m )
|
||||
{
|
||||
// Update the global variables
|
||||
_wp0_longitude_deg = wp0_longitude_deg;
|
||||
_wp0_latitude_deg = wp0_latitude_deg;
|
||||
_wp0_altitude_m = wp0_altitude_m;
|
||||
_wp1_longitude_deg = wp1_longitude_deg;
|
||||
_wp1_latitude_deg = wp1_latitude_deg;
|
||||
_wp1_altitude_m = wp1_altitude_m;
|
||||
|
||||
// Get the course and distance from wp0 to wp1
|
||||
SGWayPoint wp0(wp0_longitude_deg,
|
||||
wp0_latitude_deg, wp0_altitude_m);
|
||||
SGWayPoint wp1(wp1_longitude_deg,
|
||||
wp1_latitude_deg, wp1_altitude_m);
|
||||
|
||||
wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
|
||||
|
||||
// Get the altitude / distance ratio
|
||||
if ( distance_m > 0.0 ) {
|
||||
double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
|
||||
_alt_dist_ratio = alt_difference_m / _distance_m;
|
||||
}
|
||||
|
||||
_leg_distance_node->setDoubleValue(_distance_m);
|
||||
_leg_course_node->setDoubleValue(_course_deg);
|
||||
_alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
|
||||
}
|
||||
|
||||
|
||||
// Find the bearing and distance to waypoint 0.
|
||||
SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
|
||||
wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
|
||||
&wp0_bearing_deg, &wp0_distance);
|
||||
_wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
|
||||
_wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
|
||||
_wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
|
||||
_wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
|
||||
double wp0_mag_bearing_deg = degrange360(wp0_bearing_deg - magvar_deg);
|
||||
_wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
|
||||
wp0_bearing_error_deg = degrange180(track1_deg - wp0_bearing_deg);
|
||||
_true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
|
||||
|
||||
// Estimate time to waypoint 0.
|
||||
// The estimation does not take track into consideration,
|
||||
// so if you are going away from the waypoint the TTW will
|
||||
// increase. Makes most sense when travelling directly towards
|
||||
// the waypoint.
|
||||
if (speed_kt > 0.0 && wp_distance > 0.0) {
|
||||
wp_TTW = (wp_distance * SG_METER_TO_NM) / (speed_kt / 3600);
|
||||
if (speed_kt > 0.0 && wp0_distance > 0.0) {
|
||||
wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
|
||||
}
|
||||
else {
|
||||
wp_TTW = 0.0;
|
||||
wp0_TTW = 0.0;
|
||||
}
|
||||
unsigned int wp_TTW_seconds = (int) (wp_TTW + 0.5);
|
||||
if (wp_TTW_seconds < 356400) { // That's 99 hours
|
||||
unsigned int wp_TTW_minutes = 0;
|
||||
unsigned int wp_TTW_hours = 0;
|
||||
char wp_TTW_str[8];
|
||||
while (wp_TTW_seconds >= 3600) {
|
||||
wp_TTW_seconds -= 3600;
|
||||
wp_TTW_hours++;
|
||||
unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
|
||||
if (wp0_TTW_seconds < 356400) { // That's 99 hours
|
||||
unsigned int wp0_TTW_minutes = 0;
|
||||
unsigned int wp0_TTW_hours = 0;
|
||||
char wp0_TTW_str[9];
|
||||
while (wp0_TTW_seconds >= 3600) {
|
||||
wp0_TTW_seconds -= 3600;
|
||||
wp0_TTW_hours++;
|
||||
}
|
||||
while (wp_TTW_seconds >= 60) {
|
||||
wp_TTW_seconds -= 60;
|
||||
wp_TTW_minutes++;
|
||||
while (wp0_TTW_seconds >= 60) {
|
||||
wp0_TTW_seconds -= 60;
|
||||
wp0_TTW_minutes++;
|
||||
}
|
||||
sprintf(wp_TTW_str, "%02d:%02d:%02d",
|
||||
wp_TTW_hours, wp_TTW_minutes, wp_TTW_seconds);
|
||||
_wp_ttw_node->setStringValue(wp_TTW_str);
|
||||
snprintf(wp0_TTW_str, 8, "%02d:%02d:%02d",
|
||||
wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
|
||||
_wp0_ttw_node->setStringValue(wp0_TTW_str);
|
||||
}
|
||||
else
|
||||
_wp_ttw_node->setStringValue("--:--:--");
|
||||
_wp0_ttw_node->setStringValue("--:--:--");
|
||||
|
||||
// Course deviation is the diffenrence between the bearing
|
||||
// and the course.
|
||||
wp_course_deviation_deg = wp_bearing_deg -
|
||||
wp_course_deg;
|
||||
while (wp_course_deviation_deg < -180.0) {
|
||||
wp_course_deviation_deg += 360.0; }
|
||||
while (wp_course_deviation_deg > 180.0) {
|
||||
wp_course_deviation_deg -= 360.0; }
|
||||
wp0_course_deviation_deg = wp0_bearing_deg -
|
||||
wp0_course_deg;
|
||||
wp0_course_deviation_deg = degrange180(wp0_course_deviation_deg);
|
||||
|
||||
// If the course deviation is less than 90 degrees to either side,
|
||||
// our desired course is towards the waypoint.
|
||||
// It does not matter if we are actually moving towards or from the waypoint.
|
||||
if (fabs(wp_course_deviation_deg) < 90.0) {
|
||||
_wp_to_flag_node->setBoolValue(true); }
|
||||
// If it's more than 90 degrees the desired course is from the waypoint.
|
||||
else if (fabs(wp_course_deviation_deg) > 90.0) {
|
||||
_wp_to_flag_node->setBoolValue(false);
|
||||
// It does not matter if we are actually moving
|
||||
// towards or from the waypoint.
|
||||
if (fabs(wp0_course_deviation_deg) < 90.0) {
|
||||
_wp0_to_flag_node->setBoolValue(true); }
|
||||
// If it's more than 90 degrees the desired
|
||||
// course is from the waypoint.
|
||||
else if (fabs(wp0_course_deviation_deg) > 90.0) {
|
||||
_wp0_to_flag_node->setBoolValue(false);
|
||||
// When the course is away from the waypoint,
|
||||
// it makes sense to change the sign of the deviation.
|
||||
wp_course_deviation_deg *= -1.0;
|
||||
while (wp_course_deviation_deg < -90.0)
|
||||
wp_course_deviation_deg += 180.0;
|
||||
while (wp_course_deviation_deg > 90.0)
|
||||
wp_course_deviation_deg -= 180.0; }
|
||||
wp0_course_deviation_deg *= -1.0;
|
||||
wp0_course_deviation_deg =
|
||||
degrange(wp0_course_deviation_deg, -90.0, 90.0);
|
||||
}
|
||||
|
||||
_wp_course_deviation_node->setDoubleValue(wp_course_deviation_deg);
|
||||
_wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
|
||||
|
||||
// Cross track error.
|
||||
wp_course_error_m = sin(wp_course_deviation_deg * SG_PI / 180.0)
|
||||
* (wp_distance);
|
||||
_wp_course_error_nm_node->setDoubleValue(wp_course_error_m
|
||||
wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
|
||||
* (wp0_distance);
|
||||
_wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
|
||||
* SG_METER_TO_NM);
|
||||
|
||||
|
||||
|
||||
// Find the bearing and distance to waypoint 1.
|
||||
SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
|
||||
wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
|
||||
&wp1_bearing_deg, &wp1_distance);
|
||||
_wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
|
||||
_wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
|
||||
_wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
|
||||
_wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
|
||||
double wp1_mag_bearing_deg = degrange360(wp1_bearing_deg - magvar_deg);
|
||||
_wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
|
||||
wp1_bearing_error_deg = degrange180(track1_deg - wp1_bearing_deg);
|
||||
_true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
|
||||
|
||||
// Estimate time to waypoint 1.
|
||||
// The estimation does not take track into consideration,
|
||||
// so if you are going away from the waypoint the TTW will
|
||||
// increase. Makes most sense when travelling directly towards
|
||||
// the waypoint.
|
||||
if (speed_kt > 0.0 && wp1_distance > 0.0) {
|
||||
wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
|
||||
}
|
||||
else {
|
||||
wp1_TTW = 0.0;
|
||||
}
|
||||
unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
|
||||
if (wp1_TTW_seconds < 356400) { // That's 99 hours
|
||||
unsigned int wp1_TTW_minutes = 0;
|
||||
unsigned int wp1_TTW_hours = 0;
|
||||
char wp1_TTW_str[9];
|
||||
while (wp1_TTW_seconds >= 3600) {
|
||||
wp1_TTW_seconds -= 3600;
|
||||
wp1_TTW_hours++;
|
||||
}
|
||||
while (wp1_TTW_seconds >= 60) {
|
||||
wp1_TTW_seconds -= 60;
|
||||
wp1_TTW_minutes++;
|
||||
}
|
||||
snprintf(wp1_TTW_str, 8, "%02d:%02d:%02d",
|
||||
wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
|
||||
_wp1_ttw_node->setStringValue(wp1_TTW_str);
|
||||
}
|
||||
else
|
||||
_wp1_ttw_node->setStringValue("--:--:--");
|
||||
|
||||
// Course deviation is the diffenrence between the bearing
|
||||
// and the course.
|
||||
wp1_course_deviation_deg = wp1_bearing_deg -
|
||||
wp1_course_deg;
|
||||
wp1_course_deviation_deg = degrange180(wp1_course_deviation_deg);
|
||||
|
||||
// If the course deviation is less than 90 degrees to either side,
|
||||
// our desired course is towards the waypoint.
|
||||
// It does not matter if we are actually moving
|
||||
// towards or from the waypoint.
|
||||
if (fabs(wp1_course_deviation_deg) < 90.0) {
|
||||
_wp1_to_flag_node->setBoolValue(true); }
|
||||
// If it's more than 90 degrees the desired
|
||||
// course is from the waypoint.
|
||||
else if (fabs(wp1_course_deviation_deg) > 90.0) {
|
||||
_wp1_to_flag_node->setBoolValue(false);
|
||||
// When the course is away from the waypoint,
|
||||
// it makes sense to change the sign of the deviation.
|
||||
wp1_course_deviation_deg *= -1.0;
|
||||
wp1_course_deviation_deg =
|
||||
degrange(wp1_course_deviation_deg, -90.0, 90.0);
|
||||
}
|
||||
|
||||
_wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
|
||||
|
||||
// Cross track error.
|
||||
wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
|
||||
* (wp1_distance);
|
||||
_wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
|
||||
* SG_METER_TO_NM);
|
||||
|
||||
|
||||
// Leg course deviation is the diffenrence between the bearing
|
||||
// and the course.
|
||||
double course_deviation_deg = wp1_bearing_deg - _course_deg;
|
||||
course_deviation_deg = degrange180(course_deviation_deg);
|
||||
|
||||
// If the course deviation is less than 90 degrees to either side,
|
||||
// our desired course is towards the waypoint.
|
||||
// It does not matter if we are actually moving
|
||||
// towards or from the waypoint.
|
||||
if (fabs(course_deviation_deg) < 90.0) {
|
||||
_leg_to_flag_node->setBoolValue(true); }
|
||||
// If it's more than 90 degrees the desired
|
||||
// course is from the waypoint.
|
||||
else if (fabs(course_deviation_deg) > 90.0) {
|
||||
_leg_to_flag_node->setBoolValue(false);
|
||||
// When the course is away from the waypoint,
|
||||
// it makes sense to change the sign of the deviation.
|
||||
course_deviation_deg *= -1.0;
|
||||
course_deviation_deg =
|
||||
degrange(course_deviation_deg, -90.0, 90.0);
|
||||
}
|
||||
|
||||
_leg_course_deviation_node->setDoubleValue(course_deviation_deg);
|
||||
|
||||
// Cross track error.
|
||||
double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
|
||||
* (_distance_m);
|
||||
_leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
|
||||
|
||||
// Altitude deviation
|
||||
double desired_altitude_m = wp1_altitude_m
|
||||
+ wp1_distance * _alt_dist_ratio;
|
||||
double altitude_deviation_m = altitude_m - desired_altitude_m;
|
||||
_alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
|
||||
|
||||
|
||||
|
||||
// Tracking bug.
|
||||
double tracking_bug = _tracking_bug_node->getDoubleValue();
|
||||
double true_bug_error = tracking_bug - track1_deg;
|
||||
double magnetic_bug_error = tracking_bug - mag_track_bearing;
|
||||
|
||||
// Get the errors into the (-180,180) range.
|
||||
while (true_bug_error < -180.0)
|
||||
true_bug_error += 360.0;
|
||||
while (true_bug_error > 180.0)
|
||||
true_bug_error -= 360.0;
|
||||
|
||||
while (magnetic_bug_error < -180.0)
|
||||
magnetic_bug_error += 360.0;
|
||||
while (magnetic_bug_error > 180.0)
|
||||
magnetic_bug_error -= 360.0;
|
||||
true_bug_error = degrange180(true_bug_error);
|
||||
magnetic_bug_error = degrange180(magnetic_bug_error);
|
||||
|
||||
_true_bug_error_node->setDoubleValue(true_bug_error);
|
||||
_magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
|
||||
|
|
@ -333,9 +638,9 @@ GPS::update (double delta_time_sec)
|
|||
_last_altitude_m = altitude_m;
|
||||
}
|
||||
|
||||
double GPS::deg360 (double deg)
|
||||
double GPS::degrange360 (double deg)
|
||||
{
|
||||
while (deg <= 0.0) {
|
||||
while (deg < 0.0) {
|
||||
deg += 360.0; }
|
||||
while (deg > 360.0) {
|
||||
deg -= 360.0; }
|
||||
|
|
@ -343,7 +648,25 @@ double GPS::deg360 (double deg)
|
|||
return deg;
|
||||
}
|
||||
|
||||
|
||||
double GPS::degrange180 (double deg)
|
||||
{
|
||||
while (deg < -180.0)
|
||||
deg += 360.0;
|
||||
while (deg > 180.0)
|
||||
deg -= 360.0;
|
||||
|
||||
return deg;
|
||||
}
|
||||
|
||||
double GPS::degrange (double deg, double min, double max)
|
||||
{
|
||||
double span = max - min;
|
||||
|
||||
while (deg < min)
|
||||
deg += span;
|
||||
while (deg > max)
|
||||
deg -= span;
|
||||
|
||||
return deg;
|
||||
}
|
||||
// end of gps.cxx
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@
|
|||
*
|
||||
* /instrumentation/gps/wp-longitude-deg
|
||||
* /instrumentation/gps/wp-latitude-deg
|
||||
* /instrumentation/gps/wp-altitude-ft
|
||||
* /instrumentation/gps/wp-ID
|
||||
* /instrumentation/gps/wp-name
|
||||
* /instrumentation/gps/desired-course-deg
|
||||
|
|
@ -41,6 +42,7 @@
|
|||
* /instrumentation/gps/indicated-longitude-deg
|
||||
* /instrumentation/gps/indicated-latitude-deg
|
||||
* /instrumentation/gps/indicated-altitude-ft
|
||||
* /instrumentation/gps/indicated-vertical-speed-fpm
|
||||
* /instrumentation/gps/indicated-track-true-deg
|
||||
* /instrumentation/gps/indicated-track-magnetic-deg
|
||||
* /instrumentation/gps/indicated-ground-speed-kt
|
||||
|
|
@ -56,6 +58,8 @@
|
|||
* /instrumentation/gps/trip-odometer
|
||||
* /instrumentation/gps/true-bug-error-deg
|
||||
* /instrumentation/gps/magnetic-bug-error-deg
|
||||
* /instrumentation/gps/true-bearing-error-deg
|
||||
* /instrumentation/gps/magnetic-bearing-error-deg
|
||||
*/
|
||||
class GPS : public SGSubsystem
|
||||
{
|
||||
|
|
@ -73,9 +77,9 @@ private:
|
|||
void search (double frequency, double longitude_rad,
|
||||
double latitude_rad, double altitude_m);
|
||||
|
||||
double deg360 (double deg);
|
||||
double deg180 (double deg);
|
||||
double deg90 (double deg);
|
||||
double degrange360 (double deg);
|
||||
double degrange180 (double deg);
|
||||
double degrange (double deg, double min, double max);
|
||||
|
||||
SGPropertyNode_ptr _longitude_node;
|
||||
SGPropertyNode_ptr _latitude_node;
|
||||
|
|
@ -83,33 +87,61 @@ private:
|
|||
SGPropertyNode_ptr _magvar_node;
|
||||
SGPropertyNode_ptr _serviceable_node;
|
||||
SGPropertyNode_ptr _electrical_node;
|
||||
SGPropertyNode_ptr _wp_longitude_node;
|
||||
SGPropertyNode_ptr _wp_latitude_node;
|
||||
SGPropertyNode_ptr _wp_ID_node;
|
||||
SGPropertyNode_ptr _wp_name_node;
|
||||
SGPropertyNode_ptr _wp_course_node;
|
||||
SGPropertyNode_ptr _wp0_longitude_node;
|
||||
SGPropertyNode_ptr _wp0_latitude_node;
|
||||
SGPropertyNode_ptr _wp0_altitude_node;
|
||||
SGPropertyNode_ptr _wp0_ID_node;
|
||||
SGPropertyNode_ptr _wp0_name_node;
|
||||
SGPropertyNode_ptr _wp0_course_node;
|
||||
SGPropertyNode_ptr _get_nearest_airport_node;
|
||||
SGPropertyNode_ptr _waypoint_type_node;
|
||||
SGPropertyNode_ptr _wp0_waypoint_type_node;
|
||||
SGPropertyNode_ptr _wp1_longitude_node;
|
||||
SGPropertyNode_ptr _wp1_latitude_node;
|
||||
SGPropertyNode_ptr _wp1_altitude_node;
|
||||
SGPropertyNode_ptr _wp1_ID_node;
|
||||
SGPropertyNode_ptr _wp1_name_node;
|
||||
SGPropertyNode_ptr _wp1_course_node;
|
||||
SGPropertyNode_ptr _wp1_waypoint_type_node;
|
||||
SGPropertyNode_ptr _tracking_bug_node;
|
||||
|
||||
SGPropertyNode_ptr _raim_node;
|
||||
SGPropertyNode_ptr _indicated_longitude_node;
|
||||
SGPropertyNode_ptr _indicated_latitude_node;
|
||||
SGPropertyNode_ptr _indicated_altitude_node;
|
||||
SGPropertyNode_ptr _indicated_vertical_speed_node;
|
||||
SGPropertyNode_ptr _true_track_node;
|
||||
SGPropertyNode_ptr _magnetic_track_node;
|
||||
SGPropertyNode_ptr _speed_node;
|
||||
SGPropertyNode_ptr _wp_distance_node;
|
||||
SGPropertyNode_ptr _wp_ttw_node;
|
||||
SGPropertyNode_ptr _wp_bearing_node;
|
||||
SGPropertyNode_ptr _wp_mag_bearing_node;
|
||||
SGPropertyNode_ptr _wp_course_deviation_node;
|
||||
SGPropertyNode_ptr _wp_course_error_nm_node;
|
||||
SGPropertyNode_ptr _wp_to_flag_node;
|
||||
SGPropertyNode_ptr _wp0_distance_node;
|
||||
SGPropertyNode_ptr _wp0_ttw_node;
|
||||
SGPropertyNode_ptr _wp0_bearing_node;
|
||||
SGPropertyNode_ptr _wp0_mag_bearing_node;
|
||||
SGPropertyNode_ptr _wp0_course_deviation_node;
|
||||
SGPropertyNode_ptr _wp0_course_error_nm_node;
|
||||
SGPropertyNode_ptr _wp0_to_flag_node;
|
||||
SGPropertyNode_ptr _wp1_distance_node;
|
||||
SGPropertyNode_ptr _wp1_ttw_node;
|
||||
SGPropertyNode_ptr _wp1_bearing_node;
|
||||
SGPropertyNode_ptr _wp1_mag_bearing_node;
|
||||
SGPropertyNode_ptr _wp1_course_deviation_node;
|
||||
SGPropertyNode_ptr _wp1_course_error_nm_node;
|
||||
SGPropertyNode_ptr _wp1_to_flag_node;
|
||||
SGPropertyNode_ptr _odometer_node;
|
||||
SGPropertyNode_ptr _trip_odometer_node;
|
||||
SGPropertyNode_ptr _true_bug_error_node;
|
||||
SGPropertyNode_ptr _magnetic_bug_error_node;
|
||||
SGPropertyNode_ptr _true_wp0_bearing_error_node;
|
||||
SGPropertyNode_ptr _magnetic_wp0_bearing_error_node;
|
||||
SGPropertyNode_ptr _true_wp1_bearing_error_node;
|
||||
SGPropertyNode_ptr _magnetic_wp1_bearing_error_node;
|
||||
SGPropertyNode_ptr _leg_distance_node;
|
||||
SGPropertyNode_ptr _leg_course_node;
|
||||
SGPropertyNode_ptr _leg_magnetic_course_node;
|
||||
SGPropertyNode_ptr _alt_dist_ratio_node;
|
||||
SGPropertyNode_ptr _leg_course_deviation_node;
|
||||
SGPropertyNode_ptr _leg_course_error_nm_node;
|
||||
SGPropertyNode_ptr _leg_to_flag_node;
|
||||
SGPropertyNode_ptr _alt_deviation_node;
|
||||
|
||||
bool _last_valid;
|
||||
double _last_longitude_deg;
|
||||
|
|
@ -117,9 +149,24 @@ private:
|
|||
double _last_altitude_m;
|
||||
double _last_speed_kts;
|
||||
|
||||
double _wp_latitude;
|
||||
double _wp_longitude;
|
||||
string _last_wp_ID;
|
||||
double _wp0_latitude_deg;
|
||||
double _wp0_longitude_deg;
|
||||
double _wp0_altitude_m;
|
||||
double _wp1_latitude_deg;
|
||||
double _wp1_longitude_deg;
|
||||
double _wp1_altitude_m;
|
||||
string _last_wp0_ID;
|
||||
string _last_wp1_ID;
|
||||
|
||||
double _alt_dist_ratio;
|
||||
double _distance_m;
|
||||
double _course_deg;
|
||||
|
||||
double bias_length;
|
||||
double bias_angle;
|
||||
double azimuth_error;
|
||||
double range_error;
|
||||
double elapsed_time;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue