PX4 Firmware
PX4 Autopilot Software http://px4.io
geo.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file geo.h
36  *
37  * Definition of geo / math functions to perform geodesic calculations
38  *
39  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
40  * @author Julian Oes <joes@student.ethz.ch>
41  * @author Lorenz Meier <lm@inf.ethz.ch>
42  * @author Anton Babushkin <anton.babushkin@me.com>
43  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
44  */
45 
46 #pragma once
47 
48 #include <stdbool.h>
49 #include <stdint.h>
50 
51 static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
52 
53 static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f; // pascals (Pa)
54 static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f; // kilopascals (kPa)
55 static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA / 100.0f; // Millibar (mbar) (1 mbar = 100 Pa)
56 
57 static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f; // kg/m^3
58 static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f; // J/(kg * K)
59 static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f; // °C
60 
61 static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m)
62 static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m)
63 
64 static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f; // radians/second (rad/s)
65 
66 
67 // XXX remove
69  bool past_end; // Flag indicating we are past the end of the line/arc segment
70  float distance; // Distance in meters to closest point on line/arc
71  float bearing; // Bearing in radians to closest point on line/arc
72 } ;
73 
74 /* lat/lon are in radians */
76  uint64_t timestamp;
77  double lat_rad;
78  double lon_rad;
79  double sin_lat;
80  double cos_lat;
81  bool init_done;
82 };
83 
85  float alt;
86  bool init_done;
87 };
88 
89 /**
90  * Checks if global projection was initialized
91  * @return true if map was initialized before, false else
92  */
94 
95 /**
96  * Checks if projection given as argument was initialized
97  * @return true if map was initialized before, false else
98  */
100 
101 /**
102  * Get the timestamp of the global map projection
103  * @return the timestamp of the map_projection
104  */
105 uint64_t map_projection_global_timestamp(void);
106 
107 /**
108  * Get the timestamp of the map projection given by the argument
109  * @return the timestamp of the map_projection
110  */
111 uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
112 
113 /**
114  * Writes the reference values of the global projection to ref_lat and ref_lon
115  * @return 0 if map_projection_init was called before, -1 else
116  */
117 int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
118 
119 /**
120  * Writes the reference values of the projection given by the argument to ref_lat and ref_lon
121  * @return 0 if map_projection_init was called before, -1 else
122  */
123 int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
124 
125 /**
126  * Initializes the global map transformation.
127  *
128  * Initializes the transformation between the geographic coordinate system and
129  * the azimuthal equidistant plane
130  * @param lat in degrees (47.1234567°, not 471234567°)
131  * @param lon in degrees (8.1234567°, not 81234567°)
132  */
133 int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
134 
135 /**
136  * Initializes the map transformation given by the argument.
137  *
138  * Initializes the transformation between the geographic coordinate system and
139  * the azimuthal equidistant plane
140  * @param lat in degrees (47.1234567°, not 471234567°)
141  * @param lon in degrees (8.1234567°, not 81234567°)
142  */
143 int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp);
144 
145 /**
146  * Initializes the map transformation given by the argument and sets the timestamp to now.
147  *
148  * Initializes the transformation between the geographic coordinate system and
149  * the azimuthal equidistant plane
150  * @param lat in degrees (47.1234567°, not 471234567°)
151  * @param lon in degrees (8.1234567°, not 81234567°)
152  */
153 int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
154 
155 /**
156  * Transforms a point in the geographic coordinate system to the local
157  * azimuthal equidistant plane using the global projection
158  * @param x north
159  * @param y east
160  * @param lat in degrees (47.1234567°, not 471234567°)
161  * @param lon in degrees (8.1234567°, not 81234567°)
162  * @return 0 if map_projection_init was called before, -1 else
163  */
164 int map_projection_global_project(double lat, double lon, float *x, float *y);
165 
166 /* Transforms a point in the geographic coordinate system to the local
167  * azimuthal equidistant plane using the projection given by the argument
168 * @param x north
169 * @param y east
170 * @param lat in degrees (47.1234567°, not 471234567°)
171 * @param lon in degrees (8.1234567°, not 81234567°)
172 * @return 0 if map_projection_init was called before, -1 else
173 */
174 int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
175 
176 /**
177  * Transforms a point in the local azimuthal equidistant plane to the
178  * geographic coordinate system using the global projection
179  *
180  * @param x north
181  * @param y east
182  * @param lat in degrees (47.1234567°, not 471234567°)
183  * @param lon in degrees (8.1234567°, not 81234567°)
184  * @return 0 if map_projection_init was called before, -1 else
185  */
186 int map_projection_global_reproject(float x, float y, double *lat, double *lon);
187 
188 /**
189  * Transforms a point in the local azimuthal equidistant plane to the
190  * geographic coordinate system using the projection given by the argument
191  *
192  * @param x north
193  * @param y east
194  * @param lat in degrees (47.1234567°, not 471234567°)
195  * @param lon in degrees (8.1234567°, not 81234567°)
196  * @return 0 if map_projection_init was called before, -1 else
197  */
198 int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
199 
200 /**
201  * Get reference position of the global map projection
202  */
203 int map_projection_global_getref(double *lat_0, double *lon_0);
204 
205 /**
206  * Initialize the global mapping between global position (spherical) and local position (NED).
207  */
208 int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
209 
210 /**
211  * Checks if globallocalconverter was initialized
212  * @return true if map was initialized before, false else
213  */
215 
216 /**
217  * Convert from global position coordinates to local position coordinates using the global reference
218  */
219 int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
220 
221 /**
222  * Convert from local position coordinates to global position coordinates using the global reference
223  */
224 int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
225 
226 /**
227  * Get reference position of the global to local converter
228  */
229 int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
230 
231 /**
232  * Returns the distance to the next waypoint in meters.
233  *
234  * @param lat_now current position in degrees (47.1234567°, not 471234567°)
235  * @param lon_now current position in degrees (8.1234567°, not 81234567°)
236  * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
237  * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
238  */
239 float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
240 
241 /**
242  * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
243  * from waypoint A
244  *
245  * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
246  * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
247  * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
248  * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
249  * @param dist distance of target waypoint from waypoint A in meters (can be negative)
250  * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
251  * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
252  */
253 void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
254  double *lat_target, double *lon_target);
255 
256 /**
257  * Creates a waypoint from given waypoint, distance and bearing
258  * see http://www.movable-type.co.uk/scripts/latlong.html
259  *
260  * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
261  * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
262  * @param bearing in rad
263  * @param distance in meters
264  * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
265  * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
266  */
267 void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
268  double *lat_target, double *lon_target);
269 
270 /**
271  * Returns the bearing to the next waypoint in radians.
272  *
273  * @param lat_now current position in degrees (47.1234567°, not 471234567°)
274  * @param lon_now current position in degrees (8.1234567°, not 81234567°)
275  * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
276  * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
277  */
278 float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
279 
280 void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
281 
282 void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
283 
284 void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
285 
286 int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
287  double lat_start, double lon_start, double lat_end, double lon_end);
288 
289 int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
290  double lat_center, double lon_center,
291  float radius, float arc_start_bearing, float arc_sweep);
292 
293 /*
294  * Calculate distance in global frame
295  */
296 float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
297  double lat_next, double lon_next, float alt_next,
298  float *dist_xy, float *dist_z);
299 
300 /*
301  * Calculate distance in local frame (NED)
302  */
303 float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
304  float x_next, float y_next, float z_next,
305  float *dist_xy, float *dist_z);
bool map_projection_global_initialized()
Checks if global projection was initialized.
Definition: geo.cpp:63
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
Get the timestamp of the map projection given by the argument.
Definition: geo.cpp:78
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
Definition: geo.cpp:375
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the distance to the next waypoint in meters.
Definition: geo.cpp:270
static constexpr float CONSTANTS_AIR_GAS_CONST
Definition: geo.h:58
static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS
Definition: geo.h:59
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
Definition: geo.cpp:337
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
Writes the reference values of the projection given by the argument to ref_lat and ref_lon...
Definition: geo.cpp:115
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
Initializes the map transformation given by the argument.
Definition: geo.cpp:90
static constexpr double CONSTANTS_RADIUS_OF_EARTH
Definition: geo.h:61
uint64_t map_projection_global_timestamp(void)
Get the timestamp of the global map projection.
Definition: geo.cpp:73
int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp)
Initializes the global map transformation.
Definition: geo.cpp:84
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_target, double *lon_target)
Creates a waypoint from given waypoint, distance and bearing see http://www.movable-type.co.uk/scripts/latlong.html.
Definition: geo.cpp:303
void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
Definition: geo.cpp:349
int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
Convert from local position coordinates to global position coordinates using the global reference...
Definition: geo.cpp:241
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
Definition: geo.cpp:364
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
Convert from global position coordinates to local position coordinates using the global reference...
Definition: geo.cpp:229
float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, float x_next, float y_next, float z_next, float *dist_xy, float *dist_z)
Definition: geo.cpp:547
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep)
Definition: geo.cpp:419
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
float bearing
Definition: geo.h:71
uint64_t timestamp
Definition: geo.h:76
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, double lat_next, double lon_next, float alt_next, float *dist_xy, float *dist_z)
Definition: geo.cpp:523
int map_projection_global_getref(double *lat_0, double *lon_0)
Get reference position of the global map projection.
Definition: geo.cpp:194
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using...
Definition: geo.cpp:166
static constexpr float CONSTANTS_EARTH_SPIN_RATE
Definition: geo.h:64
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
Initialize the global mapping between global position (spherical) and local position (NED)...
Definition: geo.cpp:211
static constexpr float CONSTANTS_RADIUS_OF_EARTH_F
Definition: geo.h:62
static constexpr float CONSTANTS_STD_PRESSURE_KPA
Definition: geo.h:54
bool globallocalconverter_initialized(void)
Checks if globallocalconverter was initialized.
Definition: geo.cpp:224
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the bearing to the next waypoint in radians.
Definition: geo.cpp:320
int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
Writes the reference values of the global projection to ref_lat and ref_lon.
Definition: geo.cpp:110
int map_projection_global_reproject(float x, float y, double *lat, double *lon)
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using...
Definition: geo.cpp:161
static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C
Definition: geo.h:57
bool map_projection_initialized(const struct map_projection_reference_s *ref)
Checks if projection given as argument was initialized.
Definition: geo.cpp:68
float distance
Definition: geo.h:70
static constexpr float CONSTANTS_STD_PRESSURE_MBAR
Definition: geo.h:55
static constexpr float CONSTANTS_STD_PRESSURE_PA
Definition: geo.h:53
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
Initializes the map transformation given by the argument and sets the timestamp to now...
Definition: geo.cpp:105
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
Get reference position of the global to local converter.
Definition: geo.cpp:253
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
Definition: geo.cpp:132
bool past_end
Definition: geo.h:69
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target)
Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance from waypoint ...
Definition: geo.cpp:285
int map_projection_global_project(double lat, double lon, float *x, float *y)
Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane using...
Definition: geo.cpp:127