From a224d38e525c99d1a38ca11fadcb981cd85dc115 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 14 Nov 2024 15:43:54 +0100 Subject: [PATCH] lla: move implementation to cpp file This reduces flash usage --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + .../ekf2/EKF/lat_lon_alt/CMakeLists.txt | 44 ++++++- .../ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp | 108 ++++++++++++++++++ .../ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 76 ++---------- .../ekf2/EKF/output_predictor/CMakeLists.txt | 2 +- 6 files changed, 164 insertions(+), 68 deletions(-) create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 62e6f29103..112b920ba2 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -277,6 +277,7 @@ px4_add_module( world_magnetic_model ${EKF_LIBS} + lat_lon_alt bias_estimator output_predictor UNITY_BUILD diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 27804ebcd8..f6dc615f4e 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -155,6 +155,7 @@ target_link_libraries(ecl_EKF PRIVATE bias_estimator geo + lat_lon_alt output_predictor world_magnetic_model ${EKF_LIBS} diff --git a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt index bf2b3ebf53..acd30f51f2 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt +++ b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt @@ -1 +1,43 @@ -px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS geo) +############################################################################ +# +# 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) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp new file mode 100644 index 0000000000..93d347f2d7 --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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::Vector2d; + +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(r_e) + _altitude), + -v_ned(0) / (static_cast(r_n) + _altitude), + -v_ned(1) * tanf(_latitude_rad) / (static_cast(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(altitude); + const double de_dlon = (r_e + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_lat * d_lat_lon_to_d_xy(0)), + static_cast(delta_lon * d_lat_lon_to_d_xy(1)), + -delta_alt); +} diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp index bb6a384d6a..548f6ee30d 100644 --- a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -73,33 +73,13 @@ public: void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); } - void 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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); - _altitude -= delta_pos(2); - } - - void 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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); - } - - LatLonAlt operator+(const matrix::Vector3f &delta_pos) const - { - matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude()); - const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); - const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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; - } + /* + * 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) { @@ -108,54 +88,18 @@ public: _altitude = lla.altitude(); } - matrix::Vector3f 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(delta_lat * d_lat_lon_to_d_xy(0)), - static_cast(delta_lon * d_lat_lon_to_d_xy(1)), - -delta_alt); - } - /* * 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) - { - double r_n; - double r_e; - computeRadiiOfCurvature(_latitude_rad, r_n, r_e); - return matrix::Vector3f( - v_ned(1) / (static_cast(r_e) + _altitude), - -v_ned(0) / (static_cast(r_n) + _altitude), - -v_ned(1) * tanf(_latitude_rad) / (static_cast(r_e) + _altitude)); - } + 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) - { - double r_n; - double r_e; - computeRadiiOfCurvature(latitude, r_n, r_e); - const double dn_dlat = r_n + static_cast(altitude); - const double de_dlon = (r_e + static_cast(altitude)) * cos(latitude); - - return matrix::Vector2d(dn_dlat, de_dlon); - } + 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) - { - 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; - } + double &transverse_radius_of_curvature); struct Wgs84 { static constexpr double equatorial_radius = 6378137.0; diff --git a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt index 701a5cb13f..8d7b3e0607 100644 --- a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt +++ b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt @@ -36,4 +36,4 @@ add_library(output_predictor output_predictor.h ) -add_dependencies(output_predictor prebuild_targets) +add_dependencies(output_predictor prebuild_targets lat_lon_alt)