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有问题坐标数据；

  一组29分钟多的实际骑行记录的gps坐标资源-CSDN文库

6 小结

  还是要找个旮旯角用来存放不易归类的数据，我看csdn和道客巴巴就不错；