/*******************************************************************************
|
* 文件名称 : geo_utils.c
|
* 文件说明 : LLA/ECEF/ENU 坐标转换辅助函数
|
* 创建日期 : 2025-11-22
|
*******************************************************************************/
|
|
#include "geo_utils.h"
|
|
#include <math.h>
|
|
#define DEG2RAD (0.017453292519943295769236907684886f)
|
#define WGS84_A (6378137.0)
|
#define WGS84_E2 (6.69437999014e-3)
|
|
static void geo_lla_to_ecef(double lat_rad, double lon_rad, double alt_m, double ecef[3])
|
{
|
double sin_lat = sin(lat_rad);
|
double cos_lat = cos(lat_rad);
|
double sin_lon = sin(lon_rad);
|
double cos_lon = cos(lon_rad);
|
double N = WGS84_A / sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat);
|
|
ecef[0] = (N + alt_m) * cos_lat * cos_lon;
|
ecef[1] = (N + alt_m) * cos_lat * sin_lon;
|
ecef[2] = (N * (1.0 - WGS84_E2) + alt_m) * sin_lat;
|
}
|
|
static void geo_ecef_to_enu(const ST_GeoOrigin *origin, const double ecef[3], float enu[3])
|
{
|
double dx = ecef[0] - origin->ecef[0];
|
double dy = ecef[1] - origin->ecef[1];
|
double dz = ecef[2] - origin->ecef[2];
|
|
float east = (float)(-origin->sin_lon * dx + origin->cos_lon * dy);
|
float north = (float)(-origin->sin_lat * origin->cos_lon * dx
|
- origin->sin_lat * origin->sin_lon * dy
|
+ origin->cos_lat * dz);
|
float up = (float)(origin->cos_lat * origin->cos_lon * dx
|
+ origin->cos_lat * origin->sin_lon * dy
|
+ origin->sin_lat * dz);
|
|
enu[0] = east;
|
enu[1] = north;
|
enu[2] = up;
|
}
|
|
/* 预先计算 ENU 原点，缓存经纬度 sin/cos 以及 ECEF 坐标 */
|
HIDO_VOID Geo_OriginInit(ST_GeoOrigin *_pstOrigin, double _latDeg, double _lonDeg, double _altM)
|
{
|
if (_pstOrigin == HIDO_NULL)
|
{
|
return;
|
}
|
|
_pstOrigin->lat_deg = _latDeg;
|
_pstOrigin->lon_deg = _lonDeg;
|
_pstOrigin->alt_m = _altM;
|
_pstOrigin->lat_rad = _latDeg * DEG2RAD;
|
_pstOrigin->lon_rad = _lonDeg * DEG2RAD;
|
_pstOrigin->sin_lat = sin(_pstOrigin->lat_rad);
|
_pstOrigin->cos_lat = cos(_pstOrigin->lat_rad);
|
_pstOrigin->sin_lon = sin(_pstOrigin->lon_rad);
|
_pstOrigin->cos_lon = cos(_pstOrigin->lon_rad);
|
|
geo_lla_to_ecef(_pstOrigin->lat_rad,
|
_pstOrigin->lon_rad,
|
_pstOrigin->alt_m,
|
_pstOrigin->ecef);
|
_pstOrigin->initialized = HIDO_TRUE;
|
}
|
|
/* 将 GPRMI (经纬高) 转换为相对原点的 ENU 坐标 */
|
HIDO_VOID Geo_GprmiToENU(const ST_GPRMI *_pstGprmi, const ST_GeoOrigin *_pstOrigin, float _enu[3])
|
{
|
if (_enu == HIDO_NULL)
|
{
|
return;
|
}
|
|
_enu[0] = 0.0f;
|
_enu[1] = 0.0f;
|
_enu[2] = 0.0f;
|
|
if (_pstGprmi == HIDO_NULL || _pstOrigin == HIDO_NULL)
|
{
|
return;
|
}
|
|
if (_pstGprmi->m_bValid == HIDO_FALSE || _pstOrigin->initialized == HIDO_FALSE)
|
{
|
return;
|
}
|
|
double ecef[3];
|
geo_lla_to_ecef(_pstGprmi->m_dLatitude * DEG2RAD,
|
_pstGprmi->m_dLongitude * DEG2RAD,
|
(double)_pstGprmi->m_fAltitude,
|
ecef);
|
|
geo_ecef_to_enu(_pstOrigin, ecef, _enu);
|
}
|
