解释Prometheus_px4 geo.cpp中一个函数 map_projection_project

youxia23

geo.cpp中有个函数map_projection_project/* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567掳, not 471234567掳)
* @param lon in degrees (8.1234567掳, not 81234567掳)
* @return 0 if map_projection_init was called before, -1 else
* */
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,float *y);


我将这个函数单独复制出来运算，发现运算结果与如下网址的计算结果有较大误差。
GPS经度纬度距离计算  GPS经度纬度距离计算 - 计算专家 (jisuan.mobi)
能否解释下函数中解算x y的原理？ 也就是根据代码，解释计算原理。

我知道答案 回答被采纳将会获得3 阿木币 已有8人回答

youxia23

如果您能解释通，并且最后计算偏差较小，可以再追加。

BOSHEN97

youxia23 发表于 2022-4-26 19:55
如果您能解释通，并且最后计算偏差较小，可以再追加。

您可以了解一下mavros中local_position相关的代码

blueboats

你这个是要GPS位置转换成本地坐标系的x y点么   这个geo.cpp 是px4中的代码 还是prometheus中的代码呢

blueboats

根据代码，解释计算原理。  你可以把代码贴出来啊 C++的代码 挨个分析计算可以吗

youxia23

blueboats 发表于 2022-4-27 10:31
你这个是要GPS位置转换成本地坐标系的x y点么   这个geo.cpp 是px4中的代码 还是prometheus中的代码呢 ...

是的。geo.cpp 是px4中的代码。

youxia23

BOSHEN97 发表于 2022-4-27 09:20
您可以了解一下mavros中local_position相关的代码

其实我说的就是GPS转本地坐标系。

youxia23

blueboats 发表于 2022-4-27 10:40
根据代码，解释计算原理。  你可以把代码贴出来啊 C++的代码 挨个分析计算可以吗 ...

int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
        if (!map_projection_initialized(ref)) {
                return -1;
        }

        const double lat_rad = math::radians(lat);
        const double lon_rad = math::radians(lon);

        const double sin_lat = sin(lat_rad);
        const double cos_lat = cos(lat_rad);

        const double cos_d_lon = cos(lon_rad - ref->lon_rad);

        const double arg = math::constrain(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon, -1.0,  1.0);
        const double c = acos(arg);

        double k = 1.0;

        if (fabs(c) > 0) {
                k = (c / sin(c));
        }

        *x = static_cast<float>(k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
        *y = static_cast<float>(k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH);

        return 0;
}

youxia23

blueboats 发表于 2022-4-27 10:40
根据代码，解释计算原理。  你可以把代码贴出来啊 C++的代码 挨个分析计算可以吗 ...

贴出来了，请看下。