mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 08:30:34 +08:00
lla: move to lib directory
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
7cf42727fb
commit
b6658df169
@@ -33,7 +33,6 @@
|
||||
|
||||
add_subdirectory(bias_estimator)
|
||||
add_subdirectory(output_predictor)
|
||||
add_subdirectory(lat_lon_alt)
|
||||
|
||||
set(EKF_LIBS)
|
||||
set(EKF_SRCS)
|
||||
|
||||
@@ -66,8 +66,6 @@
|
||||
# include "aid_sources/aux_global_position/aux_global_position.hpp"
|
||||
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
|
||||
#include "lat_lon_alt/lat_lon_alt.hpp"
|
||||
|
||||
enum class Likelihood { LOW, MEDIUM, HIGH };
|
||||
class ExternalVisionVel;
|
||||
|
||||
|
||||
@@ -42,7 +42,6 @@
|
||||
#ifndef EKF_ESTIMATOR_INTERFACE_H
|
||||
#define EKF_ESTIMATOR_INTERFACE_H
|
||||
|
||||
#include "lat_lon_alt/lat_lon_alt.hpp"
|
||||
#if defined(MODULE_NAME)
|
||||
#include <px4_platform_common/log.h>
|
||||
# define ECL_INFO PX4_DEBUG
|
||||
@@ -73,6 +72,7 @@
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#include <lib/atmosphere/atmosphere.h>
|
||||
#include <lib/lat_lon_alt/lat_lon_alt.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/AlphaFilter.hpp>
|
||||
|
||||
@@ -1,43 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_library(lat_lon_alt
|
||||
lat_lon_alt.cpp
|
||||
lat_lon_alt.hpp
|
||||
)
|
||||
|
||||
add_dependencies(lat_lon_alt prebuild_targets)
|
||||
target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo)
|
||||
@@ -1,152 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "lat_lon_alt.hpp"
|
||||
|
||||
using matrix::Vector3f;
|
||||
using matrix::Vector3d;
|
||||
using matrix::Vector2d;
|
||||
|
||||
LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef)
|
||||
{
|
||||
// Convert position using Borkowski closed-form exact solution
|
||||
// P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C)
|
||||
const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2));
|
||||
const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius;
|
||||
const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1));
|
||||
const double E = (k1 - k2) / beta;
|
||||
const double F = (k1 + k2) / beta;
|
||||
|
||||
const double P = 4.0 / 3.0 * (E * F + 1);
|
||||
const double Q = 2 * (E * E - F * F);
|
||||
const double D = P * P * P + Q * Q;
|
||||
const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0);
|
||||
const double G = 0.5 * (sqrt(E * E + V) + E);
|
||||
const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G;
|
||||
|
||||
const double lon = atan2(p_ecef(1), p_ecef(0));
|
||||
const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2)));
|
||||
const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) +
|
||||
(p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat);
|
||||
|
||||
LatLonAlt lla;
|
||||
lla.setLatLonRad(lat, lon);
|
||||
lla.setAltitude(static_cast<float>(alt));
|
||||
return lla;
|
||||
}
|
||||
|
||||
Vector3d LatLonAlt::toEcef() const
|
||||
{
|
||||
const double cos_lat = cos(_latitude_rad);
|
||||
const double sin_lat = sin(_latitude_rad);
|
||||
const double cos_lon = cos(_longitude_rad);
|
||||
const double sin_lon = sin(_longitude_rad);
|
||||
|
||||
const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0));
|
||||
const double r_total = r_e + static_cast<double>(_altitude);
|
||||
|
||||
return Vector3d(r_total * cos_lat * cos_lon,
|
||||
r_total * cos_lat * sin_lon,
|
||||
((1.0 - Wgs84::eccentricity2) * r_total) * sin_lat);
|
||||
}
|
||||
|
||||
Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
|
||||
{
|
||||
double r_n;
|
||||
double r_e;
|
||||
computeRadiiOfCurvature(_latitude_rad, r_n, r_e);
|
||||
return Vector3f(
|
||||
v_ned(1) / (static_cast<float>(r_e) + _altitude),
|
||||
-v_ned(0) / (static_cast<float>(r_n) + _altitude),
|
||||
-v_ned(1) * tanf(_latitude_rad) / (static_cast<float>(r_e) + _altitude));
|
||||
}
|
||||
|
||||
Vector2d LatLonAlt::deltaLatLonToDeltaXY(const double latitude, const float altitude)
|
||||
{
|
||||
double r_n;
|
||||
double r_e;
|
||||
computeRadiiOfCurvature(latitude, r_n, r_e);
|
||||
const double dn_dlat = r_n + static_cast<double>(altitude);
|
||||
const double de_dlon = (r_e + static_cast<double>(altitude)) * cos(latitude);
|
||||
|
||||
return Vector2d(dn_dlat, de_dlon);
|
||||
}
|
||||
|
||||
void LatLonAlt::computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
|
||||
double &transverse_radius_of_curvature)
|
||||
{
|
||||
const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2);
|
||||
const double sqrt_tmp = std::sqrt(tmp);
|
||||
meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp);
|
||||
transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp;
|
||||
}
|
||||
|
||||
LatLonAlt LatLonAlt::operator+(const matrix::Vector3f &delta_pos) const
|
||||
{
|
||||
const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude());
|
||||
const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
const float altitude = _altitude - delta_pos(2);
|
||||
|
||||
LatLonAlt lla_new;
|
||||
lla_new.setLatLonRad(latitude_rad, longitude_rad);
|
||||
lla_new.setAltitude(altitude);
|
||||
return lla_new;
|
||||
}
|
||||
|
||||
void LatLonAlt::operator+=(const matrix::Vector3f &delta_pos)
|
||||
{
|
||||
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
_altitude -= delta_pos(2);
|
||||
}
|
||||
|
||||
void LatLonAlt::operator+=(const matrix::Vector2f &delta_pos)
|
||||
{
|
||||
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
}
|
||||
|
||||
matrix::Vector3f LatLonAlt::operator-(const LatLonAlt &lla) const
|
||||
{
|
||||
const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad());
|
||||
const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad());
|
||||
const float delta_alt = _altitude - lla.altitude();
|
||||
|
||||
const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
return matrix::Vector3f(static_cast<float>(delta_lat * d_lat_lon_to_d_xy(0)),
|
||||
static_cast<float>(delta_lon * d_lat_lon_to_d_xy(1)),
|
||||
-delta_alt);
|
||||
}
|
||||
@@ -1,118 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "mathlib/math/Limits.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
class LatLonAlt
|
||||
{
|
||||
public:
|
||||
LatLonAlt() = default;
|
||||
LatLonAlt(const LatLonAlt &lla)
|
||||
{
|
||||
_latitude_rad = lla.latitude_rad();
|
||||
_longitude_rad = lla.longitude_rad();
|
||||
_altitude = lla.altitude();
|
||||
}
|
||||
|
||||
LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m)
|
||||
{
|
||||
_latitude_rad = math::radians(latitude_deg);
|
||||
_longitude_rad = math::radians(longitude_deg);
|
||||
_altitude = altitude_m;
|
||||
}
|
||||
|
||||
|
||||
static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef);
|
||||
matrix::Vector3d toEcef() const;
|
||||
|
||||
void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }
|
||||
|
||||
double latitude_deg() const { return math::degrees(latitude_rad()); }
|
||||
double longitude_deg() const { return math::degrees(longitude_rad()); }
|
||||
|
||||
const double &latitude_rad() const { return _latitude_rad; }
|
||||
const double &longitude_rad() const { return _longitude_rad; }
|
||||
float altitude() const { return _altitude; }
|
||||
|
||||
void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); }
|
||||
void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); }
|
||||
void setAltitude(const float altitude) { _altitude = altitude; }
|
||||
|
||||
void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); }
|
||||
void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); }
|
||||
void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; }
|
||||
|
||||
void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); }
|
||||
|
||||
/*
|
||||
* The plus and minus operators below use approximations and should only be used when the Cartesian component is small
|
||||
*/
|
||||
LatLonAlt operator+(const matrix::Vector3f &delta_pos) const;
|
||||
void operator+=(const matrix::Vector3f &delta_pos);
|
||||
void operator+=(const matrix::Vector2f &delta_pos);
|
||||
matrix::Vector3f operator-(const LatLonAlt &lla) const;
|
||||
|
||||
void operator=(const LatLonAlt &lla)
|
||||
{
|
||||
_latitude_rad = lla.latitude_rad();
|
||||
_longitude_rad = lla.longitude_rad();
|
||||
_altitude = lla.altitude();
|
||||
}
|
||||
|
||||
/*
|
||||
* Compute the angular rate of the local navigation frame at the current latitude and height
|
||||
* with respect to an inertial frame and resolved in the navigation frame
|
||||
*/
|
||||
matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;
|
||||
|
||||
private:
|
||||
// Convert between curvilinear and cartesian errors
|
||||
static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude);
|
||||
|
||||
static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
|
||||
double &transverse_radius_of_curvature);
|
||||
|
||||
struct Wgs84 {
|
||||
static constexpr double equatorial_radius = 6378137.0;
|
||||
static constexpr double eccentricity = 0.0818191908425;
|
||||
static constexpr double eccentricity2 = eccentricity * eccentricity;
|
||||
static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2);
|
||||
};
|
||||
|
||||
double _latitude_rad{0.0};
|
||||
double _longitude_rad{0.0};
|
||||
float _altitude{0.0};
|
||||
};
|
||||
@@ -1,107 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include "lat_lon_alt.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
using math::radians;
|
||||
using math::degrees;
|
||||
|
||||
TEST(TestLatLonAlt, init)
|
||||
{
|
||||
LatLonAlt lla(5.7, -2.3, 420);
|
||||
ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7);
|
||||
ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3);
|
||||
ASSERT_EQ(lla.altitude(), 420);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, set)
|
||||
{
|
||||
LatLonAlt lla(0.0, 0.0, 0);
|
||||
ASSERT_EQ(lla.latitude_rad(), 0.0);
|
||||
ASSERT_EQ(lla.longitude_rad(), 0.0);
|
||||
ASSERT_EQ(lla.altitude(), 0);
|
||||
|
||||
lla.setLatLonRad(0.1, -0.5);
|
||||
lla.setAltitude(420);
|
||||
ASSERT_EQ(lla.latitude_rad(), 0.1);
|
||||
ASSERT_EQ(lla.longitude_rad(), -0.5);
|
||||
ASSERT_EQ(lla.altitude(), 420);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, copy)
|
||||
{
|
||||
LatLonAlt lla(-0.8, -0.1, 500);
|
||||
|
||||
LatLonAlt lla_copy = lla;
|
||||
ASSERT_EQ(lla_copy.latitude_deg(), -0.8);
|
||||
ASSERT_EQ(lla_copy.longitude_deg(), -0.1);
|
||||
ASSERT_EQ(lla_copy.altitude(), 500);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, addDeltaPos)
|
||||
{
|
||||
MapProjection pos_ref(60.0, 5.0);
|
||||
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f);
|
||||
|
||||
Vector3f delta_pos(5.f, -2.f, 3.f);
|
||||
lla += delta_pos;
|
||||
|
||||
double lat_new, lon_new;
|
||||
pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new);
|
||||
|
||||
EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6);
|
||||
EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6);
|
||||
EXPECT_EQ(lla.altitude(), 397.f);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, subLatLonAlt)
|
||||
{
|
||||
MapProjection pos_ref(60.0, 5.0);
|
||||
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f);
|
||||
|
||||
const Vector3f delta_pos_true(1.f, -2.f, 3.f);
|
||||
|
||||
double lat_new, lon_new;
|
||||
pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new);
|
||||
LatLonAlt lla_new(lat_new, lon_new, -3.f);
|
||||
Vector3f delta_pos = lla_new - lla;
|
||||
|
||||
EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2);
|
||||
EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
|
||||
EXPECT_EQ(delta_pos(2), delta_pos_true(2));
|
||||
}
|
||||
@@ -37,9 +37,9 @@
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "../RingBuffer.h"
|
||||
#include "../lat_lon_alt/lat_lon_alt.hpp"
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/lat_lon_alt/lat_lon_alt.hpp>
|
||||
|
||||
class OutputPredictor
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user