fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

move geo and geo_lookup to PX4/ecl

Browse Source
This commit is contained in:
Daniel Agar
2018-03-26 23:16:55 -04:00
parent 76bf9c6465
commit 29b3950747
87 changed files with 62 additions and 1401 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/configs/nuttx_aerocore2_default.cmake
-2
View File
@@ -112,8 +112,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_aerofc-v1_default.cmake
-2
View File
@@ -82,8 +82,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/mathlib
lib/mixer
lib/rc
cmake/configs/nuttx_auav-x21_default.cmake
-2
View File
@@ -125,8 +125,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_crazyflie_default.cmake
-2
View File
@@ -83,8 +83,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/mathlib
lib/mixer
lib/rc
cmake/configs/nuttx_mindpx-v2_default.cmake
-2
View File
@@ -129,8 +129,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_nxphlite-v3_default.cmake
-2
View File
@@ -131,8 +131,6 @@ set(config_module_list
#
lib/controllib
lib/ecl
lib/geo
lib/geo_lookup
lib/conversion
lib/led
lib/mathlib
cmake/configs/nuttx_px4-same70xplained-v1_default.cmake
-2
View File
@@ -106,8 +106,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
-1
View File
@@ -38,7 +38,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/mathlib
lib/mixer
lib/version
cmake/configs/nuttx_px4fmu-v2_default.cmake
-2
View File
@@ -153,8 +153,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4fmu-v2_test.cmake
-2
View File
@@ -125,8 +125,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4fmu-v3_default.cmake
-2
View File
@@ -142,8 +142,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4fmu-v4_default.cmake
-2
View File
@@ -129,8 +129,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
-2
View File
@@ -133,8 +133,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4fmu-v5_default.cmake
-2
View File
@@ -134,8 +134,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
-2
View File
@@ -112,8 +112,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/nuttx_tap-v1_default.cmake
-2
View File
@@ -89,8 +89,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_bebop_default.cmake
-2
View File
@@ -76,8 +76,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/mathlib
lib/mixer
lib/terrain_estimation
cmake/configs/posix_eagle_hil.cmake
-2
View File
@@ -41,8 +41,6 @@ set(config_module_list
lib/controllib
lib/mathlib
lib/ecl
lib/geo
lib/geo_lookup
lib/conversion
lib/version
lib/DriverFramework/framework
cmake/configs/posix_ocpoc_cross.cmake
-2
View File
@@ -84,8 +84,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_ocpoc_ubuntu.cmake
-2
View File
@@ -83,8 +83,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_rpi_common.cmake
-2
View File
@@ -99,8 +99,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_sdflight_default.cmake
-2
View File
@@ -73,8 +73,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_sdflight_legacy.cmake
-2
View File
@@ -65,8 +65,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/posix_sitl_default.cmake
-2
View File
@@ -123,8 +123,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mixer
lib/mathlib
cmake/configs/posix_sitl_replay.cmake
-2
View File
@@ -18,8 +18,6 @@ set(config_module_list
lib/mathlib
lib/conversion
lib/ecl
lib/geo
lib/geo_lookup
lib/version
lib/DriverFramework/framework
lib/micro-CDR
cmake/configs/qurt_eagle_hil.cmake
-2
View File
@@ -63,8 +63,6 @@ set(config_module_list
lib/controllib
lib/conversion
lib/DriverFramework/framework
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/qurt_eagle_muorb.cmake
-2
View File
@@ -38,8 +38,6 @@ set(config_module_list
# Libraries
#
lib/mathlib
lib/geo
lib/geo_lookup
lib/conversion
lib/version
lib/DriverFramework/framework
cmake/configs/qurt_eagle_travis.cmake
-2
View File
@@ -69,8 +69,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/led
lib/mathlib
lib/mixer
cmake/configs/qurt_sdflight_default.cmake
-2
View File
@@ -87,8 +87,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/mathlib
lib/mixer
lib/rc
cmake/configs/qurt_sdflight_legacy.cmake
-2
View File
@@ -86,8 +86,6 @@ set(config_module_list
lib/conversion
lib/DriverFramework/framework
lib/ecl
lib/geo
lib/geo_lookup
lib/mathlib
lib/mixer
lib/rc
src/drivers/telemetry/frsky_telemetry/frsky_data.c
+1 -1
View File
@@ -48,7 +48,7 @@
#include <string.h>
#include <stdbool.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <stdbool.h>
#include <uORB/topics/battery_status.h>
src/drivers/telemetry/frsky_telemetry/sPort_data.c
+1 -1
View File
@@ -48,7 +48,7 @@
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
src/drivers/telemetry/hott/messages.cpp
+1 -1
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
src/drivers/vmount/output.cpp
+1 -1
View File
@@ -46,7 +46,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/mount_orientation.h>
#include <px4_defines.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
src/drivers/vmount/output.h
+1 -1
View File
@@ -41,7 +41,7 @@
#include "common.h"
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
src/examples/bottle_drop/bottle_drop.cpp
+1 -1
View File
@@ -64,7 +64,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
src/examples/fixedwing_control/main.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/math.hpp>
#include <px4_config.h>
#include <px4_tasks.h>
src/examples/rover_steering_control/main.cpp
+1 -1
View File
@@ -63,7 +63,7 @@
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <matrix/math.hpp>
src/examples/segway/blocks.cpp
+1 -1
View File
@@ -38,7 +38,7 @@
*/
#include "blocks.hpp"
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
namespace control
{
src/examples/uuv_example_app/uuv_example_app.cpp
+1 -1
View File
@@ -58,7 +58,7 @@
#include <systemlib/circuit_breaker.h>
// internal libraries
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
// Include uORB and the required topics for this app
#include <uORB/uORB.h>
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 95c4777b35...8caf52fb41
src/lib/geo/CMakeLists.txt
-40
View File
@@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE lib__geo
SRCS
geo.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/lib/geo/geo.c
-705
View File
@@ -1,705 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geo.c
*
* Geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
#include <px4_config.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
__EXPORT bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
return ref->init_done;
}
__EXPORT uint64_t map_projection_global_timestamp()
{
return map_projection_timestamp(&mp_ref);
}
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
return ref->timestamp;
}
__EXPORT int map_projection_global_init(double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
}
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
ref->lon_rad = lon_0 * M_DEG_TO_RAD;
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0,
double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon;
if (arg > 1.0) {
arg = 1.0;
} else if (arg < -1.0) {
arg = -1.0;
}
double c = acos(arg);
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
return map_projection_reproject(&mp_ref, x, y, lat, lon);
}
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = (double)y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (lat_0 != NULL) {
*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
}
if (lon_0 != NULL) {
*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
}
return 0;
}
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
gl_ref.alt = alt_0;
if (!map_projection_global_init(lat_0, lon_0, timestamp)) {
gl_ref.init_done = true;
return 0;
} else {
gl_ref.init_done = false;
return -1;
}
}
__EXPORT bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_project(lat, lon, x, y);
*z = gl_ref.alt - alt;
return 0;
}
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_reproject(x, y, lat, lon);
*alt = gl_ref.alt - z;
return 0;
}
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (map_projection_global_getref(lat_0, lon_0)) {
return -1;
}
if (alt_0 != NULL) {
*alt_0 = gl_ref.alt;
}
return 0;
}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / (double)180.0 * M_PI;
double lon_now_rad = lon_now / (double)180.0 * M_PI;
double lat_next_rad = lat_next / (double)180.0 * M_PI;
double lon_next_rad = lon_next / (double)180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon /
(double)2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a));
return CONSTANTS_RADIUS_OF_EARTH * c;
}
__EXPORT 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)
{
if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A;
*lon_target = lon_A;
} else if (dist >= FLT_EPSILON) {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
} else {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
heading = _wrap_2pi(heading + M_PI_F);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
}
}
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH;
double lat_start_rad = lat_start * M_DEG_TO_RAD;
double lon_start_rad = lon_start * M_DEG_TO_RAD;
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target *= M_RAD_TO_DEG;
*lon_target *= M_RAD_TO_DEG;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad),
cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
theta = _wrap_pi(theta);
return theta;
}
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(
d_lon));
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
__EXPORT 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)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
}
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
__EXPORT 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)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
// headed towards the end point.
float dist_to_end;
float bearing_end;
float bearing_track;
float bearing_diff;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (dist_to_end < 0.1f) {
return ERROR;
}
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
bearing_diff = bearing_track - bearing_end;
bearing_diff = _wrap_pi(bearing_diff);
// Return past_end = true if past end point of line
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
crosstrack_error->past_end = true;
return_value = OK;
return return_value;
}
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sinf(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
} else {
crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
}
return_value = OK;
return return_value;
}
__EXPORT 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)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
// headed towards the end point.
// Determine if the current position is inside or outside the sector between the line from the center
// to the arc start and the line from the center to the arc end
float bearing_sector_start;
float bearing_sector_end;
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
bool in_sector;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (radius < 0.1f) { return return_value; }
if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
}
in_sector = false;
// Case where sector does not span zero
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start
&& bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start
|| bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point
if (in_sector) {
crosstrack_error->past_end = false;
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
if (dist_to_center <= radius) {
crosstrack_error->distance = radius - dist_to_center;
crosstrack_error->bearing = bearing_now + M_PI_F;
} else {
crosstrack_error->distance = dist_to_center - radius;
crosstrack_error->bearing = bearing_now;
}
// If out of the sector then calculate dist and bearing to start or end point
} else {
// Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
// calculate the position of the start and end points. We should not be doing this often
// as this function generally will not be called repeatedly when we are out of the sector.
double start_disp_x = (double)radius * sin((double)arc_start_bearing);
double start_disp_y = (double)radius * cos((double)arc_start_bearing);
double end_disp_x = (double)radius * sin((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
if (dist_to_start < dist_to_end) {
crosstrack_error->distance = dist_to_start;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
} else {
crosstrack_error->past_end = true;
crosstrack_error->distance = dist_to_end;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
}
}
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
__EXPORT 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)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
double x_rad = lat_now / 180.0 * M_PI;
double y_rad = lon_now / 180.0 * M_PI;
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
float dz = alt_now - alt_next;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
}
__EXPORT 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)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
float dz = z_now - z_next;
*dist_xy = sqrtf(dx * dx + dy * dy);
*dist_z = fabsf(dz);
return sqrtf(dx * dx + dy * dy + dz * dz);
}
__EXPORT float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_180(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 180.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -180.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
__EXPORT float _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
src/lib/geo/geo.h
-315
View File
@@ -1,315 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geo.h
*
* Definition of geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
#pragma once
#include <platforms/px4_defines.h>
#include "geo_lookup/geo_mag_declination.h"
#include <stdbool.h>
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */
#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
// XXX remove
struct crosstrack_error_s {
bool past_end; // Flag indicating we are past the end of the line/arc segment
float distance; // Distance in meters to closest point on line/arc
float bearing; // Bearing in radians to closest point on line/arc
} ;
/* lat/lon are in radians */
struct map_projection_reference_s {
uint64_t timestamp;
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
};
struct globallocal_converter_reference_s {
float alt;
bool init_done;
};
__BEGIN_DECLS
/**
* Checks if global projection was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_global_initialized(void);
/**
* Checks if projection given as argument was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
/**
* Get the timestamp of the global map projection
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_global_timestamp(void);
/**
* Get the timestamp of the map projection given by the argument
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
/**
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_global_reference(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
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad);
/**
* Initializes the global map transformation.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT 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 and sets the timestamp to now.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT 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 the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the global projection
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT 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 the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon);
/**
* Get reference position of the global map projection
*/
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
/**
* Checks if globallocalconverter was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool globallocalconverter_initialized(void);
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
/**
* Convert from local position coordinates to global position coordinates using the global reference
*/
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Get reference position of the global to local converter
*/
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
/**
* Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
* from waypoint A
*
* @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
* @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
* @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
* @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
* @param dist distance of target waypoint from waypoint A in meters (can be negative)
* @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
*/
__EXPORT 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 waypoint from given waypoint, distance and bearing
* see http://www.movable-type.co.uk/scripts/latlong.html
*
* @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
* @param bearing in rad
* @param distance in meters
* @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target);
/**
* Returns the bearing to the next waypoint in radians.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e);
__EXPORT 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);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res);
__EXPORT 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);
__EXPORT 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);
/*
* Calculate distance in global frame
*/
__EXPORT 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);
/*
* Calculate distance in local frame (NED)
*/
__EXPORT 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);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
__END_DECLS
src/lib/geo_lookup/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE lib__geo_lookup
COMPILE_FLAGS
SRCS
geo_mag_declination.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/lib/geo_lookup/geo_mag_declination.c
-135
View File
@@ -1,135 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name MAVGEO nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geo_mag_declination.c
*
* Calculation / lookup table for Earth's magnetic field declination.
*
* Lookup table from Scott Ferguson <scottfromscott@gmail.com> and
* Stephan Brown <stephan.brown.07@gmail.com>
*
* XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term.
*
*/
#include <stdint.h>
#include "geo_mag_declination.h"
/** set this always to the sampling in degrees for the table below */
#define SAMPLING_RES 10.0f
#define SAMPLING_MIN_LAT -60.0f
#define SAMPLING_MAX_LAT 60.0f
#define SAMPLING_MIN_LON -180.0f
#define SAMPLING_MAX_LON 180.0f
#define constrain(val, min, max) (val < min) ? min : ((val > max) ? max : val)
static const int8_t declination_table[13][37] = \
{
{ 47, 45, 44, 43, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -41, -48, -55, -61, -67, -71, -74, -75, -72, -61, -23, 23, 41, 46, 47, 47 },
{ 30, 30, 30, 30, 30, 29, 29, 29, 27, 23, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -46, -51, -55, -57, -56, -52, -44, -31, -14, 1, 13, 21, 26, 29, 30 },
{ 22, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -39, -31, -21, -11, -3, 3, 9, 14, 18, 20, 22 },
{ 16, 17, 17, 17, 17, 16, 16, 16, 15, 13, 8, 0, -9, -17, -22, -24, -25, -24, -22, -20, -21, -24, -29, -31, -31, -28, -23, -16, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -13, -19, -23, -24, -24, -21, -17, -12, -9, -10, -14, -17, -18, -16, -12, -8, -3, 0, 1, 3, 5, 8, 10, 12, 12 },
{ 10, 10, 10, 10, 10, 10, 10, 9, 8, 5, 0, -7, -15, -20, -22, -22, -19, -15, -10, -5, -2, -1, -4, -7, -8, -8, -6, -3, 0, 0, 1, 2, 4, 6, 8, 10, 10 },
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 3, -2, -9, -15, -19, -20, -17, -13, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 2, 5, 7, 8, 9 },
{ 8, 8, 8, 9, 9, 9, 8, 7, 5, 1, -3, -10, -15, -17, -17, -14, -10, -5, -2, 0, 1, 2, 2, 0, -1, -1, -1, -1, 0, 0, 0, 0, 0, 3, 5, 7, 8 },
{ 8, 8, 9, 9, 10, 10, 9, 8, 5, 0, -5, -11, -15, -16, -15, -11, -7, -3, -1, 0, 2, 3, 3, 1, 0, 0, 0, 0, 0, -1, -2, -3, -2, 0, 3, 6, 8 },
{ 6, 8, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -6, -3, 0, 1, 3, 4, 4, 3, 2, 1, 1, 0, -1, -3, -5, -6, -5, -3, 0, 3, 6 },
{ 5, 8, 11, 13, 14, 15, 13, 10, 5, -1, -9, -14, -16, -16, -13, -10, -6, -3, 0, 2, 3, 5, 5, 5, 5, 4, 3, 1, -1, -4, -7, -9, -8, -6, -2, 1, 5 },
{ 3, 8, 12, 15, 17, 17, 16, 12, 5, -3, -12, -18, -20, -19, -16, -12, -8, -4, 0, 2, 4, 7, 8, 9, 10, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 3 },
{ 3, 8, 13, 16, 19, 20, 19, 14, 4, -7, -18, -24, -26, -24, -21, -16, -11, -6, -2, 2, 6, 10, 13, 15, 17, 16, 13, 7, 0, -7, -13, -15, -14, -11, -6, -1, 3 },
};
static float get_lookup_table_val(unsigned lat, unsigned lon);
static unsigned get_lookup_table_index(float *val, float min, float max);
unsigned get_lookup_table_index(float *val, float min, float max)
{
/* for the rare case of hitting the bounds exactly
* the rounding logic wouldn't fit, so enforce it.
*/
/* limit to table bounds - required for maxima even when table spans full globe range */
if (*val < min) {
*val = min;
}
/* limit to (table bounds - 1) because bilinear interpolation requires checking (index + 1) */
if (*val > max) {
*val = max - SAMPLING_RES;
}
return (-(min) + *val) / SAMPLING_RES;
}
__EXPORT float get_mag_declination(float lat, float lon)
{
/*
* If the values exceed valid ranges, return zero as default
* as we have no way of knowing what the closest real value
* would be.
*/
if (lat < -90.0f || lat > 90.0f ||
lon < -180.0f || lon > 180.0f) {
return 0.0f;
}
/* round down to nearest sampling resolution */
float min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
float min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
/* find index of nearest low sampling point */
unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
unsigned min_lon_index = get_lookup_table_index(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);
float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
/* perform bilinear interpolation on the four grid corners */
float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.0f, 1.0f);
float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f);
float declination_min = lon_scale * (declination_se - declination_sw) + declination_sw;
float declination_max = lon_scale * (declination_ne - declination_nw) + declination_nw;
return lat_scale * (declination_max - declination_min) + declination_min;
}
float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
{
return declination_table[lat_index][lon_index];
}
src/lib/geo_lookup/geo_mag_declination.h
-47
View File
@@ -1,47 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name MAVGEO nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file geo_mag_declination.h
*
* Calculation / lookup table for earth magnetic field declination.
*
*/
#pragma once
__BEGIN_DECLS
__EXPORT float get_mag_declination(float lat, float lon);
__END_DECLS
src/lib/matrix
+1 -1
Submodule src/lib/matrix updated: 41a1cc7583...61af508755
src/lib/terrain_estimation/terrain_estimator.cpp
+1 -1
View File
@@ -37,7 +37,7 @@
*/
#include "terrain_estimator.h"
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+2 -1
View File
@@ -40,7 +40,8 @@
*/
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mathlib/mathlib.h>
#include <px4_config.h>
src/modules/commander/accelerometer_calibration.cpp
+1 -1
View File
@@ -141,7 +141,7 @@
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <conversion/rotation.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
src/modules/commander/calibration_routines.cpp
+1 -1
View File
@@ -48,7 +48,7 @@
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <string.h>
#include <mathlib/mathlib.h>
src/modules/commander/commander.cpp
+1 -1
View File
@@ -65,7 +65,7 @@
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <px4_defines.h>
src/modules/ekf2/CMakeLists.txt
+3 -1
View File
@@ -37,9 +37,11 @@ px4_add_module(
-Wno-sign-compare # TODO: fix all sign-compare
STACK_MAIN 2500
STACK_MAX 4000
INCLUDES
${PX4_SOURCE_DIR}/src/lib/ecl
SRCS
ekf2_main.cpp
DEPENDS
platforms__common
git_ecl
ecl
)
src/modules/fw_att_control/CMakeLists.txt
+1 -1
View File
@@ -38,5 +38,5 @@ px4_add_module(
DEPENDS
platforms__common
git_ecl
lib__ecl
ecl
)
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+1 -1
View File
@@ -37,7 +37,7 @@
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_wheel_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
src/modules/fw_pos_control_l1/CMakeLists.txt
+3 -2
View File
@@ -38,13 +38,14 @@ px4_add_module(
MODULE modules__fw_pos_control_l1
MAIN fw_pos_control_l1
STACK_MAIN 1200
COMPILE_FLAGS
INCLUDES
${PX4_SOURCE_DIR}/src/lib/ecl
SRCS
FixedwingPositionControl.cpp
Landingslope.cpp
DEPENDS
git_ecl
lib__ecl
ecl
launchdetection
runway_takeoff
)
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+1 -1
View File
@@ -57,7 +57,7 @@
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <ecl/tecs/tecs.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
src/modules/gnd_att_control/CMakeLists.txt
+1 -3
View File
@@ -38,8 +38,6 @@ px4_add_module(
SRCS
GroundRoverAttitudeControl.cpp
DEPENDS
platforms__common
git_ecl
lib__ecl
ecl
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/gnd_pos_control/CMakeLists.txt
+3 -3
View File
@@ -35,11 +35,11 @@ px4_add_module(
MAIN gnd_pos_control
STACK_MAIN 1200
COMPILE_FLAGS
INCLUDES
${PX4_SOURCE_DIR}/src/lib/ecl
SRCS
GroundRoverPositionControl.cpp
DEPENDS
platforms__common
git_ecl
lib__ecl
ecl
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
+1 -1
View File
@@ -50,7 +50,7 @@
#include <drivers/drv_hrt.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pid/pid.h>
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+1 -1
View File
@@ -4,7 +4,7 @@
#include <px4_module_params.h>
#include <controllib/blocks.hpp>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/Matrix.hpp>
// uORB Subscriptions
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -69,7 +69,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <dataman/dataman.h>
#include <version/version.h>
src/modules/mavlink/mavlink_messages.cpp
+1 -1
View File
@@ -49,7 +49,7 @@
#include <commander/px4_custom_mode.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_time.h>
#include <systemlib/err.h>
src/modules/mavlink/mavlink_mission.cpp
+1 -1
View File
@@ -45,7 +45,7 @@
#include <errno.h>
#include <math.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <px4_defines.h>
src/modules/mavlink/mavlink_receiver.cpp
+1 -1
View File
@@ -81,7 +81,7 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <commander/px4_custom_mode.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <uORB/topics/vehicle_command_ack.h>
src/modules/mc_att_control/mc_att_control_main.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <conversion/rotation.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/circuit_breaker.h>
#include <mathlib/math/Limits.hpp>
#include <mathlib/math/Functions.hpp>
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1 -1
View File
@@ -69,7 +69,7 @@
#include <uORB/topics/vehicle_status.h>
#include <float.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
src/modules/navigator/datalinkloss.cpp
+1 -1
View File
@@ -44,7 +44,7 @@
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
src/modules/navigator/enginefailure.cpp
+1 -1
View File
@@ -43,7 +43,7 @@
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
src/modules/navigator/follow_target.cpp
+1 -1
View File
@@ -51,7 +51,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp>
#include "navigator.h"
src/modules/navigator/geofence.cpp
+1 -1
View File
@@ -45,7 +45,7 @@
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/mavlink_log.h>
#include "navigator.h"
src/modules/navigator/geofence.h
+1 -1
View File
@@ -44,7 +44,7 @@
#include <px4_module_params.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <px4_defines.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
src/modules/navigator/gpsfailure.cpp
+1 -1
View File
@@ -41,7 +41,7 @@
#include "navigator.h"
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/mission.cpp
+1 -1
View File
@@ -52,7 +52,7 @@
#include <dataman/dataman.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
src/modules/navigator/mission_block.cpp
+1 -1
View File
@@ -46,7 +46,7 @@
#include <math.h>
#include <float.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <uORB/uORB.h>
src/modules/navigator/mission_feasibility_checker.cpp
+1 -1
View File
@@ -46,7 +46,7 @@
#include <drivers/drv_pwm_output.h>
#include <fw_pos_control_l1/Landingslope.hpp>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -52,7 +52,7 @@
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
src/modules/navigator/precland.h
+1 -1
View File
@@ -41,7 +41,7 @@
#pragma once
#include <matrix/math.hpp>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <px4_module_params.h>
#include <uORB/topics/landing_target_pose.h>
src/modules/navigator/rcloss.cpp
+1 -1
View File
@@ -44,7 +44,7 @@
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <uORB/uORB.h>
#include <navigator/navigation.h>
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+1 -1
View File
@@ -66,7 +66,7 @@
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h>
src/modules/simulator/simulator.h
+1 -1
View File
@@ -61,7 +61,7 @@
#include <uORB/topics/distance_sensor.h>
#include <v2.0/mavlink_types.h>
#include <v2.0/common/mavlink.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
namespace simulator
{
src/modules/simulator/simulator_mavlink.cpp
+1 -1
View File
@@ -38,7 +38,7 @@
#include "simulator.h"
#include <simulator_config.h>
#include "errno.h"
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <drivers/drv_pwm_output.h>
#include <sys/socket.h>
#include <netinet/in.h>
src/modules/systemlib/airspeed.c
+4 -4
View File
@@ -40,11 +40,11 @@
*
*/
#include <stdio.h>
#include <math.h>
#include <geo/geo.h>
#include "airspeed.h"
#include <px4_defines.h>
#include <lib/ecl/geo/geo.h>
/**
* Calculate indicated airspeed.
*
@@ -121,7 +121,7 @@ float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel,
// check if the tube diameter and dp is nonzero to avoid division by 0
if ((tube_dia_mm > 0.0f) && (dp > 0.0f)) {
const float d_tubePow4 = powf(tube_dia_mm * 1e-3f, 4);
const float denominator = (float)M_PI * d_tubePow4 * rho_air * dp;
const float denominator = M_PI_F * d_tubePow4 * rho_air * dp;
// avoid division by 0
float eps = 0.0f;
src/modules/vtol_att_control/vtol_att_control_main.h
+1 -1
View File
@@ -57,7 +57,7 @@
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <systemlib/param/param.h>
src/platforms/ros/geo.cpp
+1 -1
View File
@@ -42,7 +42,7 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <px4.h>
#include <unistd.h>
#include <pthread.h>
src/systemcmds/tests/test_autodeclination.cpp
+2 -1
View File
@@ -1,7 +1,8 @@
#include <unit_test.h>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <lib/ecl/geo/geo.h>
#include <ecl/geo_lookup/geo_mag_declination.h>
#include <px4iofirmware/px4io.h>
#include <systemlib/err.h>
#include <lib/mixer/mixer.h>