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

lla: move implementation to cpp file

Browse Source 
This reduces flash usage
This commit is contained in:
bresch 2024-11-14 15:43:54 +01:00 committed by Mathieu Bresciani
parent aeb182a8ed
commit a224d38e52
6 changed files with 164 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/CMakeLists.txt
View File

@ -277,6 +277,7 @@ px4_add_module(
world_magnetic_model
${EKF_LIBS}
lat_lon_alt
bias_estimator
output_predictor
UNITY_BUILD

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -155,6 +155,7 @@ target_link_libraries(ecl_EKF
PRIVATE
bias_estimator
geo
lat_lon_alt
output_predictor
world_magnetic_model
${EKF_LIBS}

44
src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt
View File

@ -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)

108
src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp Normal file
View File

@ -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<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);
}

76
src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp
View File

@ -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<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 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));
}
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<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;
}
/*
* 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<float>(delta_lat * d_lat_lon_to_d_xy(0)),
static_cast<float>(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<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));
}
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<double>(altitude);
const double de_dlon = (r_e + static_cast<double>(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;

2
src/modules/ekf2/EKF/output_predictor/CMakeLists.txt
View File

@ -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)