获取经纬度、转向角和速度，生成gps_road.txt

最新推荐文章于 2022-04-25 11:30:21 发布
最新推荐文章于 2022-04-25 11:30:21 发布
阅读量618 收藏 2
点赞数 1
分类专栏： 自动驾驶 文章标签： c++ c语言
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_48657767/article/details/122454863
版权

1.获取四个关键数据，生成gps_road.txt
功能：需要从星网宇达的GPFPD数据格式中提取经纬度、转向角以及从车辆底盘中提取速度信息（can总线中传输数据）。

#include "generate_route.h"
#include "ntzx_qianxun_inv.h"
#include "ntzx_mc.h"
#include "ntzx_timer_drv.h"
#include "ntzx_file_drv.h"
#include "ntzx_conf_app.h"
#include<string.h>
#include<stdio.h>
#include<pthread.h>
#include<math.h>
#include<unistd.h>
#define ROUTEFILE "gps_load.txt"
static FILE *g_route = NULL;
int main(int argc, char *argv[])
{ 
    int rt;  
    str_inv_to_pl nav_info = {0};
    str_inv_to_pl nav_info_last = {0};
    str_inv_to_pl nav_info_llast = {0};
    struct_mc_sta mc_sta = {0};
    ntzx_systimer route;
    Timer_Set(&route, 100000);

    /* 初始化配置信息 */
    rt = ntzx_conf_app_init();
    if (rt < 0) {
        printf("初始化配置信息failed\n");
        return -1;
    }
    /* 初始化千寻惯导 */
    rt = ntzx_qianxun_init();
    if (rt < 0) {
        printf("qianxun 初始化失败\n");
        printf("%d\n",rt);
        return -1;
    }  
    /* 初始化MC底层 */
    rt = ntzx_mc_usart_init();
    if (rt < 0) {
        printf("MC底层初始话失败");
        return -1;
    }
    /* 初始化文件 */
    rt = ntzx_file_init(ROUTEFILE, &g_route, "w");
    if (g_route == NULL) {
        printf("打开创建文件失败\n");
    }
    pthread_t thread_mc; // 底层数据处理线程
    pthread_t thread_inertial_nv; // 导航（或惯导信息）处理线程
    pthread_create(&thread_inertial_nv, NULL, ntzx_qianxun_inv_main, NULL);
    pthread_create(&thread_mc, NULL, ntzx_mc_main, NULL);
    
    int i = 0;
    /* 采集数据 */
    char tmp[1000000];      //临时保存
    memset(tmp,0,sizeof(tmp));
    char *p = tmp;
    int start_to_break = 0;//当收到静止数据时退出保存
    while (1) {
        if (Timer_GetReached(&route)==0) 
        {
            Timer_Set(&route, 100000);  //采样频率，车速1m/s.
            if (NTZX_QIANXUN_SUCCESS == ntzx_get_inv_to_pl(&nav_info)) 
            {
                i++;
            }else{
                printf("等待千寻正确数据。\n");
                sleep(1);
                continue;
            }
            ntzx_get_mc_state(&mc_sta);//获取车辆底盘状态
            //将路径点信息先保存到数据里面
            sprintf(p + strlen(tmp), "%.8f\n", nav_info.lat);
            sprintf(p + strlen(tmp), "%.8f\n", nav_info.lon);
            sprintf(p + strlen(tmp), "%f\n", nav_info.courseAngle);
            sprintf(p + strlen(tmp), "%d\n", mc_sta.speed_mm / 10);
            //信息点打印查看
            printf("nav_info.Latitude_degree:%.10f\t\n", nav_info.lat);
            printf("nav_info.Longitude_degree:%.10f\t\n", nav_info.lon);
            printf("nav_info.courseAngle:%f\t\n", nav_info.courseAngle);
            printf("mc_sta.speed_cm:%d\t\n", mc_sta.speed_mm / 10);
	    
            //第一种情况：当车子静止时，准备计数并退出。
            if( (fabs(nav_info.lat-nav_info_llast.lat)<=0.0000002)  && (fabs(nav_info.lon-nav_info_llast.lon)<=0.0000002) )
            {   
                start_to_break++;
                printf("开始停止计数：%d\t\n",start_to_break);
                if (start_to_break == 1200){
                printf("确认停止，结束记录路径点！\n");
                break;  
                }         
            }else{
                start_to_break = 0;//清零
            }
            nav_info_llast = nav_info_last;
            nav_info_last=nav_info;//保存当前数据到历史数据           

        //第二种情况：当车子采集满点，退出。
             if (i ==24999)
            {
                printf("已经保存4999组数据，退出！\n");
                break;
            }
         }
    }
    printf("准备写入txt文档\n");
    fprintf(g_route, "%s",  tmp);
    fflush(g_route);
    printf("写入txt文档结束，按Ctrl +C退出！\n");
    pthread_join(thread_inertial_nv, NULL);
    pthread_join(thread_mc, NULL);
}

核心问题（1）：分开粘在一起的字符串，打印出一串完整的GPFPD数据，然后存到数组里进行解析。
在这里插入图片描述
strchr()的作用是返回在recv_buf接收的数据中，首次出现‘ ’ 字 符 的 地 址 。 遍 历 数 组 后 到 下 一 个 ′ ’字符的地址。遍历数组后到下一个' ’字符的地址。遍历数组后到下一个′'字符结束，一个完整的GPFPD字符串就存放在g_qianxun_recv_buf[]中。

核心问题（2）：解析GPFPD字符串

static int ntzx_gpfpd_proc(void)
{
    char *p_GPFPD = NULL;
    char *q_GPFPD = NULL;
    int GPSWeek = 0;
    int GPSTime = 0;
    double Heading = 0;
    double Pitch = 0;
    double Roll = 0;
    double lat = 0;
    double lon = 0;
    double alt = 0;
    double ve =0;
    double vn =0;
    double vu =0;
    double Baseline =0;
    unsigned int NSV1 =0;
    unsigned int NSV2 = 0;
    unsigned int Status = 0;
    unsigned char cs = 0;
    int i = 0, j = 0;
    for(i=0; i<strlen(g_qianxun_recv_data); i++)
    {
        if(g_qianxun_recv_data[i] == QIANXUN_SEPARATOR )
        {
            j++;
        }
    }
    printf("zifuchuanzongchangdu wei:%d\n",j);
    if(j!=15)
    {
        printf("j!= 15 BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB\n");
        return NTZX_QIANXUN_ANALYSIS_ERR;
    }
    p_GPFPD = strchr(g_qianxun_recv_data, '*');
    if(p_GPFPD == NULL)
    {
        return NTZX_QIANXUN_ANALYSIS_ERR;
    }
    p_GPFPD = strchr(g_qianxun_recv_data, QIANXUN_SEPARATOR);
    if(p_GPFPD == NULL)
    {
        return NTZX_QIANXUN_ANALYSIS_ERR;
    }
    /*HUOQU XINGQISHU*/
    p_GPFPD++;
    q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    *q_GPFPD = 0;
    GPSWeek = (int)(atof(p_GPFPD));
 printf("ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc:%d\n",GPSWeek);
    /*HUOQU GELINGNIZHI SHIJIAN*/
    q_GPFPD++;
    p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // if (p_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *p_GPFPD = 0;
    GPSTime = atof(q_GPFPD);
printf("SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS%.3lf\n",GPSTime);
    p_GPFPD++;
    q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // if (q_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *q_GPFPD = 0;
    Heading = atof(p_GPFPD);
printf("DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD:%.3lf\n",Heading);
    q_GPFPD++;
p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // if (q_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *p_GPFPD = 0;
    Pitch = atof(q_GPFPD);
    p_GPFPD++;
    q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // if (q_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *q_GPFPD = 0;
    Roll = atoi(p_GPFPD);
    q_GPFPD++;
    p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // if (p_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *p_GPFPD = 0;
    lat = atof(q_GPFPD);
printf("wwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww:%.8lf\n",lat);
    p_GPFPD++;
    q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // if (q_GPFPD == NULL) {
    //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // }
    *q_GPFPD = 0;
    lon = atoi(p_GPFPD);
printf("jjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjj:%.8lf\n",lon);
    // q_GPFPD++;
    // p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // // if (p_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *p_GPFPD = 0;
    // alt = atof(q_GPFPD);
    // p_GPFPD++;
    // q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // // if (q_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *q_GPFPD = 0;
    // ve = atoi(p_GPFPD);
    // q_GPFPD++;
    // p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // // if (p_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *p_GPFPD = 0;
    // vn = atof(q_GPFPD);
    // p_GPFPD++;
    // q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // // if (q_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *q_GPFPD = 0;
    // vu = atoi(p_GPFPD);
    // q_GPFPD++;
    // p_GPFPD = strchr(q_GPFPD, QIANXUN_SEPARATOR);
    // // if (p_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *p_GPFPD = 0;
    // Baseline = atof(q_GPFPD);
    // p_GPFPD++;
    // q_GPFPD = strchr(p_GPFPD, QIANXUN_SEPARATOR);
    // // if (q_GPFPD == NULL) {
    // //     return NTZX_QIANXUN_ANALYSIS_ERR;
    // // }
    // *q_GPFPD = 0;
    // NSV1 = (unsigned int)atoi(p_GPFPD);
    // q_GPFPD++;
}
范猛男
关注
  • 1
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
用C++去获取地速、经纬度数据
Fred_1986的博客
08-14 1053
1 准备工作 硬件设备：WTGAHRS2模块、、USB-TTL、杜邦线。 WTGAHRS2 2）软件准备 ： VC2010 2 硬件连接 1、接线方式 用USB-TTL和WTGAHRS2模块进行接线然后在吧USB-TTL插到电脑上。 具体接线方式如下： USB-TTL-----WTGAHRS2 VCC----------VCC TX-------------RX RX-------------TX G
获取经纬度
麦克的博客
05-09 1031
LocationUtils.class import android.Manifest; import android.annotation.SuppressLint; import android.content.Context; import android.content.pm.PackageManager; import android.location.Location; import ...
ROS实现串口解析GPS协议，并发布到话题
少年独剑倚清秋的博客
10-14 5363
1.串口配置 ROS串口编程学习笔记：https://blog.csdn.net/u014695839/article/details/81209082
如何通过GPS定位器，通过网页请求获取：经纬度信息和速度，以及中文地址信息
zfgogo的博客
10-02 4013
GPS信息获取，通过http请求
一个经纬度相关计算的C++类
ss19890125的专栏
06-02 1088
一个经纬度相关计算的C++类 写了一个经纬度距离计算的类 　　--------------CJWD.h--------------  　　#ifndef __JWD_AND_HELPER_20051005 　　#define __JWD_AND_HELPER_20051005  　　#include "stdafx.h" 　　#include 　　#include 　　usin
JAVA_study_road.rar_study_road
09-21
JAVA学习历程 给大家一些好的建议 只得参考 少走弯路
data_road.zip
06-28
data_road数据集下载
shiyan_3.rar_codeblock_road sign_路标识别
09-24
codeblock工程文件，车尾路标识别与检测，包含检测的权值
Hua_Rong_Road.rar_华容道
09-24
华容道游戏 用java编程实现的，和平时玩的游戏一摸一样。
gps两点经纬度计算距离和方向角
12-01
通过两点经纬度坐标，得到两点的距离和角度。进行高斯转化。
获取经纬度点的高程值
12-27
输入经纬度值，可以获取该点对应的高程值，代码中增加了redis数据库的使用，用于进度条，增加omp线程优化，输入在json文件里面配置
GPS语句协议（NMEA0183）
03-03
此文档包含基本的GPS语句协议，可用于学习查看。
GPS串口解析
04-23
GPS串口解析。
地图应用之 ：获取当前位置的经纬度
weixin_30823001的博客
11-18 213
一：百度地图 说明： 1. 设置定位的模式是LocationMode.Hight_Accuracy 时,在室内可能无法获取到准确的经纬度，会得到默认的值是4.9E-324 处理办法是将模式改为Battery_Saving,或到室外 2.可以根据当前设备网络连接情况和GPS是否开启来设定定位模式 //获得网络连接情况 ConnectivityManag...
如何给定两个gps坐标 算出航向角_鱼雷是如何瞄准的？
weixin_39571403的博客
11-26 1575
大家似乎对如何瞄准系列非常感兴趣，今天我们就来介绍鱼雷是如何瞄准的。玩过战舰世界的都知道，游戏中只要锁定敌舰，就会出现一个白色的鱼雷辅助瞄准区域，也就是说如果敌舰不进行机动，保持现有航向和航速，瞄准白色区域既能命中，那实战中鱼雷是如何瞄准的呢？注：本文所介绍的鱼雷瞄准方式均为非制导鱼雷。鱼雷和舰炮有很大区别，炮弹是三维空间的运动，而鱼雷仅仅是平面上的运动，所以鱼雷不涉及弹道，不会出现跨射，瞄准相对...
最简单的方式获取当前位置，经纬度等
Evan的博客
08-17 7251
前言 不知道为啥，任性的写个前言，其实也没啥内容。 公司有个项目需求是在设备联网的时候，向服务器端推送当前设备的一些信息，其中包括了当前设备所在的经纬度和详细地点信息，当时也考虑过百度高德地图这些，但是在使用起来，有些地方差强人意，再加上，我们的产品主要是国外用户，所以后面就干脆放弃了，在经过一段时间的查找后，找到了一个网站。 这是一个神奇的网站 当然，人家也有限制，同一个IP地址在一...
计算经纬度旋转角度
RookieLink
12-11 3015
算法 百度地图开发小车行驶方向时，设置icon旋转角度算法 注：得到的角度是以水平向右方向，顺时针方向旋转角度 // 计算经纬度旋转角度 getAngle(lng1, lat1, lng2, lat2) { let dRotateAngle = Math.atan2(Math.abs(lng2 - lng1), Math.abs(lat2 - lat1)); ...
车辆自动采点
weixin_48657767的博客
04-25 319
一、整体思路： 车辆自动踩点需要获取千寻差分数据，差分数据有两种格式：分别是GPGGA和GRHDT，GPGGA内存在车辆经纬度定位信息，GRHDT内存在转向角信息，分别进行关键数据提取，然后将提取的数据存放在创建的gps_road.txt中 1.main.c中 /*int main(int argc,char *argv[])中的argc是输入数组argv[]的数的个数，输入的数都打印在argv[]中*/ int main(int argc, char *argv[]) { p = fop
检查代码错误并修改 def create_rul(self): # 村庄数目输入框 self.num_label = tk.Label(self, text="村庄数目") self.num_label.pack() self.num_entry = tk.Entry(self) self.num_entry.pack() # 可建公路数目输入框 self.edge_label = tk.Label(self, text="可建公路数目") self.edge_label.pack() self.edge_entry = tk.Entry(self) self.edge_entry.pack() # 确认按钮 self.confirm_button = tk.Button(self, text="确认", command=self.create_widgets) self.confirm_button.pack() def create_widgets(self): # 获取村庄数目和可建公路数目 num = int(self.num_entry.get()) edge = int(self.edge_entry.get()) # 村庄名称输入框 self.village_label = tk.Label(self, text="村庄名称") self.village_label.pack() for i in range(num): self.village_entry = tk.Entry(self) self.village_entry.pack() # 村村连同道路信息输入框 self.road_label = tk.Label(self, text="村村连同道路信息‘村,村,路’") self.road_label.pack() for j in range(edge): self.road_entry = tk.Entry(self) self.road_entry.pack() # 提交按钮 self.submit_button = tk.Button(self, text="确认", command=self.submit) self.submit_button.pack()
最新发布
05-31
在 `create_widgets` 函数中，每次循环创建 `self.village_entry` 和 `self.road_entry` 时，应该使用列表将它们存储起来，否则后面的循环会覆盖前面的输入框，导致只有最后一个输入框被保留。同时，在 `submit` 回...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值