mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 05:21:35 +08:00
lib/geo: move from ecl
This commit is contained in:
@@ -41,12 +41,13 @@ add_subdirectory(bezier)
|
||||
add_subdirectory(cdev)
|
||||
add_subdirectory(circuit_breaker)
|
||||
add_subdirectory(collision_prevention)
|
||||
add_subdirectory(controllib)
|
||||
add_subdirectory(component_information)
|
||||
add_subdirectory(controllib)
|
||||
add_subdirectory(conversion)
|
||||
add_subdirectory(crypto)
|
||||
add_subdirectory(drivers)
|
||||
add_subdirectory(ecl)
|
||||
add_subdirectory(geo)
|
||||
add_subdirectory(hysteresis)
|
||||
add_subdirectory(l1)
|
||||
add_subdirectory(landing_slope)
|
||||
|
||||
@@ -43,7 +43,7 @@
|
||||
#include "airspeed.h"
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel,
|
||||
float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius)
|
||||
|
||||
@@ -35,7 +35,7 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_accel_fifo.h>
|
||||
|
||||
@@ -47,7 +47,7 @@
|
||||
#include <lib/drivers/smbus/SMBus.hpp>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <px4_platform_common/param.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@@ -95,14 +95,6 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
|
||||
|
||||
add_custom_target(prebuild_targets)
|
||||
|
||||
# temporary
|
||||
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/../mathlib/math/filter/AlphaFilter.hpp ${CMAKE_CURRENT_BINARY_DIR}/mathlib/math/filter/AlphaFilter.hpp COPYONLY)
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../world_magnetic_model world_magnetic_model)
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
|
||||
|
||||
if(MSVC)
|
||||
add_compile_options(
|
||||
/W4
|
||||
@@ -152,6 +144,17 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
|
||||
add_dependencies(prebuild_targets matrix)
|
||||
include_directories(${CMAKE_BINARY_DIR}/matrix-prefix/src/matrix)
|
||||
|
||||
# temporary
|
||||
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/../mathlib/math/filter/AlphaFilter.hpp ${CMAKE_CURRENT_BINARY_DIR}/mathlib/math/filter/AlphaFilter.hpp COPYONLY)
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
add_definitions(-Dhrt_absolute_time=uint64_t)
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../geo geo)
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../world_magnetic_model world_magnetic_model)
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
|
||||
|
||||
|
||||
endif()
|
||||
|
||||
if(NOT (${CMAKE_BUILD_TYPE} MATCHES "Coverage") AND NOT (${CMAKE_BUILD_TYPE} MATCHES "Debug"))
|
||||
@@ -179,7 +182,6 @@ if(ECL_ASAN)
|
||||
endif()
|
||||
|
||||
add_subdirectory(EKF)
|
||||
add_subdirectory(geo)
|
||||
|
||||
if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
|
||||
add_subdirectory(test)
|
||||
|
||||
@@ -56,7 +56,7 @@ add_library(ecl_EKF
|
||||
add_dependencies(ecl_EKF prebuild_targets)
|
||||
target_compile_definitions(ecl_EKF PRIVATE -DMODULE_NAME="ecl/EKF")
|
||||
target_include_directories(ecl_EKF PUBLIC ${ECL_SOURCE_DIR})
|
||||
target_link_libraries(ecl_EKF PRIVATE ecl_geo world_magnetic_model)
|
||||
target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
|
||||
|
||||
set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h")
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
|
||||
@@ -48,7 +48,7 @@
|
||||
#include "sensor_range_finder.hpp"
|
||||
#include "utils.hpp"
|
||||
|
||||
#include <geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
@@ -52,7 +52,6 @@ set(SRCS
|
||||
test_EKF_flow.cpp
|
||||
test_EKF_terrain_estimator.cpp
|
||||
test_SensorRangeFinder.cpp
|
||||
test_geo.cpp
|
||||
test_EKF_utils.cpp
|
||||
)
|
||||
add_executable(ECL_GTESTS ${SRCS})
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 ECL Development Team. All rights reserved.
|
||||
# Copyright (c) 2018-2021 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
|
||||
@@ -12,7 +12,7 @@
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name ECL nor the names of its contributors may be
|
||||
# 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.
|
||||
#
|
||||
@@ -31,10 +31,14 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_library(ecl_geo
|
||||
add_library(geo
|
||||
geo.cpp
|
||||
)
|
||||
add_dependencies(ecl_geo prebuild_targets)
|
||||
target_compile_definitions(ecl_geo PRIVATE -DMODULE_NAME="ecl/geo")
|
||||
target_include_directories(ecl_geo PUBLIC ${ECL_SOURCE_DIR})
|
||||
target_link_libraries(ecl_geo PRIVATE m)
|
||||
geo.h
|
||||
)
|
||||
add_dependencies(geo prebuild_targets)
|
||||
target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_link_libraries(geo PRIVATE m)
|
||||
|
||||
if(CMAKE_PROJECT_NAME MATCHES "px4") # temporary
|
||||
px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo)
|
||||
endif()
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2021 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
|
||||
@@ -43,7 +43,6 @@
|
||||
*/
|
||||
|
||||
#include "geo.h"
|
||||
#include <ecl.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
@@ -52,23 +51,26 @@
|
||||
using matrix::wrap_pi;
|
||||
using matrix::wrap_2pi;
|
||||
|
||||
#ifndef hrt_absolute_time
|
||||
# define hrt_absolute_time() (0)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Azimuthal Equidistant Projection
|
||||
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
||||
*/
|
||||
|
||||
bool map_projection_initialized(const struct map_projection_reference_s *ref)
|
||||
bool map_projection_initialized(const map_projection_reference_s *ref)
|
||||
{
|
||||
return ref->init_done;
|
||||
}
|
||||
|
||||
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
|
||||
uint64_t map_projection_timestamp(const map_projection_reference_s *ref)
|
||||
{
|
||||
return ref->timestamp;
|
||||
}
|
||||
|
||||
// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
|
||||
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
|
||||
int map_projection_init_timestamped(map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
ref->lat_rad = math::radians(lat_0);
|
||||
ref->lon_rad = math::radians(lon_0);
|
||||
@@ -81,13 +83,12 @@ int map_projection_init_timestamped(struct map_projection_reference_s *ref, doub
|
||||
return 0;
|
||||
}
|
||||
|
||||
//lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
|
||||
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
|
||||
int map_projection_init(map_projection_reference_s *ref, double lat_0, double lon_0)
|
||||
{
|
||||
return map_projection_init_timestamped(ref, lat_0, lon_0, ecl_absolute_time());
|
||||
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
|
||||
}
|
||||
|
||||
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
|
||||
int map_projection_reference(const map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
@@ -99,7 +100,7 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl
|
||||
return 0;
|
||||
}
|
||||
|
||||
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
|
||||
int map_projection_project(const map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
@@ -128,7 +129,7 @@ int map_projection_project(const struct map_projection_reference_s *ref, double
|
||||
return 0;
|
||||
}
|
||||
|
||||
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
|
||||
int map_projection_reproject(const map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
@@ -164,7 +165,8 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
|
||||
const double d_lat = lat_next_rad - lat_now_rad;
|
||||
const double d_lon = math::radians(lon_next) - math::radians(lon_now);
|
||||
|
||||
const double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
|
||||
const double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(
|
||||
lat_next_rad);
|
||||
|
||||
const double c = atan2(sqrt(a), sqrt(1.0 - a));
|
||||
|
||||
@@ -177,6 +179,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
|
||||
if (fabsf(dist) < FLT_EPSILON) {
|
||||
*lat_target = lat_A;
|
||||
*lon_target = lon_A;
|
||||
|
||||
} else {
|
||||
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);
|
||||
@@ -192,7 +195,8 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
|
||||
double lat_start_rad = math::radians(lat_start);
|
||||
double lon_start_rad = math::radians(lon_start);
|
||||
|
||||
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((double)bearing));
|
||||
*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));
|
||||
|
||||
@@ -224,12 +228,14 @@ get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, dou
|
||||
const double d_lon = math::radians(lon_next) - math::radians(lon_now);
|
||||
|
||||
/* conscious mix of double and float trig function to maximize speed and efficiency */
|
||||
*v_n = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)));
|
||||
*v_n = static_cast<float>(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 = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad));
|
||||
}
|
||||
|
||||
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)
|
||||
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 = math::radians(lat_now);
|
||||
double lon_now_rad = math::radians(lon_now);
|
||||
@@ -244,7 +250,8 @@ get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next
|
||||
*v_e = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad));
|
||||
}
|
||||
|
||||
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
|
||||
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 = math::radians(lat_now);
|
||||
double lon_now_rad = math::radians(lon_now);
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2021 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
|
||||
@@ -52,7 +52,8 @@ static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
|
||||
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f; // pascals (Pa)
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f; // kilopascals (kPa)
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA / 100.0f; // Millibar (mbar) (1 mbar = 100 Pa)
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA /
|
||||
100.0f; // Millibar (mbar) (1 mbar = 100 Pa)
|
||||
|
||||
static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f; // kg/m^3
|
||||
static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f; // J/(kg * K)
|
||||
@@ -107,7 +108,8 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp);
|
||||
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.
|
||||
@@ -164,7 +166,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
|
||||
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
|
||||
*/
|
||||
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);
|
||||
double *lat_target, double *lon_target);
|
||||
|
||||
/**
|
||||
* Creates a waypoint from given waypoint, distance and bearing
|
||||
@@ -178,7 +180,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
|
||||
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
|
||||
*/
|
||||
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
|
||||
double *lat_target, double *lon_target);
|
||||
double *lat_target, double *lon_target);
|
||||
|
||||
/**
|
||||
* Returns the bearing to the next waypoint in radians.
|
||||
@@ -190,18 +192,21 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
|
||||
*/
|
||||
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
||||
|
||||
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
|
||||
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
|
||||
float *v_e);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
|
||||
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
|
||||
double *lon_res);
|
||||
|
||||
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);
|
||||
double lat_start, double lon_start, double lat_end, double lon_end);
|
||||
|
||||
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);
|
||||
double lat_center, double lon_center,
|
||||
float radius, float arc_start_bearing, float arc_sweep);
|
||||
|
||||
/*
|
||||
* Calculate distance in global frame
|
||||
@@ -35,10 +35,11 @@
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <memory>
|
||||
#include <geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
class GeoTest : public ::testing::Test {
|
||||
public:
|
||||
class GeoTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
void SetUp() override
|
||||
{
|
||||
origin.timestamp = 0;
|
||||
@@ -49,7 +50,7 @@ class GeoTest : public ::testing::Test {
|
||||
origin.init_done = true;
|
||||
}
|
||||
|
||||
protected:
|
||||
protected:
|
||||
map_projection_reference_s origin;
|
||||
|
||||
};
|
||||
@@ -123,7 +124,7 @@ TEST_F(GeoTest, waypoint_from_heading_and_negative_distance)
|
||||
double lon_start = 18;
|
||||
float bearing = 0;
|
||||
float lat_offset = -0.01f;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH * M_PI / 180.f * lat_offset;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH_F * M_PI_F / 180.f * lat_offset;
|
||||
|
||||
double lat_target = 0;
|
||||
double lon_target = 0;
|
||||
@@ -132,7 +133,7 @@ TEST_F(GeoTest, waypoint_from_heading_and_negative_distance)
|
||||
waypoint_from_heading_and_distance(lat_start, lon_start, bearing, dist, &lat_target, &lon_target);
|
||||
|
||||
// THEN: it should be the same
|
||||
EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target);
|
||||
EXPECT_FLOAT_EQ(static_cast<float>(lat_start) + lat_offset, lat_target);
|
||||
EXPECT_DOUBLE_EQ(lon_start, lon_target);
|
||||
}
|
||||
|
||||
@@ -143,7 +144,7 @@ TEST_F(GeoTest, waypoint_from_heading_and_positive_distance)
|
||||
double lon_start = 18;
|
||||
float bearing = 0;
|
||||
float lat_offset = 0.01f;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH * M_PI / 180.f * lat_offset;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH_F * M_PI_F / 180.f * lat_offset;
|
||||
|
||||
double lat_target = 0;
|
||||
double lon_target = 0;
|
||||
@@ -152,7 +153,7 @@ TEST_F(GeoTest, waypoint_from_heading_and_positive_distance)
|
||||
waypoint_from_heading_and_distance(lat_start, lon_start, bearing, dist, &lat_target, &lon_target);
|
||||
|
||||
// THEN: it should be the same
|
||||
EXPECT_FLOAT_EQ(lat_start + lat_offset, lat_target);
|
||||
EXPECT_FLOAT_EQ(static_cast<float>(lat_start) + lat_offset, lat_target);
|
||||
EXPECT_DOUBLE_EQ(lon_start, lon_target);
|
||||
}
|
||||
|
||||
@@ -184,7 +185,7 @@ TEST_F(GeoTest, waypoint_from_line_and_positive_distance)
|
||||
double lat_offset = 0.01;
|
||||
double lat_end = lat_start + lat_offset;
|
||||
double lon_end = 18;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH * M_PI / 180.f * lat_offset;
|
||||
float dist = CONSTANTS_RADIUS_OF_EARTH_F * M_PI_F / 180.f * static_cast<float>(lat_offset);
|
||||
|
||||
double lat_target = 0;
|
||||
double lon_target = 0;
|
||||
@@ -206,7 +207,7 @@ TEST_F(GeoTest, waypoint_from_line_and_negative_distance)
|
||||
double lat_offset = 0.01;
|
||||
double lat_end = lat_start + lat_offset;
|
||||
double lon_end = 18;
|
||||
float dist = -CONSTANTS_RADIUS_OF_EARTH * M_PI / 180.f * lat_offset;
|
||||
float dist = -CONSTANTS_RADIUS_OF_EARTH_F * M_PI_F / 180.f * static_cast<float>(lat_offset);
|
||||
|
||||
double lat_target = 0;
|
||||
double lon_target = 0;
|
||||
@@ -36,4 +36,4 @@ px4_add_library(l1
|
||||
ECL_L1_Pos_Controller.hpp
|
||||
)
|
||||
|
||||
target_link_libraries(l1 PRIVATE ecl_geo)
|
||||
target_link_libraries(l1 PRIVATE geo)
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
|
||||
#include "ECL_L1_Pos_Controller.hpp"
|
||||
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
|
||||
@@ -36,4 +36,4 @@ px4_add_library(tecs
|
||||
TECS.hpp
|
||||
)
|
||||
|
||||
target_link_libraries(tecs PRIVATE ecl_geo)
|
||||
target_link_libraries(tecs PRIVATE geo)
|
||||
|
||||
@@ -33,7 +33,7 @@
|
||||
|
||||
#include "TECS.hpp"
|
||||
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
|
||||
@@ -37,7 +37,7 @@
|
||||
*/
|
||||
|
||||
#include "terrain_estimator.h"
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead
|
||||
|
||||
@@ -36,6 +36,7 @@ add_library(world_magnetic_model
|
||||
geo_mag_declination.h
|
||||
geo_magnetic_tables.hpp
|
||||
)
|
||||
add_dependencies(world_magnetic_model prebuild_targets)
|
||||
target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
if(CMAKE_PROJECT_NAME MATCHES "px4") # temporary
|
||||
|
||||
Reference in New Issue
Block a user