553 lines
17 KiB
C
553 lines
17 KiB
C
#include <math.h>
|
||
#include <stdint.h>
|
||
#include <string.h>
|
||
|
||
#include "cJSON.h"
|
||
#include "coordtrans.h"
|
||
#include "rtk_gps.h"
|
||
|
||
#include "ql_log.h"
|
||
#define LOGD(msg, ...) QL_LOG(QL_LOG_LEVEL_DEBUG, "rtk", msg, ##__VA_ARGS__)
|
||
#define LOGI(msg, ...) QL_LOG(QL_LOG_LEVEL_INFO, "rtk", msg, ##__VA_ARGS__)
|
||
#define LOGW(msg, ...) QL_LOG(QL_LOG_LEVEL_WARN, "rtk", msg, ##__VA_ARGS__)
|
||
#define LOGE(msg, ...) QL_LOG(QL_LOG_LEVEL_ERROR, "rtk", msg, ##__VA_ARGS__)
|
||
|
||
extern uint16_t gWordVar[];
|
||
|
||
/**
|
||
* @brief 计算参数
|
||
*
|
||
*/
|
||
struct gps_coord_calc
|
||
{
|
||
double angle_offset; // GPS1(左侧Gps)旋转角度
|
||
double x_offset; // x偏移距离m
|
||
double y_offset; // y偏移距离m
|
||
};
|
||
/**
|
||
struct ConfigParam cfp = {
|
||
.dx = 0, // x坐标平移量(米)
|
||
.dy = 0, // y坐标平移量(米)
|
||
.dz = 0, // z坐标平移量(米)
|
||
.wx = 0, // x坐标旋转角度(秒)
|
||
.wy = 0, // y坐标旋转角度(秒)
|
||
.wz = 0, // z坐标旋转角度(秒)
|
||
.k = 0, // 尺度因子(ppm)，这里其实应该用变量m表示
|
||
.height = 0, // 投影面高程
|
||
.gcsSrc = WGS84, // 原始坐标系
|
||
.gcsDst = WGS84, // 结果坐标系
|
||
};
|
||
! 大地坐标系类型
|
||
enum GEODETIC_COORD_SYSTEM { BJ54 = 0, XA80, WGS84, WGS2000 };
|
||
{
|
||
"dx": 0,
|
||
"dy": 0,
|
||
"dz": 0,
|
||
"wx": 0,
|
||
"wy": 0,
|
||
"wz": 0,
|
||
"k": 0,
|
||
"height": 0,
|
||
"gcsSrc": "WGS84",
|
||
"gcsDst": "WGS84",
|
||
}
|
||
*/
|
||
#define COORD_CFG_FILE_PATH "/coord_cfg.json"
|
||
static struct ConfigParam gps_coord_cfg; // 转换配置参数
|
||
/**
|
||
{
|
||
"y_offset": 0, // 单位m
|
||
"x_offset": 0, // 单位m
|
||
"angle_offset": 0, // 角度 度
|
||
}
|
||
*/
|
||
#define CALC_CFG_FILE_PATH "/calc_cfg.json"
|
||
static struct gps_coord_calc gps_coord_calc_cfg; // 计算配置参数
|
||
|
||
/**
|
||
* @brief 保存json配置文件
|
||
*
|
||
* @param json
|
||
* @param name
|
||
*/
|
||
static void save_json_cfg_file(cJSON *json, const char *name)
|
||
{
|
||
if (name == NULL || json == NULL)
|
||
{
|
||
return NULL;
|
||
}
|
||
int8_t *buff_p = NULL;
|
||
int32_t buf_len = 0;
|
||
int32_t fileFd = -1;
|
||
int ret = 0;
|
||
fileFd = LSAPI_FS_Open(name, LSAPI_FS_O_RDONLY, 0x0);
|
||
if (fileFd < 0)
|
||
{
|
||
LOGE("LSAPI_FS_Open failed, file:%s\n", name);
|
||
return;
|
||
}
|
||
buff_p = cJSON_Print(json);
|
||
if (buff_p == NULL)
|
||
{
|
||
LOGE("cJSON_Print failed, buff_p is null\n");
|
||
goto exit0;
|
||
}
|
||
buf_len = strlen(buff_p);
|
||
ret = LSAPI_FS_Write(fileFd, (void *)buff_p, buf_len);
|
||
if (ret <= 0)
|
||
{
|
||
LOGE("LSAPI_FS_Write failed. ret:%d\n", ret);
|
||
goto exit1;
|
||
}
|
||
exit1:
|
||
LSAPI_OSI_Free(buff_p);
|
||
exit0:
|
||
LSAPI_FS_Close(fileFd);
|
||
return;
|
||
}
|
||
/**
|
||
* @brief 加载json配置文件
|
||
*
|
||
* @param name
|
||
* @return cJSON*
|
||
*/
|
||
static cJSON *load_json_cfg_file(const char *name)
|
||
{
|
||
if (name == NULL)
|
||
{
|
||
return NULL;
|
||
}
|
||
cJSON *json = NULL;
|
||
int8_t *buff_p = NULL;
|
||
int32_t fileFd = -1;
|
||
int32_t ret = 0;
|
||
fileFd = LSAPI_FS_Open(name, LSAPI_FS_O_RDONLY, 0x0);
|
||
if (fileFd < 0)
|
||
{
|
||
LOGE("LSAPI_FS_Open failed, file:%s\n", name);
|
||
return NULL;
|
||
}
|
||
LSAPI_FS_Stat_info_t fileStat = {0};
|
||
ret = LSAPI_FS_Fstat(fileFd, &fileStat);
|
||
if (fileStat.st_size > 4096)
|
||
{
|
||
fileStat.st_size = 4096;
|
||
}
|
||
buff_p = (int8_t *)LSAPI_OSI_Malloc(fileStat.st_size + 1); // byte
|
||
if (buff_p == NULL)
|
||
{
|
||
LOGE("LSAPI_OSI_Malloc failed, size:%d\n", fileStat.st_size + 1);
|
||
goto exit0;
|
||
}
|
||
memset(buff_p, 0x00, fileStat.st_size + 1);
|
||
ret = LSAPI_FS_Read(fileFd, (void *)buff_p, fileStat.st_size);
|
||
if (ret <= 0)
|
||
{
|
||
LOGE("LSAPI_FS_Read failed. ret:%d\n", ret);
|
||
goto exit1;
|
||
}
|
||
buff_p[fileStat.st_size] = '\0';
|
||
LSAPI_FS_Close(fileFd);
|
||
json = cJSON_Parse((const char *)buff_p);
|
||
LSAPI_OSI_Free(buff_p);
|
||
buff_p = NULL;
|
||
if (json == NULL)
|
||
{
|
||
LOGE("JSON_Parse Err ret%s", cJSON_GetErrorPtr());
|
||
}
|
||
return json;
|
||
exit1:
|
||
LSAPI_OSI_Free(buff_p);
|
||
buff_p = NULL;
|
||
exit0:
|
||
LSAPI_FS_Close(fileFd);
|
||
return NULL;
|
||
}
|
||
|
||
/**
|
||
* @brief 加载转换参数配置
|
||
*
|
||
* @param coord_cfg
|
||
*/
|
||
void load_coord_cfg(struct ConfigParam *coord_cfg, const char *filePath)
|
||
{
|
||
cJSON *node, *child;
|
||
cJSON *cfg_json = load_json_cfg_file(filePath);
|
||
if (cfg_json != NULL)
|
||
{
|
||
node = cJSON_GetObjectItem(cfg_json, "dx");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->dx = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "dy");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->dy = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "dz");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->dz = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "wx");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->wx = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "wy");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->wy = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "wz");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->wz = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "k");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->k = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "height");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->height = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "gcsSrc");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->gcsSrc = node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "gcsDst");
|
||
if (node != NULL)
|
||
{
|
||
coord_cfg->gcsDst = node->valueint;
|
||
}
|
||
LOGE("load_coord_cfg success, filePath:%s\n", filePath);
|
||
cJSON_Delete(cfg_json);
|
||
}
|
||
else
|
||
{
|
||
LOGE("cfg_json is null, filePath:%s\n", filePath);
|
||
}
|
||
}
|
||
/**
|
||
* @brief 保存转换参数配置
|
||
*
|
||
* @param coord_cfg
|
||
*/
|
||
void save_coord_cfg(struct ConfigParam *coord_cfg, const char *filePath)
|
||
{
|
||
cJSON *cfg_json = cJSON_CreateObject();
|
||
if (cfg_json != NULL)
|
||
{
|
||
cJSON_AddNumberToObject(cfg_json, "dx", coord_cfg->dx);
|
||
cJSON_AddNumberToObject(cfg_json, "dy", coord_cfg->dy);
|
||
cJSON_AddNumberToObject(cfg_json, "dz", coord_cfg->dz);
|
||
cJSON_AddNumberToObject(cfg_json, "wx", coord_cfg->wx);
|
||
cJSON_AddNumberToObject(cfg_json, "wy", coord_cfg->wy);
|
||
cJSON_AddNumberToObject(cfg_json, "wz", coord_cfg->wz);
|
||
cJSON_AddNumberToObject(cfg_json, "k", coord_cfg->k);
|
||
cJSON_AddNumberToObject(cfg_json, "height", coord_cfg->height);
|
||
cJSON_AddNumberToObject(cfg_json, "gcsSrc", coord_cfg->gcsSrc);
|
||
cJSON_AddNumberToObject(cfg_json, "gcsDst", coord_cfg->gcsDst);
|
||
save_cfg_file(cfg_json, filePath);
|
||
cJSON_Delete(cfg_json);
|
||
}
|
||
}
|
||
/**
|
||
* @brief 加载计算参数配置
|
||
*
|
||
* @param coord_cfg
|
||
*/
|
||
void load_calc_cfg(struct gps_coord_calc *calc_cfg, const char *filePath)
|
||
{
|
||
cJSON *node, *child;
|
||
cJSON *cfg_json = load_json_cfg_file(filePath);
|
||
if (cfg_json != NULL)
|
||
{
|
||
node = cJSON_GetObjectItem(cfg_json, "y_offset");
|
||
if (node != NULL)
|
||
{
|
||
calc_cfg->y_offset = (double)node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "x_offset");
|
||
if (node != NULL)
|
||
{
|
||
calc_cfg->x_offset = (double)node->valueint;
|
||
}
|
||
node = cJSON_GetObjectItem(cfg_json, "angle_offset");
|
||
if (node != NULL)
|
||
{
|
||
calc_cfg->angle_offset = (double)node->valueint;
|
||
}
|
||
LOGT("load_calc_cfg success, filePath:%s, y_offset:%lf, x_offset:%lf, angle_offset:%lf\n",
|
||
filePath, calc_cfg->y_offset, calc_cfg->x_offset, calc_cfg->angle_offset);
|
||
cJSON_Delete(cfg_json);
|
||
}
|
||
else
|
||
{
|
||
LOGE("cfg_json is null, filePath:%s\n", filePath);
|
||
}
|
||
}
|
||
|
||
void gps_coord_init(const char *filePath)
|
||
{
|
||
memset(&gps_coord_cfg, 0x00, sizeof(gps_coord_cfg));
|
||
/**
|
||
* @brief 加载配置 若无配置需要在远程传输后自动加载配置 可指定配置文件路径
|
||
*
|
||
*/
|
||
if (filePath == NULL)
|
||
{
|
||
load_coord_cfg(&gps_coord_cfg, COORD_CFG_FILE_PATH);
|
||
}
|
||
else
|
||
{
|
||
load_coord_cfg(&gps_coord_cfg, filePath);
|
||
}
|
||
// 初始化常用椭球参数信息
|
||
DatumEllipsoidal(0, 0, NULL); // 初始化中央子午线118
|
||
}
|
||
|
||
void gps_coord_calc_init(const char *filePath)
|
||
{
|
||
memset(&gps_coord_calc_cfg, 0x00, sizeof(gps_coord_calc_cfg));
|
||
if (filePath == NULL)
|
||
{
|
||
load_calc_cfg(&gps_coord_calc_cfg, CALC_CFG_FILE_PATH);
|
||
}
|
||
else
|
||
{
|
||
load_calc_cfg(&gps_coord_calc_cfg, filePath);
|
||
}
|
||
}
|
||
|
||
#define PI (3.1415926)
|
||
#define ANGLE_TO_RADIAN(a) ((a) * (PI) / 180) // 角度转弧度
|
||
#define ANGLE_TO_DEGREES(a) ((a)*180 / (PI)) // 角度转度
|
||
/**
|
||
* @brief 点旋转
|
||
*
|
||
* @param point 指定点
|
||
* @param angle 旋转角度
|
||
* @param origin 原始点
|
||
* @param res 结果点
|
||
* @return int
|
||
*/
|
||
static int pointRotate(struct Point *point, double angle, struct Point *origin, struct Point *res)
|
||
{
|
||
if (point == NULL || res == NULL)
|
||
{
|
||
return 1;
|
||
}
|
||
double r = ANGLE_TO_RADIAN(angle); // 角度转弧度??
|
||
if (origin == NULL || (origin->x == 0 && origin->y == 0))
|
||
{
|
||
// See: https://en.wikipedia.org/wiki/Cartesian_coordinate_system#Rotation
|
||
res->x = point->x * cos(r) - point->y * sin(r);
|
||
res->y = point->x * sin(r) + point->y * cos(r);
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
// See: https://math.stackexchange.com/questions/1964905/rotation-around-non-zero-point
|
||
struct Point p0 = {point->x - origin->x, point->y - origin->y};
|
||
struct Point rotated;
|
||
rotated.x = p0.x * cos(r) - p0.y * sin(r);
|
||
rotated.y = p0.x * sin(r) + p0.y * cos(r);
|
||
res->x = rotated.x + origin->x;
|
||
res->y = rotated.y + origin->y;
|
||
return 0;
|
||
}
|
||
}
|
||
/**
|
||
* @brief 以度数和距离为单位按角度平移点
|
||
*
|
||
* @param point 需要平移的原始点
|
||
* @param angle 角度
|
||
* @param distance 距离
|
||
* @param res_point 结果点
|
||
*/
|
||
static void pointTranslate(struct Point *point, double angle, double distance,
|
||
struct Point *res_point)
|
||
{
|
||
if (point == NULL || res_point == NULL)
|
||
{
|
||
return;
|
||
}
|
||
double r = ANGLE_TO_RADIAN(angle);
|
||
res_point->x = point->x + distance * cos(r);
|
||
res_point->y = point->y + distance * sin(r);
|
||
return;
|
||
}
|
||
static double lineAngle(struct Point *line_s, struct Point *line_e)
|
||
{
|
||
if (line_s == NULL || line_e == NULL)
|
||
{
|
||
return 0;
|
||
}
|
||
return ANGLE_TO_DEGREES(atan2(line_e->y - line_s->y, line_e->x - line_s->x));
|
||
}
|
||
/**
|
||
* @brief 计算出从指定线起始点为x_offset距离的点的坐标
|
||
*
|
||
* @param line_s 线起始点
|
||
* @param line_e 线终点
|
||
* @param x_offset 偏移距离
|
||
* @param res_point 结果点
|
||
* @return int
|
||
*/
|
||
static int lineInterpolate(struct Point *line_s, struct Point *line_e, double x_offset,
|
||
struct Point *res_point)
|
||
{ // line_s + line_e构成一条直线
|
||
if (line_e == NULL || line_s == NULL)
|
||
{
|
||
return 1;
|
||
}
|
||
/**
|
||
* @brief 以起始点line_s作为原点进行旋转
|
||
* 旋转角度为起始点和终点的连线并且得出与水平线的夹角lineAngle(line_s, line_e)顺时针旋转
|
||
* 最终根据偏移x_offset计算出结果点
|
||
*/
|
||
pointTranslate(line_s, lineAngle(line_s, line_e), x_offset, res_point);
|
||
return 0;
|
||
}
|
||
|
||
// 七参数modbus参数地址集合
|
||
#define MODBUS_GPS_COORD_DX_ADDR 1024
|
||
#define MODBUS_GPS_COORD_DY_ADDR 1026
|
||
#define MODBUS_GPS_COORD_DZ_ADDR 1028
|
||
#define MODBUS_GPS_COORD_WX_ADDR 1030
|
||
#define MODBUS_GPS_COORD_WY_ADDR 1032
|
||
#define MODBUS_GPS_COORD_WZ_ADDR 1034
|
||
#define MODBUS_GPS_COORD_K_ADDR 1036
|
||
#define MODBUS_GPS_COORD_HEIGHT_ADDR 1038
|
||
#define MODBUS_GPS_COORD_GCSSRC_ADDR 1040
|
||
#define MODBUS_GPS_COORD_GCSDST_ADDR 1041
|
||
// 计算参数modbus参数地址集合
|
||
#define MODBUS_GPS_CALC_X_ADDR 1042
|
||
#define MODBUS_GPS_CALC_Y_ADDR 1044
|
||
#define MODBUS_GPS_CALC_ANGLE_ADDR 1046
|
||
|
||
static inline void get_gps_coord_param_bymodbus(struct ConfigParam *gps_coord_cfg)
|
||
{
|
||
if (gps_coord_cfg == NULL)
|
||
{
|
||
return;
|
||
}
|
||
gps_coord_cfg->dx =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_DX_ADDR] | gWordVar[MODBUS_GPS_COORD_DX_ADDR + 1] << 16) /
|
||
1000000.0); // 六位小数
|
||
gps_coord_cfg->dy =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_DY_ADDR] | gWordVar[MODBUS_GPS_COORD_DY_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->dz =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_DZ_ADDR] | gWordVar[MODBUS_GPS_COORD_DZ_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->wx =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_WX_ADDR] | gWordVar[MODBUS_GPS_COORD_WX_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->wy =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_WY_ADDR] | gWordVar[MODBUS_GPS_COORD_WY_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->wz =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_WZ_ADDR] | gWordVar[MODBUS_GPS_COORD_WZ_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->k =
|
||
(double)((gWordVar[MODBUS_GPS_COORD_K_ADDR] | gWordVar[MODBUS_GPS_COORD_K_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->height = (double)((gWordVar[MODBUS_GPS_COORD_HEIGHT_ADDR] |
|
||
gWordVar[MODBUS_GPS_COORD_HEIGHT_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_coord_cfg->gcsSrc = (gWordVar[MODBUS_GPS_COORD_GCSSRC_ADDR]);
|
||
gps_coord_cfg->gcsDst = (gWordVar[MODBUS_GPS_COORD_GCSDST_ADDR]);
|
||
}
|
||
|
||
static inline void get_gps_calc_param_bymodbus(struct gps_coord_calc *gps_calc_cfg)
|
||
{
|
||
if (gps_calc_cfg == NULL)
|
||
{
|
||
return;
|
||
}
|
||
gps_calc_cfg->x_offset =
|
||
(double)((gWordVar[MODBUS_GPS_CALC_X_ADDR] | gWordVar[MODBUS_GPS_CALC_X_ADDR + 1] << 16) /
|
||
1000000.0); // 六位小数
|
||
gps_calc_cfg->y_offset =
|
||
(double)((gWordVar[MODBUS_GPS_CALC_Y_ADDR] | gWordVar[MODBUS_GPS_CALC_Y_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
gps_calc_cfg->angle_offset =
|
||
(double)((gWordVar[MODBUS_GPS_CALC_ANGLE_ADDR] | gWordVar[MODBUS_GPS_CALC_ANGLE_ADDR + 1] << 16) /
|
||
1000000.0);
|
||
}
|
||
/**
|
||
* @brief 坐标转换+计算中心坐标点任务
|
||
* 1、获取Gps1/2的原始经纬度坐标
|
||
* 2、将经纬度坐标转换成平面坐标
|
||
* 3、根据计算参数计算目标节点坐标
|
||
*/
|
||
/**
|
||
* @brief
|
||
*
|
||
* @param gps1
|
||
* @param gps2
|
||
*/
|
||
void gps_calc_target_point(gps_t *gps1, gps_t *gps2, struct car_param_data *cpd)
|
||
{
|
||
if (gps1 == NULL || gps2 == NULL || cpd == NULL)
|
||
{
|
||
return;
|
||
}
|
||
if (!(gps1->LAT != 0 && gps1->LNG != 0 && gps2->LAT != 0 && gps2->LNG != 0))
|
||
{
|
||
return;
|
||
}
|
||
// get_gps_coord_param_bymodbus(&gps_coord_cfg); // 从modbus更新七参数
|
||
// get_gps_calc_param_bymodbus(&gps_coord_calc_cfg); // 从modbus更新计算参数
|
||
LOGD("angle_offset:%lf, x_offset:%lf, y_offset:%lf\n",
|
||
gps_coord_calc_cfg.angle_offset, gps_coord_calc_cfg.x_offset,
|
||
gps_coord_calc_cfg.y_offset);
|
||
struct Point gps1_d2p_i = {(double)(gps1->LAT / 10000000.00), (double)(gps1->LNG / 10000000.00),
|
||
0.000000000000};
|
||
struct Point gps1_d2p_res = d2p(&gps1_d2p_i, &gps_coord_cfg); // 大地坐标转平面坐标
|
||
|
||
struct Point gps2_d2p_i = {(double)(gps2->LAT / 10000000.00), (double)(gps2->LNG / 10000000.00),
|
||
0.000000000000};
|
||
struct Point gps2_d2p_res = d2p(&gps2_d2p_i, &gps_coord_cfg);
|
||
|
||
LOGD("gps1: src_x:%lf, src_y:%lf, res_x:%lf, res_y:%lf\n", gps1_d2p_i.x,
|
||
gps1_d2p_i.y, gps1_d2p_res.x, gps1_d2p_res.y);
|
||
LOGD("gps2: src_x:%lf, src_y:%lf, res_x:%lf, res_y:%lf\n", gps2_d2p_i.x,
|
||
gps2_d2p_i.y, gps2_d2p_res.x, gps2_d2p_res.y);
|
||
// ## 1 旋转Gps2点
|
||
struct Point gps2ResPoint; // 旋转后Gps2的坐标
|
||
// pointRotate(&gps2_d2p_res, gps_coord_calc_cfg.angle_offset, &gps1_d2p_res, &gps2ResPoint);
|
||
// cpd->angle = lineAngle(&gps1_d2p_res, &gps2ResPoint) - 90; // 保留当前车体角度
|
||
// ## 2 平移Gps1点(x_offset)
|
||
struct Point gps1ResPoint; // 平移后Gps1的坐标
|
||
lineInterpolate(&gps1_d2p_res, &gps2ResPoint, gps_coord_calc_cfg.x_offset, &gps1ResPoint);
|
||
// ## 3 平移Gps1点(y_offset) // 旋转+平移
|
||
struct Point gps2_ResPoint; // 旋转后Gps2`的坐标
|
||
double angle_offset_tmp = 0;
|
||
if (gps_coord_calc_cfg.y_offset >= 0)
|
||
{
|
||
angle_offset_tmp = -90; // 目标点在gps1_d2p_res与gps2ResPoint之间的直线右侧
|
||
}
|
||
else
|
||
{
|
||
angle_offset_tmp = 90; // 目标点在gps1_d2p_res与gps2ResPoint之间的直线左侧
|
||
}
|
||
pointRotate(&gps2ResPoint, angle_offset_tmp, &gps1ResPoint,
|
||
&gps2_ResPoint); // 以gps1ResPoint为起点
|
||
struct Point flat_res_point;
|
||
lineInterpolate(&gps1ResPoint, &gps2_ResPoint, gps_coord_calc_cfg.y_offset,
|
||
&flat_res_point); // 以gps1ResPoint为起点
|
||
// ## end 将平面坐标转换成大地坐标(上面的结果为平面坐标)
|
||
// cpd->target = p2d(&flat_res_point, &gps_coord_cfg); // 大地坐标转平面坐标
|
||
// log_printf(LDEBUG, LOG_TAG "res_point: x:%lf, y:%lf, angle:%lf\n", cpd->target.x, cpd->target.y,
|
||
// cpd->angle);
|
||
return;
|
||
}
|