PX4经纬度转东北天坐标c++代码

本文链接：https://blog.csdn.net/benchuspx/article/details/135855283

博客介绍了C++代码使用px4 v1.13.3里geo.h头文件的情况。指出经纬度必须用double数据类型，因float精度无法满足要求。还提到geo.h转换较简单、精度不高，远距离转换会有偏差。最后给出编译geo.cpp的两种方式。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

c++代码，用到了px4 v1.13.3里的geo.h头文件：

#include "geo.h" // from px4 geo utils (gps to ENU)

// 设置参考点，即东北为(0,0)的点的经纬度
double ref_lat_ = 39.978861; //纬度
double ref_lon_ = 116.339803; //经度
MapProjection global_local_proj_ref{ref_lat_, ref_lon_, 0};

// ENU坐标转换到经纬度
float north = 0.0； 
float east = 0.0;
double lat; //纬度
double lon; //经度
global_local_proj_ref.reproject(north, east, lat, lon);

// 经纬度转换到ENU坐标系
double lat = 39.978861; //纬度
double lon = 116.339803; //经度
float north； 
float east;
global_local_proj_ref.reproject(lat, lon， north, east);

这里有个陷阱，经纬度必须用double数据类型！！！float的精度是不能满足小数点后七到八位的要求的。

顺便一提，这个geo.h转换比较简单，精度可能没那么高，距离远的话（比如1km）可能会和更精确的转换差1m左右。

附上geo.h和geo.cpp，编译的时候可以把geo.cpp编译成一个library再链接到你的程序，或者和你的程序一起编译。

#CMakeLists.txt

add_library(geo_lib
              src/geo.cpp)


add_executable(my_node
                  src/my_node.cpp)

# link libraries for this lib
target_link_libraries(my_node
   ${catkin_LIBRARIES}
   geo_lib
)

或者

#CMakeLists.txt

add_executable(my_node
                  src/my_node.cpp
                  src/geo.cpp)

geo.h：

/****************************************************************************
 *
 *   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
 * 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 <[email protected]>
 * @author Julian Oes <[email protected]>
 * @author Lorenz Meier <[email protected]>
 * @author Anton Babushkin <[email protected]>
 * Additional functions - @author Doug Weibel <[email protected]>
 */

#pragma once

#include <stdbool.h>
#include <stdint.h>

#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>

/************* Added by Peixuan Shu **********/
#include <drivers/drv_hrt.h>
/********************************************/

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_AIR_DENSITY_SEA_LEVEL_15C = 1.225f;				// kg/m^3
static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f;					// J/(kg * K)
static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f;				// °C

static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000;					// meters (m)
static constexpr float  CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH;		// meters (m)

static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f;				// radians/second (rad/s)


// 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
} ;


/**
 * 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°)
 */
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 waypoint