|
|
楼主 |
发表于 2015-6-29 23:47:36
|
显示全部楼层
没找到，有个
/**
******************************************************************************
* @file GPS_App.c
* @author DevLabs
* @version V0.1
* @date
* @brief
******************************************************************************
* @attention
*
* <br />Copyright 2013
* <br />http://www.DevLabs.cn
*
* <h2><center>© COPYRIGHT 2013 DevLabs</center></h2>
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "GPS_App.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
//! @name WGS84椭球
//! @{
//! a = 6378137.0000000000;
//! b = 6356752.3142;
//! c = 6399593.6258;
//! _a = 1.0 / 298.257223563;
//! e2 = 0.0066943799013;
//! e2 = (a * a - b * b) / (a * a);
//! e_2 = 0.00673949674227;
//! e_2 = (a * a - b * b) / (b * b);
#define v0 (6.36744914585577e+006)
#define v1 (6.39959362580000e+006)
#define v2 (-2.15650202422767e+004)
#define v3 (1.09003037983916e+002)
#define v4 (-6.12188017458933e-001)
#define v5 (-3.21444799442298e+004)
#define v6 (1.35366946123277e+002)
#define v7 (-7.09481085293974e-001)
#define v8 (4.06137292124076e-003)
#define v9 (-4.16666666666667e-002)
#define v10 (2.50000000000000e-001)
#define v11 (2.52731128372009e-003)
#define v12 (7.57013608867423e-006)
#define v13 (1.38888888888889e-003)
#define v14 (-8.33333333333333e-002)
#define v15 (1.66666666666667e-001)
#define v16 (-1.66666666666667e-001)
#define v17 (3.33333333333333e-001)
#define v18 (1.12324945943115e-003)
#define v19 (8.33333333333333e-003)
#define v20 (-1.66666666666667e-001)
#define v21 (1.96742576567650e-001)
#define v22 (4.04369805395214e-003)
#define PI (3.1415926535898)
//! @}
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
static BOOL init = FALSE;
static float distance = 0.0;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/**
* @brief 大地坐标->高斯平面坐标
*
* @param B 纬度(单位度)
* @param L 经度(单位度)
* @param x x坐标(单位米)
* @param y y坐标(单位米)
*/
void WGS84_to_cartesian(float B, float L, float *x, float *y)
{
float l, l2, _N, w0, w1, w2, w3, w4, cosb, cosb2;
l = L - ((int)L / 6 * 6 + 3); // 求经差
B = B * PI / 180.0; // 转弧度
l = l * PI / 180.0; // 转弧度
l2 = l * l;
cosb = cos(B);
cosb2 = cosb * cosb;
_N = v1 + (v2 + (v3 + v4 * cosb2) * cosb2) * cosb2;
w0 = v5 + (v6 + (v7 + v8 * cosb2) * cosb2) * cosb2;
w1 = v9 + (v10 + (v11 + v12 * cosb2) * cosb2) * cosb2;
w2 = v13 + (v14 + v15 * cosb2) * cosb2;
w3 = v16 + (v17 + v18 * cosb2) * cosb2;
w4 = v19 + (v20 + (v21 + v22 * cosb2) * cosb2) * cosb2;
*y = v0 * B + (w0 + (0.5 + (w1 + w2 * l2) * l2) * l2 * _N) * cosb * sin(B);
*x = (1.0 + (w3 + w4 * l2) * l2) * l * _N * cosb;
*x += 500000.0;
}
//! 清零记录的距离值(从当前点开始重新计算)
void dis_reset(void)
{
distance = 0;
init = FALSE;
return;
}
//! 距离计算
void dis_calc_process(void)
{
static float x0, y0; // 前一次坐标值
float len = 0.0;
float x1, y1; // 当前坐标值
float lat, lon; // 纬度,经度
// 如果GPS未定位
if (!GPS_get_state())
{
return;
}
// TODO
// 即使定位偶尔也会出现经纬度为0的情况
// if (..)
// {
// }
lat = GPS_get_latitude();
lon = GPS_get_longitude();
// 坐标转换
WGS84_to_cartesian(lat, lon, &x1, &y1);
if (init)
{
len = (sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)));
}
else
{
init = TRUE;
}
x0 = x1;
y0 = y1;
distance += len;
return;
}
/**
* @brief 获取从指定点开始的距离
*
* @return distance
*/
INT32U dis_get(void)
{
return (INT32U)distance;
}
/******************* (C) COPYRIGHT 2013 DevLabs **********END OF FILE**********/
|
|
发表于 2015-6-28 14:34:15
