← Back to branch summary

~ubuntu-branches/debian/sid/flightgear/sid

« back to all changes in this revision

Viewing changes to src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp

  • Committer: Package Import Robot
  • Author(s): Markus Wanner, Markus Wanner, Rebecca Palmer
  • Date: 2014-01-21 22:31:02 UTC
  • mfrom: (1.3.1) (15.1.2 experimental)
  • Revision ID: package-import@ubuntu.com-20140121223102-cjw7g9le25acd119
Tags: 3.0.0~git20140204+c99ea4-1
http://bugs.debian.org/724686
http://bugs.debian.org/556362
http://bugs.debian.org/698308
http://bugs.debian.org/574173
http://bugs.debian.org/738436
http://bugs.debian.org/682947
http://bugs.debian.org/713924
http://bugs.debian.org/252899
[ Markus Wanner ]
* Upload to unstable.
* Adjust B-D to allow building on kfreebsd-*. Closes: #724686.
* Add a lintian-overrides on autotools; we use cmake.
* Upstream corrected the fgfs manpage. Closes: #556362.
* Drop unnecessary man page for gl-info. Closes: #698308.
* Drop README.Linux: it's outdated to the point of uselessness.
  Closes: #574173.
* Add an upper limit of libsimgear-dev versions that flightgear can be
  built with. Closes: #738436.
* Drop the libsvn-dev dependency, neither flightgear nor simgear depend
  on libsvn, anymore. Closes: #682947.
* List icons in debian/install rather than copying around from rules.
* Update menu entry for flightgear, add one for fgcom; add .xpm icons.
  Closes: #713924.
* flightgear.desktop: add German translation
* Bump Standards-Version to 3.9.5; no changes needed.

[ Rebecca Palmer ]
* New upstream release.
* Install the icons (based on code by Saikrishna Arcot).  (Not a
  complete fix for LP908153 as it only sets the menu/Dash icon, not the
  running window's icon, but better than nothing).
* Disable screensaver while running. Closes: LP#793599. Add required
  libdbus-1-dev dependency.
* Remove outdated README.Debian.
* Terrasync now works after just ticking the box. Closes: #252899.
* Always set Terrasync directory.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp
 
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 Software
 
11
 Foundation; either version 2 of the License, or (at your option) any later
 
12
 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 with
 
20
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 
21
 Place - Suite 330, Boston, MA  02111-1307, USA.
 
22
 
 
23
 Further information about the GNU Lesser General Public License can also be found on
 
24
 the world wide web at http://www.gnu.org.
 
25
 
 
26
FUNCTIONAL DESCRIPTION
 
27
--------------------------------------------------------------------------------
 
28
 
 
29
HISTORY
 
30
--------------------------------------------------------------------------------
 
31
 
 
32
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
33
COMMENTS, REFERENCES,  and NOTES
 
34
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
35
 
 
36
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
37
INCLUDES
 
38
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
39
 
 
40
#include "FGWaypoint.h"
 
41
#include "input_output/FGXMLElement.h"
 
42
#include "input_output/FGPropertyManager.h"
 
43
 
 
44
using namespace std;
 
45
 
 
46
namespace JSBSim {
 
47
 
 
48
static const char *IdSrc = "$Id: FGWaypoint.cpp,v 1.2 2013/08/30 04:44:59 jberndt Exp $";
 
49
static const char *IdHdr = ID_WAYPOINT;
 
50
 
 
51
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
52
CLASS IMPLEMENTATION
 
53
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
54
 
 
55
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
56
 
 
57
FGWaypoint::FGWaypoint(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
 
58
{
 
59
  if      (Type == "WAYPOINT_HEADING")  WaypointType = eHeading;
 
60
  else if (Type == "WAYPOINT_DISTANCE") WaypointType = eDistance;
 
61
 
 
62
  target_latitude_unit = 1.0;
 
63
  target_longitude_unit = 1.0;
 
64
  source_latitude_unit = 1.0;
 
65
  source_longitude_unit = 1.0;
 
66
 
 
67
  if (element->FindElement("target_latitude") ) {
 
68
    target_latitude_pNode = PropertyManager->GetNode(element->FindElementValue("target_latitude"));
 
69
    if (element->FindElement("target_latitude")->HasAttribute("unit")) {
 
70
      if (element->FindElement("target_latitude")->GetAttributeValue("unit") == "DEG") {
 
71
        target_latitude_unit = 0.017453293;
 
72
      }
 
73
    }
 
74
  } else {
 
75
    throw("Target latitude is required for waypoint component: "+Name);
 
76
  }
 
77
 
 
78
  if (element->FindElement("target_longitude") ) {
 
79
    target_longitude_pNode = PropertyManager->GetNode(element->FindElementValue("target_longitude"));
 
80
    if (element->FindElement("target_longitude")->HasAttribute("unit")) {
 
81
      if (element->FindElement("target_longitude")->GetAttributeValue("unit") == "DEG") {
 
82
        target_longitude_unit = 0.017453293;
 
83
      }
 
84
    }
 
85
  } else {
 
86
    throw("Target longitude is required for waypoint component: "+Name);
 
87
  }
 
88
 
 
89
  if (element->FindElement("source_latitude") ) {
 
90
    source_latitude_pNode = PropertyManager->GetNode(element->FindElementValue("source_latitude"));
 
91
    if (element->FindElement("source_latitude")->HasAttribute("unit")) {
 
92
      if (element->FindElement("source_latitude")->GetAttributeValue("unit") == "DEG") {
 
93
        source_latitude_unit = 0.017453293;
 
94
      }
 
95
    }
 
96
  } else {
 
97
    throw("Source latitude is required for waypoint component: "+Name);
 
98
  }
 
99
 
 
100
  if (element->FindElement("source_longitude") ) {
 
101
    source_longitude_pNode = PropertyManager->GetNode(element->FindElementValue("source_longitude"));
 
102
    if (element->FindElement("source_longitude")->HasAttribute("unit")) {
 
103
      if (element->FindElement("source_longitude")->GetAttributeValue("unit") == "DEG") {
 
104
        source_longitude_unit = 0.017453293;
 
105
      }
 
106
    }
 
107
  } else {
 
108
    throw("Source longitude is required for waypoint component: "+Name);
 
109
  }
 
110
 
 
111
  if (element->FindElement("radius")) {
 
112
    radius = element->FindElementValueAsNumberConvertTo("radius", "FT");
 
113
  } else {
 
114
    radius = 21144000; // Radius of Earth in feet.
 
115
  }
 
116
 
 
117
  unit = element->GetAttributeValue("unit");
 
118
  if (WaypointType == eHeading) {
 
119
    if (!unit.empty()) {
 
120
    if      (unit == "DEG") eUnit = eDeg;
 
121
    else if (unit == "RAD") eUnit = eRad;
 
122
    else throw("Unknown unit "+unit+" in HEADING waypoint component, "+Name);
 
123
  } else {
 
124
      eUnit = eRad; // Default is radians if unspecified
 
125
    }
 
126
  } else {
 
127
    if (!unit.empty()) {
 
128
    if      (unit == "FT") eUnit = eFeet;
 
129
    else if (unit == "M")  eUnit = eMeters;
 
130
    else throw("Unknown unit "+unit+" in DISTANCE waypoint component, "+Name);
 
131
    } else {
 
132
      eUnit = eFeet; // Default is feet if unspecified
 
133
    }
 
134
  }
 
135
 
 
136
  FGFCSComponent::bind();
 
137
  Debug(0);
 
138
}
 
139
 
 
140
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
141
 
 
142
FGWaypoint::~FGWaypoint()
 
143
{
 
144
  Debug(1);
 
145
}
 
146
 
 
147
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
148
//  The calculations, below, implement the Haversine formulas to calculate
 
149
//  heading and distance to a set of lat/long coordinates from the current
 
150
//  position. The latitude and longitude are expected to be in radian units
 
151
//  and are measured from the 0 meridian and the equator, with positive 
 
152
//  longitude being east from there, and positive latitude being north.
 
153
//
 
154
//  The basic equations are (lat1, long1 are source positions; lat2
 
155
//  long2 are target positions):
 
156
//
 
157
//  R = earth’s radius
 
158
//  Δlat = lat2 − lat1
 
159
//  Δlong = long2 − long1
 
160
//
 
161
//  For the heading angle calculation:
 
162
//
 
163
//  θ = atan2(sin(Δlong)∙cos(lat2), cos(lat1)∙sin(lat2) − sin(lat1) ∙cos(lat2)∙cos(Δlong) )
 
164
//
 
165
//  For the waypoint distance calculation:
 
166
//
 
167
//  a = sin²(Δlat/2) + cos(lat1)∙cos(lat2)∙sin²(Δlong/2)
 
168
//  c = 2∙atan2(√a, √(1−a))
 
169
//  d = R∙c
 
170
 
 
171
bool FGWaypoint::Run(void )
 
172
{
 
173
  target_latitude = target_latitude_pNode->getDoubleValue() * target_latitude_unit;
 
174
  target_longitude = target_longitude_pNode->getDoubleValue() * target_longitude_unit;
 
175
  source_latitude = source_latitude_pNode->getDoubleValue() * source_latitude_unit;
 
176
  source_longitude = source_longitude_pNode->getDoubleValue() * source_longitude_unit;
 
177
 
 
178
  double delta_lat_rad = target_latitude  - source_latitude;
 
179
  double delta_lon_rad = target_longitude - source_longitude;
 
180
 
 
181
  if (WaypointType == eHeading) {     // Calculate Heading
 
182
 
 
183
    double Y = sin(delta_lon_rad) * cos(target_latitude);
 
184
    double X = (cos(source_latitude) * sin(target_latitude))
 
185
               - (sin(source_latitude) * cos(target_latitude) * cos(delta_lon_rad));
 
186
 
 
187
    double heading_to_waypoint_rad = atan2(Y, X);
 
188
    if (heading_to_waypoint_rad < 0) heading_to_waypoint_rad += 2.0*M_PI;
 
189
 
 
190
    double heading_to_waypoint = 0;
 
191
    if (eUnit == eDeg) heading_to_waypoint = heading_to_waypoint_rad * radtodeg;
 
192
    else               heading_to_waypoint = heading_to_waypoint_rad;
 
193
 
 
194
    Output = heading_to_waypoint;
 
195
 
 
196
  } else {                            // Calculate Distance
 
197
 
 
198
    double distance_a = pow(sin(delta_lat_rad/2.0), 2.0)
 
199
                        + (cos(source_latitude) * cos(target_latitude)
 
200
                          * (pow(sin(delta_lon_rad/2.0), 2.0)));
 
201
 
 
202
    double wp_distance = 2.0 * radius * atan2(pow(distance_a, 0.5), pow((1.0 - distance_a), 0.5));
 
203
 
 
204
    if (eUnit == eMeters) {
 
205
      Output = FeetToMeters(wp_distance);
 
206
    } else {
 
207
      Output = wp_distance;
 
208
    }
 
209
  }
 
210
 
 
211
  Clip();
 
212
  if (IsOutput) SetOutput();
 
213
 
 
214
  return true;
 
215
}
 
216
 
 
217
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
218
//    The bitmasked value choices are as follows:
 
219
//    unset: In this case (the default) JSBSim would only print
 
220
//       out the normally expected messages, essentially echoing
 
221
//       the config files as they are read. If the environment
 
222
//       variable is not set, debug_lvl is set to 1 internally
 
223
//    0: This requests JSBSim not to output any messages
 
224
//       whatsoever.
 
225
//    1: This value explicity requests the normal JSBSim
 
226
//       startup messages
 
227
//    2: This value asks for a message to be printed out when
 
228
//       a class is instantiated
 
229
//    4: When this value is set, a message is displayed when a
 
230
//       FGModel object executes its Run() method
 
231
//    8: When this value is set, various runtime state variables
 
232
//       are printed out periodically
 
233
//    16: When set various parameters are sanity checked and
 
234
//       a message is printed out when they go out of bounds
 
235
 
 
236
void FGWaypoint::Debug(int from)
 
237
{
 
238
  if (debug_lvl <= 0) return;
 
239
 
 
240
  if (debug_lvl & 1) { // Standard console startup message output
 
241
    if (from == 0) { // Constructor
 
242
    }
 
243
  }
 
244
  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 
245
    if (from == 0) cout << "Instantiated: FGWaypoint" << endl;
 
246
    if (from == 1) cout << "Destroyed:    FGWaypoint" << endl;
 
247
  }
 
248
  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 
249
  }
 
250
  if (debug_lvl & 8 ) { // Runtime state variables
 
251
  }
 
252
  if (debug_lvl & 16) { // Sanity checking
 
253
  }
 
254
  if (debug_lvl & 64) {
 
255
    if (from == 0) { // Constructor
 
256
      cout << IdSrc << endl;
 
257
      cout << IdHdr << endl;
 
258
    }
 
259
  }
 
260
}
 
261
}