haversine公式_计算gps距离接口例程
1 haversine公式
先放着，后续补充原理；
2 接口函数目的
前几天测试反馈了一条骑行记录的bug，实际记录和具体坐标对不上；骑行记录的数据又多，分析不直观；
实际gps坐标数据拿出来模拟仿真没什么问题，估计采样点还是哪里有问题把，先放放；
这几天没什么事，整了一个函数接口用来对预处理的gps数据进行解析，
gps数据都解析完了，干脆把haversine公式的接口也整上好了，复制粘贴一下也很快；
gps数据的私有协议如下图所示，为实际数据存储record的格式；
orgfile.log为预处理数据，已经去掉协议头部分，剩下的全部是12字节一组的record；
gpsPztSimulator.c作为一个单独的仿真例程，直接使用vscode+mingw就可以编译了；
3 gps记录私有协议
将gsensor传感器采集的gps经纬度坐标通过haversine公式，计算出运动距离；
4 haversine使用接口
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#define uint8_t unsigned char
#define uint16_t unsigned short
#define uint32_t unsigned int
#define FILE_PATH_SRC "C:/Users/WINDOWS/Desktop/orgfile.log"
//读取src文件大小
uint32_t readFileSrcLen(FILE *fpSrc){
uint32_t lenRet = 0;
fseek(fpSrc, 0, SEEK_END);
lenRet = ftell(fpSrc);
rewind(fpSrc);
printf("orgfile lenRet:(d)%d \n",lenRet);
return lenRet;
}
//读取 文件数据 至psrcData数组
bool readFileCharDataToBuff(uint8_t *psrcData, uint32_t *plenSrcData, FILE *fpSrc,uint32_t lenFileSize ){
uint8_t charGet = 0;
uint32_t charCnt = 0;
do{
charGet = (uint8_t)fgetc(fpSrc);
if( ((char)charGet>='0')&&((uint8_t)charGet <='9') ){
psrcData[charCnt] = (uint8_t)charGet;
charCnt++;
}
else if(((uint8_t)charGet>='a')&&((uint8_t)charGet <='f') ){
psrcData[charCnt] = (uint8_t)charGet;
charCnt++;
}
//printf("%c",charGet);
//文件结尾标志符EOF = (int)(-1); -1怎么截都是0xff，那我直接截成char型也不是不行。。。
}while(charGet!= 0xff);
*plenSrcData = charCnt;
printf("*plenSrcData:(d)%d \n",*plenSrcData);
for(uint32_t i=0;i<charCnt;i++) printf("%c ",psrcData[i]); printf("\n");
return true;
}
//将 字符数组 两两组合为16进制数组
bool changeCharToHexData(uint8_t *phexData, uint32_t *phexDataCnt, uint8_t *psrcData, uint32_t srcDataLen){
uint8_t hbits,lbits;
uint32_t hexDataCnt = 0;
for(uint32_t i = 0;i<srcDataLen;i++){
hbits = psrcData[i];
if( (hbits >= '0')&&(hbits<='9') ){
hbits = hbits-'0';
}
else if( (hbits >= 'a')&&(hbits<='f') ){
hbits = (hbits - 'a')+10;
}
lbits = psrcData[i+1];
if( (lbits >= '0')&&(lbits<='9') ){
lbits = lbits-'0';
}
else if( (lbits >= 'a')&&(lbits<='f') ){
lbits = (lbits - 'a')+10;
}
phexData[hexDataCnt] = hbits*16 + lbits;
hexDataCnt++;
i++; //i每次加2;for循环中+1,这里+1；为什么要写的这么皮，因为for循环中不能i+2；
}
*phexDataCnt = hexDataCnt;
printf("*phexDataCnt(d):%d \n",*phexDataCnt);
for(uint32_t i=0;i<hexDataCnt;i++) printf("%02x,",phexData[i]);
return true;
}
#define DBP_EXERCISE printf
#define GPS_PI 3.1415926
#define EARTH_RADIUS 6378.137 //地球近似半径
//求弧度
float CalculateRadian(float d){
return d * GPS_PI / 180.0; //角度1˚ = π / 180
}
//Haversine算法公式
float count_gpsdistance(float old_latitude, float old_longitude, float cur_latitude, float cur_longitude){
DBP_EXERCISE("gps pzt: %f %f %f %f ",old_latitude,old_longitude,cur_latitude,cur_longitude);
if((old_latitude == 0)||(old_longitude == 0)||(cur_latitude == 0)||(cur_longitude == 0)){
DBP_EXERCISE("\n");
return 0 ;
}
float rad_cur_latitude = CalculateRadian(old_latitude); //获取当前纬度弧度
float rad_pro_latitude = CalculateRadian(cur_latitude); //获取运动过程纬度弧度
float latitude_difference = rad_cur_latitude - rad_pro_latitude; //获取到纬度弧度差
float longitude_difference = CalculateRadian(old_longitude) - CalculateRadian(cur_longitude);//获取到经度弧度差
//Haversine公式:通过gps坐标计算位置距离;
float gpsdistance = 2 * asin((sqrt(pow(sin(latitude_difference / 2), 2) + cos(rad_cur_latitude) * cos(rad_pro_latitude) * pow(sin(longitude_difference / 2), 2) )));
gpsdistance = gpsdistance * EARTH_RADIUS;
gpsdistance = round(gpsdistance * 10000) / 10000;
gpsdistance = gpsdistance * 1000; //千米换算为米
DBP_EXERCISE("delta_gpsDistance:%0.3f \n", gpsdistance);
return gpsdistance;
};
//调用haversine公式，通过gps坐标差值计算运动距离
void gpsDistanceSimulator(uint8_t *pDataBuf, uint32_t lenofBuff){
//record: 4bytes_lati, 4bytes_longi, gpsSpeed, jumpState,heartRates, reserved
//0x23,0x10,0xe4,0x42, 0x19,0x92,0xb5,0x41, 0x00,0x00,0x64,0x00,
float old_latitude = *(float*)(&pDataBuf[0]);
float old_longitude= *(float*)(&pDataBuf[4]);
DBP_EXERCISE("lat: %f %f \n",old_latitude,old_longitude) ;
float cur_latitude;
float cur_longitude;
float delta_distance = 0;
float total_distance =0;
uint32_t i = 0;
do{
cur_latitude = *(float*)(&pDataBuf[i]);
cur_longitude = *(float*)(&pDataBuf[i+4]);
//提取坐标后，将坐标数据传入Haversine算法处理
delta_distance = count_gpsdistance(old_latitude, old_longitude, cur_latitude, cur_longitude);
total_distance +=delta_distance;
i += 12;
old_latitude = cur_latitude;
old_longitude = cur_longitude;
}while( i<lenofBuff );
DBP_EXERCISE("total_distance:%f \n",total_distance);
}
//打印出记录块中的跳变点
void gpsDistanceJumpPoint(uint8_t *pDataBuf,uint32_t lenofbuff){
DBP_EXERCISE("lenofbuff(d):%d \n",lenofbuff);
float cur_latitude;
float cur_longitude;
float delta_distance = 0.0;
float total_distance =0.0;
uint32_t i = 9;
uint8_t state = 0;
do{
state = *(uint8_t *)(&pDataBuf[i]);
if(state){
cur_latitude = *(float*)(&pDataBuf[i-9]);
cur_longitude = *(float*)(&pDataBuf[i-5]);
DBP_EXERCISE("cae:%f %f state=%x i=%d record=%d min=%d \n",cur_latitude,cur_longitude,state,i,(i/12),((i/12)/30));
}
i += 12;
}while( i<lenofbuff );
}
//Haversine公式：将gps坐标delta值 换算成运动距离的算法；
//以一组切好的30分钟骑行的gps坐标记录数据为例
//1 先把文件数据提取到数组中
//2 从数组中两两组合提取16进制数到数组中
//3 以record为单位提取gps坐标，调用Haversine算法计算距离
int main(){
FILE *fpSrc = NULL;
long lenFileSize = 0;
uint8_t *psrcData = NULL;
uint32_t lenSrcData = 0;
uint8_t *phexData = NULL;
uint32_t phexDataCnt = 0;
fpSrc = fopen(FILE_PATH_SRC, "r+");
lenFileSize= readFileSrcLen(fpSrc);
//读取桌面源文件至psrcData数组
psrcData = malloc(lenFileSize);
if(psrcData==NULL){
printf("malloc psrcData fail; \n");
return false;
}
else
memset(psrcData,0,lenFileSize);
readFileCharDataToBuff(psrcData, &lenSrcData, fpSrc, lenFileSize );
//将字符型数组提取为16进制数组
phexData = malloc(lenSrcData);
if(phexData ==NULL){
printf("malloc phexData fail; \n");
return false;
}
else
memset(phexData,0,lenSrcData);
changeCharToHexData(phexData, &phexDataCnt, psrcData, lenSrcData);
//将16进制数据按协议提取后打印出来
gpsDistanceSimulator(phexData,phexDataCnt);
gpsDistanceJumpPoint(phexData,phexDataCnt);
fclose(fpSrc);
if(psrcData){
free(psrcData);
psrcData = NULL;
}
if(phexData){
free(phexData);
phexData = NULL;
}
return 0;
}5 orgfile.log预处理文本数据
将预处理文本放到桌面，运行前文代码即可进行解析；数据为不知道谁运动的一组实际骑行gps有问题坐标数据；
6 小结
还是要找个旮旯角用来存放不易归类的数据，我看csdn和道客巴巴就不错；
