FlightGear next
FGWaypoint.cpp
Go to the documentation of this file.
1/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGWaypoint.cpp
4 Author: Jon S. Berndt
5 Date started: 6/2013
6
7 ------------- Copyright (C) 2013 Jon S. Berndt (jon@jsbsim.org) -------------
8
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free
11 Software Foundation; either version 2 of the License, or (at your option) any
12 later version.
13
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
17 details.
18
19 You should have received a copy of the GNU Lesser General Public License along
20 with this program; if not, write to the Free Software Foundation, Inc., 59
21 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
22
23 Further information about the GNU Lesser General Public License can also be
24 found on the world wide web at http://www.gnu.org.
25
26FUNCTIONAL DESCRIPTION
27--------------------------------------------------------------------------------
28
29HISTORY
30--------------------------------------------------------------------------------
31
32%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33COMMENTS, REFERENCES, and NOTES
34%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35
36%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37INCLUDES
38%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39
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"
46
47using namespace std;
48
49namespace JSBSim {
50
51/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52CLASS IMPLEMENTATION
53%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54
55//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56
57FGWaypoint::FGWaypoint(FGFCS* fcs, Element* element)
58 : FGFCSComponent(fcs, element)
59{
60 if (Type == "WAYPOINT_HEADING") WaypointType = eHeading;
61 else if (Type == "WAYPOINT_DISTANCE") WaypointType = eDistance;
62
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();
68
69 if (element->FindElement("target_latitude") ) {
70 target_latitude.reset(new FGPropertyValue(element->FindElementValue("target_latitude"),
71 PropertyManager));
72 if (element->FindElement("target_latitude")->HasAttribute("unit")) {
73 if (element->FindElement("target_latitude")->GetAttributeValue("unit") == "DEG") {
74 target_latitude_unit = 0.017453293;
75 }
76 }
77 } else {
78 cerr << element->ReadFrom() << endl
79 << "Target latitude is required for waypoint component: " << Name
80 << endl;
81 throw("Malformed waypoint definition");
82 }
83
84 if (element->FindElement("target_longitude") ) {
85 target_longitude.reset(new FGPropertyValue(element->FindElementValue("target_longitude"),
86 PropertyManager));
87 if (element->FindElement("target_longitude")->HasAttribute("unit")) {
88 if (element->FindElement("target_longitude")->GetAttributeValue("unit") == "DEG") {
89 target_longitude_unit = 0.017453293;
90 }
91 }
92 } else {
93 cerr << element->ReadFrom() << endl
94 << "Target longitude is required for waypoint component: " << Name
95 << endl;
96 throw("Malformed waypoint definition");
97 }
98
99 if (element->FindElement("source_latitude") ) {
100 source_latitude.reset(new FGPropertyValue(element->FindElementValue("source_latitude"),
101 PropertyManager));
102 if (element->FindElement("source_latitude")->HasAttribute("unit")) {
103 if (element->FindElement("source_latitude")->GetAttributeValue("unit") == "DEG") {
104 source_latitude_unit = 0.017453293;
105 }
106 }
107 } else {
108 cerr << element->ReadFrom() << endl
109 << "Source latitude is required for waypoint component: " << Name
110 << endl;
111 throw("Malformed waypoint definition");
112 }
113
114 if (element->FindElement("source_longitude") ) {
115 source_longitude.reset(new FGPropertyValue(element->FindElementValue("source_longitude"),
116 PropertyManager));
117 if (element->FindElement("source_longitude")->HasAttribute("unit")) {
118 if (element->FindElement("source_longitude")->GetAttributeValue("unit") == "DEG") {
119 source_longitude_unit = 0.017453293;
120 }
121 }
122 } else {
123 cerr << element->ReadFrom() << endl
124 << "Source longitude is required for waypoint component: " << Name
125 << endl;
126 throw("Malformed waypoint definition");
127 }
128
129 if (element->FindElement("radius"))
130 radius = element->FindElementValueAsNumberConvertTo("radius", "FT");
131 else {
132 radius = 20925646.32546; // Radius of Earth in feet.
133 }
134
135 unit = element->GetAttributeValue("unit");
136 if (WaypointType == eHeading) {
137 if (!unit.empty()) {
138 if (unit == "DEG") eUnit = eDeg;
139 else if (unit == "RAD") eUnit = eRad;
140 else {
141 cerr << element->ReadFrom() << endl
142 << "Unknown unit " << unit << " in HEADING waypoint component, "
143 << Name << endl;
144 throw("Malformed waypoint definition");
145 }
146 } else {
147 eUnit = eRad; // Default is radians if unspecified
148 }
149 } else {
150 if (!unit.empty()) {
151 if (unit == "FT") eUnit = eFeet;
152 else if (unit == "M") eUnit = eMeters;
153 else {
154 cerr << element->ReadFrom() << endl
155 << "Unknown unit " << unit << " in DISTANCE waypoint component, "
156 << Name << endl;
157 throw("Malformed waypoint definition");
158 }
159 } else {
160 eUnit = eFeet; // Default is feet if unspecified
161 }
162 }
163
164 bind(element);
165 Debug(0);
166}
167
168//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
169
170FGWaypoint::~FGWaypoint()
171{
172 Debug(1);
173}
174
175//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
176
177bool FGWaypoint::Run(void )
178{
179 double source_latitude_rad = source_latitude->GetValue() * source_latitude_unit;
180 double source_longitude_rad = source_longitude->GetValue() * source_longitude_unit;
181 double target_latitude_rad = target_latitude->GetValue() * target_latitude_unit;
182 double target_longitude_rad = target_longitude->GetValue() * target_longitude_unit;
183 source.SetPosition(source_longitude_rad, source_latitude_rad, radius);
184
185 if (fabs(target_latitude_rad) > M_PI/2.0) {
186 cerr << endl;
187 cerr << "Target latitude in waypoint \"" << Name << "\" must be less than or equal to 90 degrees." << endl;
188 cerr << "(is longitude being mistakenly supplied?)" << endl;
189 cerr << endl;
190 throw("Waypoint target latitude exceeded 90 degrees.");
191 }
192
193 if (fabs(source_latitude_rad) > M_PI/2.0) {
194 cerr << endl;
195 cerr << "Source latitude in waypoint \"" << Name << "\" must be less than or equal to 90 degrees." << endl;
196 cerr << "(is longitude being mistakenly supplied?)" << endl;
197 cerr << endl;
198 throw("Source latitude exceeded 90 degrees.");
199 }
200
201 if (WaypointType == eHeading) { // Calculate Heading
202
203 double heading_to_waypoint_rad = source.GetHeadingTo(target_longitude_rad,
204 target_latitude_rad);
205
206 if (eUnit == eDeg) Output = heading_to_waypoint_rad * radtodeg;
207 else Output = heading_to_waypoint_rad;
208
209 } else { // Calculate Distance
210
211 double wp_distance = source.GetDistanceTo(target_longitude_rad,
212 target_latitude_rad);
213 if (eUnit == eMeters) Output = FeetToMeters(wp_distance);
214 else Output = wp_distance;
215
216 }
217
218 Clip();
219 SetOutput();
220
221 return true;
222}
223
224//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
225// The bitmasked value choices are as follows:
226// unset: In this case (the default) JSBSim would only print
227// out the normally expected messages, essentially echoing
228// the config files as they are read. If the environment
229// variable is not set, debug_lvl is set to 1 internally
230// 0: This requests JSBSim not to output any messages
231// whatsoever.
232// 1: This value explicity requests the normal JSBSim
233// startup messages
234// 2: This value asks for a message to be printed out when
235// a class is instantiated
236// 4: When this value is set, a message is displayed when a
237// FGModel object executes its Run() method
238// 8: When this value is set, various runtime state variables
239// are printed out periodically
240// 16: When set various parameters are sanity checked and
241// a message is printed out when they go out of bounds
242
243void FGWaypoint::Debug(int from)
244{
245 if (debug_lvl <= 0) return;
246
247 if (debug_lvl & 1) { // Standard console startup message output
248 if (from == 0) { // Constructor
249 }
250 }
251 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
252 if (from == 0) cout << "Instantiated: FGWaypoint" << endl;
253 if (from == 1) cout << "Destroyed: FGWaypoint" << endl;
254 }
255 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
256 }
257 if (debug_lvl & 8 ) { // Runtime state variables
258 }
259 if (debug_lvl & 16) { // Sanity checking
260 }
261 if (debug_lvl & 64) {
262 if (from == 0) { // Constructor
263 }
264 }
265}
266}
M_PI
#define M_PI
Definition FGJSBBase.h:50
JSBSim::Element::FindElement
Element * FindElement(const std::string &el="")
Searches for a specified element.
Definition FGXMLElement.cpp:389
JSBSim::FGFCSComponent::FGFCSComponent
FGFCSComponent(FGFCS *fcs, Element *el)
Constructor.
Definition FGFCSComponent.cpp:52
JSBSim::FGFCSComponent::SetOutput
virtual void SetOutput(void)
Definition FGFCSComponent.cpp:239
JSBSim::FGJSBBase::radtodeg
static constexpr double radtodeg
Definition FGJSBBase.h:348
JSBSim::FGJSBBase::FeetToMeters
static constexpr double FeetToMeters(double measure)
Converts from feet to meters.
Definition FGJSBBase.h:258
JSBSim::FGWaypoint::Run
bool Run(void) override
Definition FGWaypoint.cpp:177
JSBSim::FGWaypoint::FGWaypoint
FGWaypoint(FGFCS *fcs, Element *element)
Definition FGWaypoint.cpp:57
JSBSim::debug_lvl
short debug_lvl