40#include "FGWaypoint.h"
41#include "input_output/FGXMLElement.h"
42#include "math/FGLocation.h"
43#include "models/FGFCS.h"
44#include "models/FGInertial.h"
45#include "initialization/FGInitialCondition.h"
57FGWaypoint::FGWaypoint(FGFCS* fcs, Element* element)
58 : FGFCSComponent(fcs, element)
60 if (Type ==
"WAYPOINT_HEADING") WaypointType = eHeading;
61 else if (Type ==
"WAYPOINT_DISTANCE") WaypointType = eDistance;
63 target_latitude_unit = 1.0;
64 target_longitude_unit = 1.0;
65 source_latitude_unit = 1.0;
66 source_longitude_unit = 1.0;
67 source = fcs->GetExec()->GetIC()->GetPosition();
69 auto PropertyManager = fcs->GetPropertyManager();
71 if (element->FindElement(
"target_latitude") ) {
72 target_latitude = std::make_unique<FGPropertyValue>(element->FindElementValue(
"target_latitude"),
73 PropertyManager, element);
74 if (element->FindElement(
"target_latitude")->HasAttribute(
"unit")) {
75 if (element->FindElement(
"target_latitude")->GetAttributeValue(
"unit") ==
"DEG") {
76 target_latitude_unit = 0.017453293;
80 cerr << element->ReadFrom() << endl
81 <<
"Target latitude is required for waypoint component: " << Name
83 throw(
"Malformed waypoint definition");
86 if (element->FindElement(
"target_longitude") ) {
87 target_longitude = std::make_unique<FGPropertyValue>(element->FindElementValue(
"target_longitude"),
88 PropertyManager, element);
89 if (element->FindElement(
"target_longitude")->HasAttribute(
"unit")) {
90 if (element->FindElement(
"target_longitude")->GetAttributeValue(
"unit") ==
"DEG") {
91 target_longitude_unit = 0.017453293;
95 cerr << element->ReadFrom() << endl
96 <<
"Target longitude is required for waypoint component: " << Name
98 throw(
"Malformed waypoint definition");
101 if (element->FindElement(
"source_latitude") ) {
102 source_latitude = std::make_unique<FGPropertyValue>(element->FindElementValue(
"source_latitude"),
103 PropertyManager, element);
104 if (element->FindElement(
"source_latitude")->HasAttribute(
"unit")) {
105 if (element->FindElement(
"source_latitude")->GetAttributeValue(
"unit") ==
"DEG") {
106 source_latitude_unit = 0.017453293;
110 cerr << element->ReadFrom() << endl
111 <<
"Source latitude is required for waypoint component: " << Name
113 throw(
"Malformed waypoint definition");
116 if (element->FindElement(
"source_longitude") ) {
117 source_longitude = std::make_unique<FGPropertyValue>(element->FindElementValue(
"source_longitude"),
118 PropertyManager, element);
119 if (element->FindElement(
"source_longitude")->HasAttribute(
"unit")) {
120 if (element->FindElement(
"source_longitude")->GetAttributeValue(
"unit") ==
"DEG") {
121 source_longitude_unit = 0.017453293;
125 cerr << element->ReadFrom() << endl
126 <<
"Source longitude is required for waypoint component: " << Name
128 throw(
"Malformed waypoint definition");
131 unit = element->GetAttributeValue(
"unit");
132 if (WaypointType == eHeading) {
134 if (unit ==
"DEG") eUnit = eDeg;
135 else if (unit ==
"RAD") eUnit = eRad;
137 cerr << element->ReadFrom() << endl
138 <<
"Unknown unit " << unit <<
" in HEADING waypoint component, "
140 throw(
"Malformed waypoint definition");
147 if (unit ==
"FT") eUnit = eFeet;
148 else if (unit ==
"M") eUnit = eMeters;
150 cerr << element->ReadFrom() << endl
151 <<
"Unknown unit " << unit <<
" in DISTANCE waypoint component, "
153 throw(
"Malformed waypoint definition");
160 bind(element, PropertyManager.get());
166FGWaypoint::~FGWaypoint()
173bool FGWaypoint::Run(
void )
175 double source_latitude_rad = source_latitude->GetValue() * source_latitude_unit;
176 double source_longitude_rad = source_longitude->GetValue() * source_longitude_unit;
177 double target_latitude_rad = target_latitude->GetValue() * target_latitude_unit;
178 double target_longitude_rad = target_longitude->GetValue() * target_longitude_unit;
179 source.SetPositionGeodetic(source_longitude_rad, source_latitude_rad, 0.0);
181 if (fabs(target_latitude_rad) > M_PI/2.0) {
183 cerr <<
"Target latitude in waypoint \"" << Name <<
"\" must be less than or equal to 90 degrees." << endl;
184 cerr <<
"(is longitude being mistakenly supplied?)" << endl;
186 throw(
"Waypoint target latitude exceeded 90 degrees.");
189 if (fabs(source_latitude_rad) > M_PI/2.0) {
191 cerr <<
"Source latitude in waypoint \"" << Name <<
"\" must be less than or equal to 90 degrees." << endl;
192 cerr <<
"(is longitude being mistakenly supplied?)" << endl;
194 throw(
"Source latitude exceeded 90 degrees.");
197 if (WaypointType == eHeading) {
199 double heading_to_waypoint_rad = source.GetHeadingTo(target_longitude_rad,
200 target_latitude_rad);
202 if (eUnit == eDeg) Output = heading_to_waypoint_rad * radtodeg;
203 else Output = heading_to_waypoint_rad;
207 double wp_distance = source.GetDistanceTo(target_longitude_rad,
208 target_latitude_rad);
209 if (eUnit == eMeters) Output = FeetToMeters(wp_distance);
210 else Output = wp_distance;
239void FGWaypoint::Debug(
int from)
241 if (debug_lvl <= 0)
return;
247 if (debug_lvl & 2 ) {
248 if (from == 0) cout <<
"Instantiated: FGWaypoint" << endl;
249 if (from == 1) cout <<
"Destroyed: FGWaypoint" << endl;
251 if (debug_lvl & 4 ) {
253 if (debug_lvl & 8 ) {
255 if (debug_lvl & 16) {
257 if (debug_lvl & 64) {