121 lines
3.1 KiB
C
121 lines
3.1 KiB
C
/**
|
|
******************************************************************************
|
|
* @file LBS_Transmit.c
|
|
* @author
|
|
* @version
|
|
* @date
|
|
* @brief
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "stm32f10x.h"
|
|
#include "global.h"
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
|
|
|
|
// PI定义
|
|
#define M_PI 3.1415926535897932384626433832795
|
|
|
|
// 常量定义
|
|
#define EARTH_RADIUS 6378245.0 // 地球长半径
|
|
#define EE 0.00669342162296594323 // 偏心率平方
|
|
|
|
|
|
/**
|
|
* @brief 检查坐标是否在中国大陆以外
|
|
* @return 1:境外, 0:境内
|
|
*/
|
|
static int is_out_of_china(double lon, double lat)
|
|
{
|
|
if (lon < 72.004 || lon > 137.8347)
|
|
{
|
|
return 1;
|
|
}
|
|
if (lat < 0.8293 || lat > 55.8271)
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 纬度转换辅助函数
|
|
*/
|
|
static double transform_lat(double x, double y)
|
|
{
|
|
double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
|
|
ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
|
|
ret += (20.0 * sin(y * M_PI) + 40.0 * sin(y / 3.0 * M_PI)) * 2.0 / 3.0;
|
|
ret += (160.0 * sin(y / 12.0 * M_PI) + 320 * sin(y * M_PI / 30.0)) * 2.0 / 3.0;
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* @brief 经度转换辅助函数
|
|
*/
|
|
static double transform_lon(double x, double y)
|
|
{
|
|
double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
|
|
ret += (20.0 * sin(6.0 * x * M_PI) + 20.0 * sin(2.0 * x * M_PI)) * 2.0 / 3.0;
|
|
ret += (20.0 * sin(x * M_PI) + 40.0 * sin(x / 3.0 * M_PI)) * 2.0 / 3.0;
|
|
ret += (150.0 * sin(x / 12.0 * M_PI) + 300.0 * sin(x / 30.0 * M_PI)) * 2.0 / 3.0;
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* @brief GCJ-02转WGS84坐标系
|
|
* @param gcj_lon GCJ-02经度
|
|
* @param gcj_lat GCJ-02纬度
|
|
* @param wgs_lon WGS84经度输出指针
|
|
* @param wgs_lat WGS84纬度输出指针
|
|
* @note 迭代7次，精度约0.1-0.5米
|
|
*/
|
|
void gcj02_to_wgs84(double gcj_lon, double gcj_lat, double *wgs_lon, double *wgs_lat)
|
|
{
|
|
//如果坐标不在中国大陆，直接返回原坐标
|
|
if(is_out_of_china(gcj_lon, gcj_lat))
|
|
{
|
|
*wgs_lon = gcj_lon;
|
|
*wgs_lat = gcj_lat;
|
|
return;
|
|
}
|
|
|
|
//使用迭代法进行转换（7次迭代足够精确）
|
|
double d_lon = 0.0, d_lat = 0.0;
|
|
double tmp_lon = gcj_lon, tmp_lat = gcj_lat;
|
|
|
|
for(int i = 0; i < 7; i++)
|
|
{
|
|
//计算当前WGS84坐标转GCJ02的偏移
|
|
double delta_lat = transform_lat(tmp_lon - 105.0, tmp_lat - 35.0);
|
|
double delta_lon = transform_lon(tmp_lon - 105.0, tmp_lat - 35.0);
|
|
|
|
double rad_lat = tmp_lat * M_PI / 180.0;
|
|
double magic = sin(rad_lat);
|
|
magic = 1 - EE * magic * magic;
|
|
double sqrt_magic = sqrt(magic);
|
|
|
|
delta_lat = (delta_lat * 180.0) / ((EARTH_RADIUS * (1 - EE)) / (magic * sqrt_magic) * M_PI);
|
|
delta_lon = (delta_lon * 180.0) / (EARTH_RADIUS / sqrt_magic * cos(rad_lat) * M_PI);
|
|
|
|
// 计算与目标GCJ02坐标的差值
|
|
d_lat = gcj_lat - (tmp_lat + delta_lat);
|
|
d_lon = gcj_lon - (tmp_lon + delta_lon);
|
|
|
|
// 更新WGS84坐标估计
|
|
tmp_lat += d_lat;
|
|
tmp_lon += d_lon;
|
|
}
|
|
|
|
*wgs_lon = tmp_lon;
|
|
*wgs_lat = tmp_lat;
|
|
}
|
|
|